EdgeBotix / Mbed 2 deprecated eBot_Firmware_V1

Initial Commit

Dependencies:   mbed HC05 QEI MODSERIAL SWSPI mbed-rtos

main.cpp@1:1da89c13dfa1, 2014-10-11 (annotated)

Committer:
Throwbot
Date:
Sat Oct 11 03:53:27 2014 +0000
Revision:
1:1da89c13dfa1
Parent:
0:43331220df0d
Child:
2:0f80c8a1c236
Ultrasonic seperate class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Throwbot 1:1da89c13dfa1 1 // FORMAT_CODE_START
Throwbot 0:43331220df0d 2 #include "QEI.h"
Throwbot 0:43331220df0d 3 #include "mbed.h"
Throwbot 0:43331220df0d 4 #include "robot.h"
Throwbot 0:43331220df0d 5 #include "rtos.h"
Throwbot 1:1da89c13dfa1 6 #include "MPU6050.h"
Throwbot 1:1da89c13dfa1 7 #include "I2Cdev.h"
Throwbot 1:1da89c13dfa1 8 #include "tone.h"
Throwbot 1:1da89c13dfa1 9 #include "ultrasonic.h"
Throwbot 0:43331220df0d 10 char c;
Throwbot 1:1da89c13dfa1 11 char buffer[10];
Throwbot 1:1da89c13dfa1 12 int bit_size=20;
Throwbot 1:1da89c13dfa1 13 int thetha1=300;
Throwbot 1:1da89c13dfa1 14 int thetha=0;
Throwbot 1:1da89c13dfa1 15 int stall=0;
Throwbot 1:1da89c13dfa1 16 int bump=1;
Throwbot 1:1da89c13dfa1 17 int UL_rR = 0;
Throwbot 1:1da89c13dfa1 18 int UL_R = 0;
Throwbot 1:1da89c13dfa1 19 int UL_F = 0;
Throwbot 1:1da89c13dfa1 20 int UL_L = 0;
Throwbot 1:1da89c13dfa1 21 int UL_rL = 0;
Throwbot 1:1da89c13dfa1 22 int UL_B = 0;
Throwbot 1:1da89c13dfa1 23 int dx1=100;
Throwbot 1:1da89c13dfa1 24 int dy1=200;
Throwbot 1:1da89c13dfa1 25
Throwbot 1:1da89c13dfa1 26 int linear_velocity_value ;
Throwbot 1:1da89c13dfa1 27 int linear_velocity_direction;
Throwbot 1:1da89c13dfa1 28 int rotational_velocity_value ;
Throwbot 1:1da89c13dfa1 29 int rotational_velocity_direction;
Throwbot 1:1da89c13dfa1 30 int Lspeed=1;
Throwbot 1:1da89c13dfa1 31 int Rspeed=1;
Throwbot 1:1da89c13dfa1 32
Throwbot 1:1da89c13dfa1 33 int main()
Throwbot 1:1da89c13dfa1 34 {
Throwbot 1:1da89c13dfa1 35 initRobot();
Throwbot 1:1da89c13dfa1 36 //motor_control((100),(-100));
Throwbot 1:1da89c13dfa1 37 while(buffer[3] != 'O') {
Throwbot 1:1da89c13dfa1 38 while(buffer[3] != 'E') {
Throwbot 1:1da89c13dfa1 39 if(bt.readable()) {
Throwbot 1:1da89c13dfa1 40 bt.gets(buffer, 5);
Throwbot 1:1da89c13dfa1 41 if(buffer[3] == 'E') {
Throwbot 1:1da89c13dfa1 42 bt.printf(">>1B");
Throwbot 1:1da89c13dfa1 43 } else if(buffer[3] == '?') {
Throwbot 1:1da89c13dfa1 44 bt.printf("K");
Throwbot 1:1da89c13dfa1 45 }
Throwbot 1:1da89c13dfa1 46 }
Throwbot 1:1da89c13dfa1 47 }
Throwbot 1:1da89c13dfa1 48 if(bt.readable()) {
Throwbot 1:1da89c13dfa1 49 bt.gets(buffer, 5);
Throwbot 1:1da89c13dfa1 50 }
Throwbot 1:1da89c13dfa1 51 }
Throwbot 1:1da89c13dfa1 52
Throwbot 1:1da89c13dfa1 53 //imperial_march();
Throwbot 1:1da89c13dfa1 54 Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL);
Throwbot 1:1da89c13dfa1 55 Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL);
Throwbot 1:1da89c13dfa1 56 buzzer=0;
Throwbot 1:1da89c13dfa1 57 //motor_control(lMotor*m_speed,rMotor*m_speed*0.8);
Throwbot 1:1da89c13dfa1 58 //motor_control(lMotor*m_speed,0);
Throwbot 1:1da89c13dfa1 59 while(1) {
Throwbot 1:1da89c13dfa1 60 //bt.lock();
Throwbot 1:1da89c13dfa1 61 //bt.printf("LDR1_%lf;LDR2_%lf;Pulses is: %i Revolutions is: %i Pulses is: %i ultrasonic: , \r\n",ldrread1(),ldrread2(), left.getPulses(), left.getRevolutions(),right.getPulses());//, uL.read()*5*102.54*2.54);
Throwbot 0:43331220df0d 62 //bt.printf("s;LDR1_%lf;LDR2_%lf;Pulses is: %i ; %i:s\n\r",ldrread1(),ldrread2(),wheel.getPulses(), wheel.getRevolutions());
Throwbot 1:1da89c13dfa1 63 // motor_control(lMotor*m_speed,rMotor*m_speed);
Throwbot 1:1da89c13dfa1 64 //bt.printf("a/g:\t %d:\t %d:\t %d:\t %d:\t %d:\t %d:\t\r\n",ax ,ay,az,gx,gy,gz);
Throwbot 1:1da89c13dfa1 65 //bt.unlock();
Throwbot 1:1da89c13dfa1 66 if(bt.readable()) {
Throwbot 1:1da89c13dfa1 67 bt.gets(buffer, 5);
Throwbot 1:1da89c13dfa1 68 if(buffer[3] == 'S') {
Throwbot 1:1da89c13dfa1 69 SRX=1;
Throwbot 1:1da89c13dfa1 70 wait_ms(2);
Throwbot 1:1da89c13dfa1 71 UL_rR=sensorvalue(urR);
Throwbot 1:1da89c13dfa1 72 SRX=0;
Throwbot 1:1da89c13dfa1 73 wait_ms(2);
Throwbot 1:1da89c13dfa1 74
Throwbot 1:1da89c13dfa1 75 SRX=1;
Throwbot 1:1da89c13dfa1 76 wait_ms(2);
Throwbot 1:1da89c13dfa1 77 UL_R=sensorvalue(uR);
Throwbot 1:1da89c13dfa1 78 SRX = 0;
Throwbot 1:1da89c13dfa1 79 wait_ms(2);
Throwbot 1:1da89c13dfa1 80
Throwbot 1:1da89c13dfa1 81 SRX=1;
Throwbot 1:1da89c13dfa1 82 wait_ms(2);
Throwbot 1:1da89c13dfa1 83 UL_F=sensorvalue(uF);
Throwbot 1:1da89c13dfa1 84 SRX=0;
Throwbot 1:1da89c13dfa1 85 wait_ms(2);
Throwbot 1:1da89c13dfa1 86
Throwbot 1:1da89c13dfa1 87 SRX=1;
Throwbot 1:1da89c13dfa1 88 wait_ms(2);
Throwbot 1:1da89c13dfa1 89 UL_L=sensorvalue(uL);
Throwbot 1:1da89c13dfa1 90 SRX=0;
Throwbot 1:1da89c13dfa1 91 wait_ms(2);
Throwbot 1:1da89c13dfa1 92
Throwbot 1:1da89c13dfa1 93 SRX=1;
Throwbot 1:1da89c13dfa1 94 wait_ms(2);
Throwbot 1:1da89c13dfa1 95 UL_rL=sensorvalue(urL);
Throwbot 1:1da89c13dfa1 96 SRX=0;
Throwbot 1:1da89c13dfa1 97 wait_ms(2);
Throwbot 1:1da89c13dfa1 98
Throwbot 1:1da89c13dfa1 99 SRX=1;
Throwbot 1:1da89c13dfa1 100 wait_ms(2);
Throwbot 1:1da89c13dfa1 101 UL_B=sensorvalue(uB);
Throwbot 1:1da89c13dfa1 102 SRX=0;
Throwbot 1:1da89c13dfa1 103
Throwbot 1:1da89c13dfa1 104 bt.lock();
Throwbot 1:1da89c13dfa1 105 stdio_mutex.lock();
Throwbot 1:1da89c13dfa1 106 heading=heading*11.375;
Throwbot 1:1da89c13dfa1 107 bt.printf(">>D%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
Throwbot 1:1da89c13dfa1 108 dx/256,dx%256,dy/256,dy%256,\
Throwbot 1:1da89c13dfa1 109 (heading)/256,(heading)%256,\
Throwbot 1:1da89c13dfa1 110 stall, bump,\
Throwbot 1:1da89c13dfa1 111 UL_rR/256,UL_rR%256,\
Throwbot 1:1da89c13dfa1 112 UL_R/256,UL_R%256, \
Throwbot 1:1da89c13dfa1 113 UL_F/256,UL_F%256,\
Throwbot 1:1da89c13dfa1 114 UL_L/256,UL_L%256,\
Throwbot 1:1da89c13dfa1 115 UL_rL/256,UL_rL%256,\
Throwbot 1:1da89c13dfa1 116 UL_B/256,UL_B%256);
Throwbot 1:1da89c13dfa1 117 dx=0;
Throwbot 1:1da89c13dfa1 118 dy=0;
Throwbot 1:1da89c13dfa1 119 stdio_mutex.unlock();
Throwbot 1:1da89c13dfa1 120 bt.unlock();
Throwbot 1:1da89c13dfa1 121 } else if (buffer[3] == 'R') {
Throwbot 1:1da89c13dfa1 122 Thread::wait(20);
Throwbot 1:1da89c13dfa1 123 if(bt.readable()) {
Throwbot 1:1da89c13dfa1 124 bt.gets(buffer, 10);
Throwbot 1:1da89c13dfa1 125 linear_velocity_value = buffer[4]<<8|buffer[3];
Throwbot 1:1da89c13dfa1 126 linear_velocity_direction= buffer[5];
Throwbot 1:1da89c13dfa1 127 rotational_velocity_value = buffer[7]<<8|buffer[6];
Throwbot 1:1da89c13dfa1 128 rotational_velocity_direction= buffer[8];
Throwbot 1:1da89c13dfa1 129 if( linear_velocity_direction==0x01) {
Throwbot 1:1da89c13dfa1 130 Lspeed=lMotor;
Throwbot 1:1da89c13dfa1 131 Rspeed=rMotor;
Throwbot 1:1da89c13dfa1 132 } else if( linear_velocity_direction==0x10) {
Throwbot 1:1da89c13dfa1 133 Lspeed=-lMotor;
Throwbot 1:1da89c13dfa1 134 Rspeed=-rMotor;
Throwbot 1:1da89c13dfa1 135 }
Throwbot 1:1da89c13dfa1 136 if( rotational_velocity_direction==0x01 && linear_velocity_value !=0) {
Throwbot 1:1da89c13dfa1 137
Throwbot 1:1da89c13dfa1 138
Throwbot 1:1da89c13dfa1 139 motor_control((Lspeed*linear_velocity_value/35)-10,(Rspeed*linear_velocity_value/35) + 10);
Throwbot 1:1da89c13dfa1 140
Throwbot 1:1da89c13dfa1 141 } else if( rotational_velocity_direction==0x10 && linear_velocity_value !=0) {
Throwbot 1:1da89c13dfa1 142 motor_control((Lspeed*linear_velocity_value/35)+10,(Rspeed*linear_velocity_value/30) -10);
Throwbot 1:1da89c13dfa1 143 }
Throwbot 1:1da89c13dfa1 144 else
Throwbot 1:1da89c13dfa1 145 motor_control((Lspeed*linear_velocity_value/30),(Rspeed*linear_velocity_value/30));
Throwbot 1:1da89c13dfa1 146
Throwbot 1:1da89c13dfa1 147 }
Throwbot 1:1da89c13dfa1 148 }
Throwbot 1:1da89c13dfa1 149 //memset(buffer, 0, sizeof buffer);
Throwbot 0:43331220df0d 150 }
Throwbot 1:1da89c13dfa1 151
Throwbot 1:1da89c13dfa1 152 if(ldrread1()>0.6) {
Throwbot 1:1da89c13dfa1 153 }
Throwbot 1:1da89c13dfa1 154 if(ldrread2()>0.6) {
Throwbot 1:1da89c13dfa1 155 Led_on();
Throwbot 1:1da89c13dfa1 156 } else {
Throwbot 1:1da89c13dfa1 157 Led_off();
Throwbot 1:1da89c13dfa1 158 }
Throwbot 1:1da89c13dfa1 159 Thread::wait(20);
Throwbot 0:43331220df0d 160 }
Throwbot 0:43331220df0d 161 }