Maor Tanami / Mbed 2 deprecated SmartRobotCar_copy

Made by Maor and Shai

Dependencies:   DriveCommand HCSR04 Queue mbed-rtos mbed

Fork of Nucleo_UltrasonicHelloWorld by EJ Teb

main.cpp@2:9a4938a0475b, 2016-07-04 (annotated)

Committer:
Maor_T
Date:
Mon Jul 04 15:28:11 2016 +0000
Revision:
2:9a4938a0475b
Parent:
1:4a5586eb1765 
SmartRobotCar;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ejteb 0:1704ea055c4f 1 #include "mbed.h"
Maor_T 2:9a4938a0475b 2 #include "hcsr04.h"
Maor_T 2:9a4938a0475b 3 #include "rtos.h"
Maor_T 2:9a4938a0475b 4 #include <string>
Maor_T 2:9a4938a0475b 5
Maor_T 2:9a4938a0475b 6 #include "DriveCommand.h"
Maor_T 2:9a4938a0475b 7 #include "queue.h"
Maor_T 2:9a4938a0475b 8
Maor_T 2:9a4938a0475b 9 #define ULTRASONIC_TRIG_PIN p8
Maor_T 2:9a4938a0475b 10 #define ULTRASONIC_ECHO_PIN p9
Maor_T 2:9a4938a0475b 11 #define MOTOR_A_PIN1 p23
Maor_T 2:9a4938a0475b 12 #define MOTOR_A_PIN2 p24
Maor_T 2:9a4938a0475b 13 #define MOTOR_B_PIN1 p21
Maor_T 2:9a4938a0475b 14 #define MOTOR_B_PIN2 p22
Maor_T 2:9a4938a0475b 15 #define BLUETOOTH_RX p27
Maor_T 2:9a4938a0475b 16 #define BLUETOOTH_TX p28
Maor_T 2:9a4938a0475b 17 #define SERVO_PWM_PIN p25
Maor_T 2:9a4938a0475b 18
Maor_T 2:9a4938a0475b 19 #define BT_CHECK_DELAY_MS 50
Maor_T 2:9a4938a0475b 20
Maor_T 2:9a4938a0475b 21 #define ULTRASONIC_CHECK_DELAY_MS 50
Maor_T 2:9a4938a0475b 22 #define ULTRASONIC_EMERGENCY_DELAY_MS 20
Maor_T 2:9a4938a0475b 23 #define AUTODRIVE_CHECK_DELAY_MS 100
Maor_T 2:9a4938a0475b 24
Maor_T 2:9a4938a0475b 25
Maor_T 2:9a4938a0475b 26
Maor_T 2:9a4938a0475b 27 //------DEBUG OPTIONS ----------
Maor_T 2:9a4938a0475b 28 //#define DEBUG_BT_COMMANDS
Maor_T 2:9a4938a0475b 29 #define DEBUG_ULTRASONICE
Maor_T 2:9a4938a0475b 30
Maor_T 2:9a4938a0475b 31 //------------------------------
Maor_T 2:9a4938a0475b 32
Maor_T 2:9a4938a0475b 33 PwmOut motorA1(MOTOR_A_PIN2); //left motor
Maor_T 2:9a4938a0475b 34 PwmOut motorA2(MOTOR_A_PIN1); //left motor
Maor_T 2:9a4938a0475b 35 PwmOut motorB1(MOTOR_B_PIN1); //right motor
Maor_T 2:9a4938a0475b 36 PwmOut motorB2(MOTOR_B_PIN2); //right motor
Maor_T 2:9a4938a0475b 37
Maor_T 2:9a4938a0475b 38 PwmOut servo(SERVO_PWM_PIN);
Maor_T 2:9a4938a0475b 39 Serial bt(BLUETOOTH_TX,BLUETOOTH_RX);
Maor_T 2:9a4938a0475b 40 Serial pc(USBTX,USBRX);
Maor_T 2:9a4938a0475b 41
Maor_T 2:9a4938a0475b 42 HCSR04 usensor(p8,p9);
Maor_T 2:9a4938a0475b 43 float lastDistance = 0; // save the last ultrasonic val
Maor_T 2:9a4938a0475b 44 bool moveForward_flag = false; // determined when vehicle moving forward - using to stop and avoid obascales
Maor_T 2:9a4938a0475b 45 bool turnRight_Flag = false;
Maor_T 2:9a4938a0475b 46 bool turnLeft_Flag = false;
Maor_T 2:9a4938a0475b 47 bool obstacleAlert = false;
Maor_T 2:9a4938a0475b 48 float vehicleSpeed = 1;
Maor_T 2:9a4938a0475b 49 int rotateRightMillis = 650;
Maor_T 2:9a4938a0475b 50 int rotateLeftMillis = 650;
Maor_T 2:9a4938a0475b 51 float turnSpeed = 0.3;
Maor_T 2:9a4938a0475b 52 int obstacleDistance = 30;
Maor_T 2:9a4938a0475b 53
Maor_T 2:9a4938a0475b 54 Timeout stopper;
Maor_T 2:9a4938a0475b 55 Queue autoDriverQueue(sizeof(DriveCommand),10);
Maor_T 2:9a4938a0475b 56
Maor_T 2:9a4938a0475b 57
Maor_T 2:9a4938a0475b 58 //string globalString =" ";
Maor_T 2:9a4938a0475b 59 char lastCommandChar ='x';
Maor_T 2:9a4938a0475b 60 char incomingMsg[1024];
Maor_T 2:9a4938a0475b 61 int incomingMsg_iter = 0;
Maor_T 2:9a4938a0475b 62 bool succesReadFlag = false;
Maor_T 2:9a4938a0475b 63 bool autoDriveFlag = false;
Maor_T 2:9a4938a0475b 64 bool autoDriveRun = false;
Maor_T 2:9a4938a0475b 65 int autoDriveCounter = 0;
Maor_T 2:9a4938a0475b 66 bool autoDrivePartOk = true; // check if every part of auto drive is whitout obstacles
Maor_T 2:9a4938a0475b 67
Maor_T 2:9a4938a0475b 68 //Prototype
Maor_T 2:9a4938a0475b 69 void Drive_Stop();
Maor_T 2:9a4938a0475b 70 void AutoDrive_Stop();
Maor_T 2:9a4938a0475b 71 void ExecuteNextCommand();
Maor_T 2:9a4938a0475b 72 void Rotate_Left();
Maor_T 2:9a4938a0475b 73 void MotorRight_Run(float speedPercent,bool forwardDir);
Maor_T 2:9a4938a0475b 74 void MotorLeft_Run(float speedPercent,bool forwardDir);
Maor_T 2:9a4938a0475b 75 void Drive_Force_Stop();
Maor_T 2:9a4938a0475b 76 int Get_Min_Distance();
Maor_T 2:9a4938a0475b 77 void Servo_Look_Left();
Maor_T 2:9a4938a0475b 78 void Servo_Look_Right();
Maor_T 2:9a4938a0475b 79 void Servo_Look_Center();
Maor_T 2:9a4938a0475b 80
Maor_T 2:9a4938a0475b 81
Maor_T 2:9a4938a0475b 82 void FlagController(char dir)
Maor_T 2:9a4938a0475b 83 {
Maor_T 2:9a4938a0475b 84 switch(dir)
Maor_T 2:9a4938a0475b 85 {
Maor_T 2:9a4938a0475b 86 case 'f':
Maor_T 2:9a4938a0475b 87 moveForward_flag = true;
Maor_T 2:9a4938a0475b 88 turnRight_Flag = false;
Maor_T 2:9a4938a0475b 89 turnLeft_Flag = false;
Maor_T 2:9a4938a0475b 90 break;
Maor_T 2:9a4938a0475b 91 case 'l':
Maor_T 2:9a4938a0475b 92 moveForward_flag = false;
Maor_T 2:9a4938a0475b 93 turnRight_Flag = false;
Maor_T 2:9a4938a0475b 94 turnLeft_Flag = true;
Maor_T 2:9a4938a0475b 95 break;
Maor_T 2:9a4938a0475b 96 case 'r':
Maor_T 2:9a4938a0475b 97 moveForward_flag = false;
Maor_T 2:9a4938a0475b 98 turnRight_Flag = true;
Maor_T 2:9a4938a0475b 99 turnLeft_Flag = false;
Maor_T 2:9a4938a0475b 100 break;
Maor_T 2:9a4938a0475b 101 case 'b':
Maor_T 2:9a4938a0475b 102 moveForward_flag = false;
Maor_T 2:9a4938a0475b 103 turnRight_Flag = false;
Maor_T 2:9a4938a0475b 104 turnLeft_Flag = false;
Maor_T 2:9a4938a0475b 105 break;
Maor_T 2:9a4938a0475b 106 }
Maor_T 2:9a4938a0475b 107 }
Maor_T 2:9a4938a0475b 108
Maor_T 2:9a4938a0475b 109 void StopAfter(float sec)
Maor_T 2:9a4938a0475b 110 {
Maor_T 2:9a4938a0475b 111 // printf("StopAfter Got %f\n",sec);
Maor_T 2:9a4938a0475b 112 stopper.attach(&AutoDrive_Stop,sec);
Maor_T 2:9a4938a0475b 113 }
Maor_T 2:9a4938a0475b 114
Maor_T 2:9a4938a0475b 115 float GetDistance()
Maor_T 2:9a4938a0475b 116 {
Maor_T 2:9a4938a0475b 117 float sum = 0;
Maor_T 2:9a4938a0475b 118
Maor_T 2:9a4938a0475b 119 for(int i=i ; i< 7 ; i++)
Maor_T 2:9a4938a0475b 120 {
Maor_T 2:9a4938a0475b 121 sum += usensor.get_dist_cm();
Maor_T 2:9a4938a0475b 122 }
Maor_T 2:9a4938a0475b 123
Maor_T 2:9a4938a0475b 124 sum /= 7.0;
Maor_T 2:9a4938a0475b 125 return sum;
Maor_T 2:9a4938a0475b 126 }
Maor_T 2:9a4938a0475b 127
Maor_T 2:9a4938a0475b 128
Maor_T 2:9a4938a0475b 129
Maor_T 2:9a4938a0475b 130 void Reset_IncomingMsg()
Maor_T 2:9a4938a0475b 131 {
Maor_T 2:9a4938a0475b 132 for(int i=0 ; i<1024 ; i++)
Maor_T 2:9a4938a0475b 133 incomingMsg[i] = '*';
Maor_T 2:9a4938a0475b 134 incomingMsg_iter =0;
Maor_T 2:9a4938a0475b 135 }
Maor_T 2:9a4938a0475b 136
Maor_T 2:9a4938a0475b 137 string Get_last_Msg()
Maor_T 2:9a4938a0475b 138 {
Maor_T 2:9a4938a0475b 139 int i=0;
Maor_T 2:9a4938a0475b 140 string temp = "";
Maor_T 2:9a4938a0475b 141 while(incomingMsg[i] != '~' && incomingMsg[i] != '*')
Maor_T 2:9a4938a0475b 142 {
Maor_T 2:9a4938a0475b 143 temp+=incomingMsg[i++];
Maor_T 2:9a4938a0475b 144 }
Maor_T 2:9a4938a0475b 145
Maor_T 2:9a4938a0475b 146 if(incomingMsg[i] == '~')
Maor_T 2:9a4938a0475b 147 {
Maor_T 2:9a4938a0475b 148 succesReadFlag = true;
Maor_T 2:9a4938a0475b 149 Reset_IncomingMsg();
Maor_T 2:9a4938a0475b 150 }
Maor_T 2:9a4938a0475b 151
Maor_T 2:9a4938a0475b 152 else
Maor_T 2:9a4938a0475b 153 {
Maor_T 2:9a4938a0475b 154 succesReadFlag = false;
Maor_T 2:9a4938a0475b 155 }
Maor_T 2:9a4938a0475b 156 return temp;
Maor_T 2:9a4938a0475b 157 }
Maor_T 2:9a4938a0475b 158
Maor_T 2:9a4938a0475b 159 float map(float x, float in_min, float in_max, float out_min, float out_max)
Maor_T 2:9a4938a0475b 160 {
Maor_T 2:9a4938a0475b 161 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
Maor_T 2:9a4938a0475b 162 }
Maor_T 2:9a4938a0475b 163
ejteb 0:1704ea055c4f 164
ejteb 0:1704ea055c4f 165 void dist(int distance)
ejteb 0:1704ea055c4f 166 {
ejteb 1:4a5586eb1765 167 //put code here to happen when the distance is changed
ejteb 1:4a5586eb1765 168 printf("Distance changed to %dmm\r\n", distance);
ejteb 0:1704ea055c4f 169 }
ejteb 0:1704ea055c4f 170
Maor_T 2:9a4938a0475b 171
Maor_T 2:9a4938a0475b 172 void MotorLeft_Run(float speedPercent,bool forwardDir)
Maor_T 2:9a4938a0475b 173 {
Maor_T 2:9a4938a0475b 174 if(forwardDir)
Maor_T 2:9a4938a0475b 175 {
Maor_T 2:9a4938a0475b 176 motorA1.write(0);
Maor_T 2:9a4938a0475b 177 motorA2.write(speedPercent);
Maor_T 2:9a4938a0475b 178 }
Maor_T 2:9a4938a0475b 179 else
Maor_T 2:9a4938a0475b 180 {
Maor_T 2:9a4938a0475b 181 motorA1.write(speedPercent);
Maor_T 2:9a4938a0475b 182 motorA2.write(0);
Maor_T 2:9a4938a0475b 183 }
Maor_T 2:9a4938a0475b 184 }
Maor_T 2:9a4938a0475b 185
Maor_T 2:9a4938a0475b 186 void MotorLeft_Stop()
Maor_T 2:9a4938a0475b 187 {
Maor_T 2:9a4938a0475b 188 motorA1.write(0);
Maor_T 2:9a4938a0475b 189 motorA2.write(0);
Maor_T 2:9a4938a0475b 190 }
Maor_T 2:9a4938a0475b 191
Maor_T 2:9a4938a0475b 192 void MotorRight_Run(float speedPercent,bool forwardDir)
Maor_T 2:9a4938a0475b 193 {
Maor_T 2:9a4938a0475b 194 if(forwardDir)
Maor_T 2:9a4938a0475b 195 {
Maor_T 2:9a4938a0475b 196 motorB1.write(0);
Maor_T 2:9a4938a0475b 197 motorB2.write(speedPercent);
Maor_T 2:9a4938a0475b 198 }
Maor_T 2:9a4938a0475b 199 else
Maor_T 2:9a4938a0475b 200 {
Maor_T 2:9a4938a0475b 201 motorB1.write(speedPercent);
Maor_T 2:9a4938a0475b 202 motorB2.write(0);
Maor_T 2:9a4938a0475b 203 }
Maor_T 2:9a4938a0475b 204 }
Maor_T 2:9a4938a0475b 205
Maor_T 2:9a4938a0475b 206 void MotorRight_Stop()
Maor_T 2:9a4938a0475b 207 {
Maor_T 2:9a4938a0475b 208 motorB1.write(0);
Maor_T 2:9a4938a0475b 209 motorB2.write(0);
Maor_T 2:9a4938a0475b 210 }
Maor_T 2:9a4938a0475b 211
Maor_T 2:9a4938a0475b 212 void Drive_Forward(float speedPercent)
Maor_T 2:9a4938a0475b 213 {
Maor_T 2:9a4938a0475b 214 FlagController('f');
Maor_T 2:9a4938a0475b 215 MotorLeft_Run(speedPercent,true);
Maor_T 2:9a4938a0475b 216 MotorRight_Run(speedPercent,true) ;
Maor_T 2:9a4938a0475b 217 }
Maor_T 2:9a4938a0475b 218
Maor_T 2:9a4938a0475b 219 void Drive_Left(float leftPercent)
Maor_T 2:9a4938a0475b 220 {
Maor_T 2:9a4938a0475b 221 FlagController('f');
Maor_T 2:9a4938a0475b 222 MotorRight_Run(leftPercent,true);
Maor_T 2:9a4938a0475b 223 MotorLeft_Run(1-leftPercent,true) ; // left motor reversed
Maor_T 2:9a4938a0475b 224 }
Maor_T 2:9a4938a0475b 225
Maor_T 2:9a4938a0475b 226 void Reverse_Left(float leftPercent)
Maor_T 2:9a4938a0475b 227 {
Maor_T 2:9a4938a0475b 228 MotorRight_Run(leftPercent,false);
Maor_T 2:9a4938a0475b 229 MotorLeft_Run(1-leftPercent,false) ; // left motor reversed
Maor_T 2:9a4938a0475b 230 }
Maor_T 2:9a4938a0475b 231
Maor_T 2:9a4938a0475b 232 void Turn_Left(float leftPercent)
Maor_T 2:9a4938a0475b 233 {
Maor_T 2:9a4938a0475b 234 FlagController('l');
Maor_T 2:9a4938a0475b 235 Servo_Look_Left();
Maor_T 2:9a4938a0475b 236 wait_ms(100);
Maor_T 2:9a4938a0475b 237 MotorRight_Run(leftPercent,true);
Maor_T 2:9a4938a0475b 238 MotorLeft_Run(leftPercent,false) ; // left motor reversed
Maor_T 2:9a4938a0475b 239 }
Maor_T 2:9a4938a0475b 240
Maor_T 2:9a4938a0475b 241 void Drive_Right(float rightPercent)
Maor_T 2:9a4938a0475b 242 {
Maor_T 2:9a4938a0475b 243 MotorLeft_Run(rightPercent,true) ;
Maor_T 2:9a4938a0475b 244 MotorRight_Run(1-rightPercent,true); // right motor reversed
Maor_T 2:9a4938a0475b 245 }
Maor_T 2:9a4938a0475b 246
Maor_T 2:9a4938a0475b 247 void Reverse_Right(float rightPercent)
Maor_T 2:9a4938a0475b 248 {
Maor_T 2:9a4938a0475b 249 MotorLeft_Run(rightPercent,false) ;
Maor_T 2:9a4938a0475b 250 MotorRight_Run(1-rightPercent,false); // right motor reversed
Maor_T 2:9a4938a0475b 251 }
Maor_T 2:9a4938a0475b 252
Maor_T 2:9a4938a0475b 253 void Turn_Right(float rightPercent)
Maor_T 2:9a4938a0475b 254 {
Maor_T 2:9a4938a0475b 255 FlagController('r');
Maor_T 2:9a4938a0475b 256 Servo_Look_Right();
Maor_T 2:9a4938a0475b 257 wait_ms(200);
Maor_T 2:9a4938a0475b 258 MotorLeft_Run(rightPercent,true) ;
Maor_T 2:9a4938a0475b 259 MotorRight_Run(rightPercent,false); // right motor reversed
Maor_T 2:9a4938a0475b 260 }
Maor_T 2:9a4938a0475b 261
Maor_T 2:9a4938a0475b 262 void Drive_Backwards(float speedPercent)
Maor_T 2:9a4938a0475b 263 {
Maor_T 2:9a4938a0475b 264 FlagController('b');
Maor_T 2:9a4938a0475b 265 MotorLeft_Run(speedPercent,false);
Maor_T 2:9a4938a0475b 266 MotorRight_Run(speedPercent,false) ;
Maor_T 2:9a4938a0475b 267 moveForward_flag = false;
Maor_T 2:9a4938a0475b 268 }
Maor_T 2:9a4938a0475b 269
Maor_T 2:9a4938a0475b 270
Maor_T 2:9a4938a0475b 271 void Drive_Stop()
Maor_T 2:9a4938a0475b 272 {
Maor_T 2:9a4938a0475b 273 FlagController('s');
Maor_T 2:9a4938a0475b 274 MotorLeft_Stop();
Maor_T 2:9a4938a0475b 275 MotorRight_Stop();
Maor_T 2:9a4938a0475b 276 moveForward_flag = false;
Maor_T 2:9a4938a0475b 277 autoDriveRun = false;
Maor_T 2:9a4938a0475b 278 }
Maor_T 2:9a4938a0475b 279 void Drive_Force_Stop()
Maor_T 2:9a4938a0475b 280 {
Maor_T 2:9a4938a0475b 281 MotorRight_Run(turnSpeed,false);
Maor_T 2:9a4938a0475b 282 MotorLeft_Run(turnSpeed,false) ;
Maor_T 2:9a4938a0475b 283 wait_ms(500);
Maor_T 2:9a4938a0475b 284 MotorLeft_Stop();
Maor_T 2:9a4938a0475b 285 MotorRight_Stop();
Maor_T 2:9a4938a0475b 286 moveForward_flag = false;
Maor_T 2:9a4938a0475b 287 autoDriveRun = false;
Maor_T 2:9a4938a0475b 288
Maor_T 2:9a4938a0475b 289 }
Maor_T 2:9a4938a0475b 290
Maor_T 2:9a4938a0475b 291
Maor_T 2:9a4938a0475b 292 void AutoDrive_Stop()
Maor_T 2:9a4938a0475b 293 {
Maor_T 2:9a4938a0475b 294 // if(autoDriveFlag) // stop only if autoDrive mode is running
Maor_T 2:9a4938a0475b 295 // {
Maor_T 2:9a4938a0475b 296 MotorLeft_Stop();
Maor_T 2:9a4938a0475b 297 MotorRight_Stop();
Maor_T 2:9a4938a0475b 298 moveForward_flag = false;
Maor_T 2:9a4938a0475b 299 // }
Maor_T 2:9a4938a0475b 300 autoDriveRun = false;
Maor_T 2:9a4938a0475b 301 }
Maor_T 2:9a4938a0475b 302
Maor_T 2:9a4938a0475b 303
Maor_T 2:9a4938a0475b 304 void Rotate_Left()
Maor_T 2:9a4938a0475b 305 {
Maor_T 2:9a4938a0475b 306 MotorRight_Run(turnSpeed,true);
Maor_T 2:9a4938a0475b 307 MotorLeft_Run(turnSpeed,false) ; // left motor reversed
Maor_T 2:9a4938a0475b 308 wait_ms(rotateLeftMillis);
Maor_T 2:9a4938a0475b 309 Drive_Stop();
Maor_T 2:9a4938a0475b 310 }
Maor_T 2:9a4938a0475b 311
Maor_T 2:9a4938a0475b 312 void Rotate_Right()
Maor_T 2:9a4938a0475b 313 {
Maor_T 2:9a4938a0475b 314 MotorRight_Run(turnSpeed,false);
Maor_T 2:9a4938a0475b 315 MotorLeft_Run(turnSpeed,true) ; // left motor reversed
Maor_T 2:9a4938a0475b 316 wait_ms(rotateRightMillis);
Maor_T 2:9a4938a0475b 317 Drive_Stop();
Maor_T 2:9a4938a0475b 318 }
Maor_T 2:9a4938a0475b 319
Maor_T 2:9a4938a0475b 320
Maor_T 2:9a4938a0475b 321 void ServoMove(int pos)
Maor_T 2:9a4938a0475b 322 {
Maor_T 2:9a4938a0475b 323 if(pos < 0 || pos >180)
Maor_T 2:9a4938a0475b 324 return;
Maor_T 2:9a4938a0475b 325 float outVal = map(pos,0,180,1000,2000);
Maor_T 2:9a4938a0475b 326 servo.pulsewidth_us(outVal);
Maor_T 2:9a4938a0475b 327 }
Maor_T 2:9a4938a0475b 328
Maor_T 2:9a4938a0475b 329
Maor_T 2:9a4938a0475b 330 int Get_Min_Distance()
Maor_T 2:9a4938a0475b 331 {
Maor_T 2:9a4938a0475b 332 int min;
Maor_T 2:9a4938a0475b 333
Maor_T 2:9a4938a0475b 334 Servo_Look_Left();
Maor_T 2:9a4938a0475b 335 wait_ms(250);
Maor_T 2:9a4938a0475b 336 usensor.start();
Maor_T 2:9a4938a0475b 337 int left = usensor.get_dist_cm();
Maor_T 2:9a4938a0475b 338 min=left;
Maor_T 2:9a4938a0475b 339
Maor_T 2:9a4938a0475b 340 if(min < obstacleDistance )
Maor_T 2:9a4938a0475b 341 {
Maor_T 2:9a4938a0475b 342 autoDrivePartOk = false;
Maor_T 2:9a4938a0475b 343 return min;
Maor_T 2:9a4938a0475b 344 }
Maor_T 2:9a4938a0475b 345
Maor_T 2:9a4938a0475b 346 Servo_Look_Right();
Maor_T 2:9a4938a0475b 347 wait_ms(250);
Maor_T 2:9a4938a0475b 348 usensor.start();
Maor_T 2:9a4938a0475b 349 int right = usensor.get_dist_cm();
Maor_T 2:9a4938a0475b 350 if(right<min)
Maor_T 2:9a4938a0475b 351 min=right;
Maor_T 2:9a4938a0475b 352
Maor_T 2:9a4938a0475b 353 if(min < obstacleDistance )
Maor_T 2:9a4938a0475b 354 {
Maor_T 2:9a4938a0475b 355 autoDrivePartOk = false;
Maor_T 2:9a4938a0475b 356 return min;
Maor_T 2:9a4938a0475b 357
Maor_T 2:9a4938a0475b 358 }
Maor_T 2:9a4938a0475b 359
Maor_T 2:9a4938a0475b 360 Servo_Look_Center();
Maor_T 2:9a4938a0475b 361 wait_ms(250);
Maor_T 2:9a4938a0475b 362 usensor.start();
Maor_T 2:9a4938a0475b 363 int center = usensor.get_dist_cm();
Maor_T 2:9a4938a0475b 364 if(center<min)
Maor_T 2:9a4938a0475b 365 min=center;
Maor_T 2:9a4938a0475b 366
Maor_T 2:9a4938a0475b 367 if(min < obstacleDistance )
Maor_T 2:9a4938a0475b 368 {
Maor_T 2:9a4938a0475b 369 autoDrivePartOk = false;
Maor_T 2:9a4938a0475b 370 return min;
Maor_T 2:9a4938a0475b 371 }
Maor_T 2:9a4938a0475b 372 // printf("Min Distance %d\n",min);
Maor_T 2:9a4938a0475b 373
Maor_T 2:9a4938a0475b 374 return min;
Maor_T 2:9a4938a0475b 375 }
Maor_T 2:9a4938a0475b 376
Maor_T 2:9a4938a0475b 377
Maor_T 2:9a4938a0475b 378
Maor_T 2:9a4938a0475b 379 void Servo_Look_Left()
Maor_T 2:9a4938a0475b 380 {
Maor_T 2:9a4938a0475b 381 servo.pulsewidth_us(2000);
Maor_T 2:9a4938a0475b 382 }
Maor_T 2:9a4938a0475b 383
Maor_T 2:9a4938a0475b 384 void Servo_Look_Right()
Maor_T 2:9a4938a0475b 385 {
Maor_T 2:9a4938a0475b 386 servo.pulsewidth_us(1000);
Maor_T 2:9a4938a0475b 387 }
Maor_T 2:9a4938a0475b 388
Maor_T 2:9a4938a0475b 389 void Servo_Look_Center()
Maor_T 2:9a4938a0475b 390 {
Maor_T 2:9a4938a0475b 391 servo.pulsewidth_us(1500);
Maor_T 2:9a4938a0475b 392 }
Maor_T 2:9a4938a0475b 393
Maor_T 2:9a4938a0475b 394 void DataReceived_ISR(){
Maor_T 2:9a4938a0475b 395 // char globalChar = LPC_UART2->RBR;
Maor_T 2:9a4938a0475b 396 // lastCommandChar = LPC_UART2->RBR;
Maor_T 2:9a4938a0475b 397 incomingMsg[incomingMsg_iter++] = LPC_UART2->RBR ;
Maor_T 2:9a4938a0475b 398
Maor_T 2:9a4938a0475b 399 // globalString += globalChar;
Maor_T 2:9a4938a0475b 400 }
Maor_T 2:9a4938a0475b 401
Maor_T 2:9a4938a0475b 402 void DataReceived_PCISR(){
Maor_T 2:9a4938a0475b 403 // char globalChar = LPC_UART2->RBR;
Maor_T 2:9a4938a0475b 404 // lastCommandChar = LPC_UART2->RBR;
Maor_T 2:9a4938a0475b 405 incomingMsg[incomingMsg_iter++] = LPC_UART0->RBR ;
Maor_T 2:9a4938a0475b 406
Maor_T 2:9a4938a0475b 407 // globalString += globalChar;
Maor_T 2:9a4938a0475b 408 }
Maor_T 2:9a4938a0475b 409
Maor_T 2:9a4938a0475b 410
Maor_T 2:9a4938a0475b 411 void Servo_NoNo(int count)
Maor_T 2:9a4938a0475b 412 {
Maor_T 2:9a4938a0475b 413 for(int i=0 ; i< count ; i++) {
Maor_T 2:9a4938a0475b 414 Servo_Look_Left();
Maor_T 2:9a4938a0475b 415 wait(0.2);
Maor_T 2:9a4938a0475b 416 Servo_Look_Right();
Maor_T 2:9a4938a0475b 417 wait(0.2);
Maor_T 2:9a4938a0475b 418 }
Maor_T 2:9a4938a0475b 419 Servo_Look_Center();
Maor_T 2:9a4938a0475b 420 }
Maor_T 2:9a4938a0475b 421
Maor_T 2:9a4938a0475b 422 float getCommandValue(string cmd){
Maor_T 2:9a4938a0475b 423 string cmdValue;
Maor_T 2:9a4938a0475b 424 std::size_t sharpPosition = cmd.find("#");
Maor_T 2:9a4938a0475b 425 cmdValue = cmd.substr(sharpPosition+1);
Maor_T 2:9a4938a0475b 426 // printf("cmdvalue %s\n",cmdValue); // - Print the incoming command
Maor_T 2:9a4938a0475b 427 return atof(cmdValue.c_str());
Maor_T 2:9a4938a0475b 428 }
Maor_T 2:9a4938a0475b 429
Maor_T 2:9a4938a0475b 430
Maor_T 2:9a4938a0475b 431
Maor_T 2:9a4938a0475b 432
Maor_T 2:9a4938a0475b 433 void BTCommands(string cmd)
Maor_T 2:9a4938a0475b 434 {
Maor_T 2:9a4938a0475b 435 #ifdef DEBUG_BT_COMMANDS
Maor_T 2:9a4938a0475b 436 printf("Incoming BT command : %s\n",cmd); // - Print the incoming command
Maor_T 2:9a4938a0475b 437 #endif
Maor_T 2:9a4938a0475b 438 if (cmd.find("forward#")!= string::npos)
Maor_T 2:9a4938a0475b 439 {
Maor_T 2:9a4938a0475b 440 float time = getCommandValue(cmd);
Maor_T 2:9a4938a0475b 441 DriveCommand* dc;
Maor_T 2:9a4938a0475b 442 dc = (DriveCommand*)malloc(sizeof(DriveCommand));
Maor_T 2:9a4938a0475b 443 dc->SetTime(time);
Maor_T 2:9a4938a0475b 444 dc->SetDir('f');
Maor_T 2:9a4938a0475b 445 autoDriverQueue.Put(dc);
Maor_T 2:9a4938a0475b 446 }
Maor_T 2:9a4938a0475b 447 else if(cmd.find("backwards#")!= string::npos)
Maor_T 2:9a4938a0475b 448 {
Maor_T 2:9a4938a0475b 449 float time = getCommandValue(cmd);
Maor_T 2:9a4938a0475b 450 DriveCommand* dc;
Maor_T 2:9a4938a0475b 451 dc = (DriveCommand*)malloc(sizeof(DriveCommand));
Maor_T 2:9a4938a0475b 452 dc->SetTime(time);
Maor_T 2:9a4938a0475b 453 dc->SetDir('b');
Maor_T 2:9a4938a0475b 454 autoDriverQueue.Put(dc);
Maor_T 2:9a4938a0475b 455 }
Maor_T 2:9a4938a0475b 456 else if(cmd.find("obstacle#")!= string::npos)
Maor_T 2:9a4938a0475b 457 {
Maor_T 2:9a4938a0475b 458 obstacleDistance = getCommandValue(cmd);
Maor_T 2:9a4938a0475b 459
Maor_T 2:9a4938a0475b 460 }
Maor_T 2:9a4938a0475b 461
Maor_T 2:9a4938a0475b 462 else if (cmd.find("rotleftms#")!= string::npos)
Maor_T 2:9a4938a0475b 463 {
Maor_T 2:9a4938a0475b 464 rotateLeftMillis = getCommandValue(cmd);
Maor_T 2:9a4938a0475b 465 Rotate_Left(); // todo remove after testing
Maor_T 2:9a4938a0475b 466
Maor_T 2:9a4938a0475b 467 }
Maor_T 2:9a4938a0475b 468 else if (cmd.find("rotrightms#")!= string::npos)
Maor_T 2:9a4938a0475b 469 {
Maor_T 2:9a4938a0475b 470 rotateRightMillis = getCommandValue(cmd);
Maor_T 2:9a4938a0475b 471 Rotate_Right(); // todo remove after testing
Maor_T 2:9a4938a0475b 472 }
Maor_T 2:9a4938a0475b 473 else if (cmd.find("turnspeed#")!= string::npos)
Maor_T 2:9a4938a0475b 474 {
Maor_T 2:9a4938a0475b 475 turnSpeed = getCommandValue(cmd);
Maor_T 2:9a4938a0475b 476
Maor_T 2:9a4938a0475b 477 }
Maor_T 2:9a4938a0475b 478 else if(cmd.find("rotleft")!= string::npos)
Maor_T 2:9a4938a0475b 479 {
Maor_T 2:9a4938a0475b 480 Rotate_Left();
Maor_T 2:9a4938a0475b 481 }
Maor_T 2:9a4938a0475b 482 else if(cmd.find("rotright")!= string::npos)
Maor_T 2:9a4938a0475b 483 {
Maor_T 2:9a4938a0475b 484 Rotate_Right();
Maor_T 2:9a4938a0475b 485 }
Maor_T 2:9a4938a0475b 486 else if(cmd.find("left#")!= string::npos)
Maor_T 2:9a4938a0475b 487 {
Maor_T 2:9a4938a0475b 488 float time = getCommandValue(cmd);
Maor_T 2:9a4938a0475b 489 DriveCommand* dc;
Maor_T 2:9a4938a0475b 490 dc = (DriveCommand*)malloc(sizeof(DriveCommand));
Maor_T 2:9a4938a0475b 491 dc->SetTime(time);
Maor_T 2:9a4938a0475b 492 dc->SetDir('l');
Maor_T 2:9a4938a0475b 493 autoDriverQueue.Put(dc);
Maor_T 2:9a4938a0475b 494 }
Maor_T 2:9a4938a0475b 495 else if(cmd.find("right#")!= string::npos)
Maor_T 2:9a4938a0475b 496 {
Maor_T 2:9a4938a0475b 497 float time = getCommandValue(cmd);
Maor_T 2:9a4938a0475b 498 DriveCommand* dc;
Maor_T 2:9a4938a0475b 499 dc = (DriveCommand*)malloc(sizeof(DriveCommand));
Maor_T 2:9a4938a0475b 500 dc->SetTime(time);
Maor_T 2:9a4938a0475b 501 dc->SetDir('r');
Maor_T 2:9a4938a0475b 502 autoDriverQueue.Put(dc);
Maor_T 2:9a4938a0475b 503 }
Maor_T 2:9a4938a0475b 504 else if (cmd.find("autodrive")!= string::npos)
Maor_T 2:9a4938a0475b 505 {
Maor_T 2:9a4938a0475b 506 autoDriveFlag = true;
Maor_T 2:9a4938a0475b 507
Maor_T 2:9a4938a0475b 508 }
Maor_T 2:9a4938a0475b 509 else if (cmd.find("forward")!= string::npos)
Maor_T 2:9a4938a0475b 510 {
Maor_T 2:9a4938a0475b 511 if(lastDistance < obstacleDistance)
Maor_T 2:9a4938a0475b 512 {
Maor_T 2:9a4938a0475b 513 Servo_NoNo(2);
Maor_T 2:9a4938a0475b 514 return;
Maor_T 2:9a4938a0475b 515 }
Maor_T 2:9a4938a0475b 516 Drive_Forward(vehicleSpeed);
Maor_T 2:9a4938a0475b 517 }
Maor_T 2:9a4938a0475b 518 else if (cmd.find("stop")!= string::npos)
Maor_T 2:9a4938a0475b 519 {
Maor_T 2:9a4938a0475b 520 Drive_Stop();
Maor_T 2:9a4938a0475b 521 }
Maor_T 2:9a4938a0475b 522 else if (cmd.find("backwards")!= string::npos)
Maor_T 2:9a4938a0475b 523 {
Maor_T 2:9a4938a0475b 524 Drive_Backwards(vehicleSpeed);
Maor_T 2:9a4938a0475b 525 }
Maor_T 2:9a4938a0475b 526 else if (cmd.find("tleft")!= string::npos)
Maor_T 2:9a4938a0475b 527 {
Maor_T 2:9a4938a0475b 528 Turn_Left(vehicleSpeed);
Maor_T 2:9a4938a0475b 529 }
Maor_T 2:9a4938a0475b 530 else if (cmd.find("tright")!= string::npos)
Maor_T 2:9a4938a0475b 531 {
Maor_T 2:9a4938a0475b 532 Turn_Right(vehicleSpeed);
Maor_T 2:9a4938a0475b 533 }
Maor_T 2:9a4938a0475b 534 else if (cmd.find("dright")!= string::npos)
Maor_T 2:9a4938a0475b 535 {
Maor_T 2:9a4938a0475b 536 Drive_Right(vehicleSpeed);
Maor_T 2:9a4938a0475b 537 }
Maor_T 2:9a4938a0475b 538 else if (cmd.find("dleft")!= string::npos)
Maor_T 2:9a4938a0475b 539 {
Maor_T 2:9a4938a0475b 540 Drive_Left(vehicleSpeed);
Maor_T 2:9a4938a0475b 541 }
Maor_T 2:9a4938a0475b 542 else if (cmd.find("rleft")!= string::npos)
Maor_T 2:9a4938a0475b 543 {
Maor_T 2:9a4938a0475b 544 Reverse_Left(vehicleSpeed);
Maor_T 2:9a4938a0475b 545 }
Maor_T 2:9a4938a0475b 546 else if (cmd.find("rright")!= string::npos)
Maor_T 2:9a4938a0475b 547 {
Maor_T 2:9a4938a0475b 548 Reverse_Right(vehicleSpeed);
Maor_T 2:9a4938a0475b 549 }
Maor_T 2:9a4938a0475b 550 else if (cmd.find("speed1")!= string::npos)
Maor_T 2:9a4938a0475b 551 {
Maor_T 2:9a4938a0475b 552 vehicleSpeed = 0.25;
Maor_T 2:9a4938a0475b 553
Maor_T 2:9a4938a0475b 554 }
Maor_T 2:9a4938a0475b 555 else if (cmd.find("speed2")!= string::npos)
Maor_T 2:9a4938a0475b 556 {
Maor_T 2:9a4938a0475b 557 vehicleSpeed = 0.50;
Maor_T 2:9a4938a0475b 558
Maor_T 2:9a4938a0475b 559 }
Maor_T 2:9a4938a0475b 560 else if (cmd.find("speed3")!= string::npos)
Maor_T 2:9a4938a0475b 561 {
Maor_T 2:9a4938a0475b 562 vehicleSpeed = 0.75;
Maor_T 2:9a4938a0475b 563
Maor_T 2:9a4938a0475b 564 }
Maor_T 2:9a4938a0475b 565 else if (cmd.find("speed4")!= string::npos)
Maor_T 2:9a4938a0475b 566 {
Maor_T 2:9a4938a0475b 567 vehicleSpeed = 1;
Maor_T 2:9a4938a0475b 568
Maor_T 2:9a4938a0475b 569 }
Maor_T 2:9a4938a0475b 570 else
Maor_T 2:9a4938a0475b 571 {
Maor_T 2:9a4938a0475b 572 Drive_Stop();
Maor_T 2:9a4938a0475b 573 }
Maor_T 2:9a4938a0475b 574
Maor_T 2:9a4938a0475b 575 }
Maor_T 2:9a4938a0475b 576
Maor_T 2:9a4938a0475b 577 void BTCommands2(char cmd)
Maor_T 2:9a4938a0475b 578 {
Maor_T 2:9a4938a0475b 579 #ifdef DEBUG_BT_COMMANDS
Maor_T 2:9a4938a0475b 580 printf("Incoming BT command : %c\n",cmd); // - Print the incoming command
Maor_T 2:9a4938a0475b 581 #endif
Maor_T 2:9a4938a0475b 582
Maor_T 2:9a4938a0475b 583 switch(cmd)
Maor_T 2:9a4938a0475b 584 {
Maor_T 2:9a4938a0475b 585 case 'F' : Drive_Forward(1); break;
Maor_T 2:9a4938a0475b 586 case 'B' : Drive_Backwards(1); break;
Maor_T 2:9a4938a0475b 587 case 'S' : Drive_Stop(); break;
Maor_T 2:9a4938a0475b 588 case 'L' : Drive_Left(1); break;
Maor_T 2:9a4938a0475b 589 case 'R' : Drive_Right(1); break;
Maor_T 2:9a4938a0475b 590 }
Maor_T 2:9a4938a0475b 591 }
Maor_T 2:9a4938a0475b 592
Maor_T 2:9a4938a0475b 593
Maor_T 2:9a4938a0475b 594 void init()
Maor_T 2:9a4938a0475b 595 {
Maor_T 2:9a4938a0475b 596 bt.baud(115200);
Maor_T 2:9a4938a0475b 597 bt.attach(&DataReceived_ISR,Serial::RxIrq);
Maor_T 2:9a4938a0475b 598 pc.attach(&DataReceived_PCISR,Serial::RxIrq);
Maor_T 2:9a4938a0475b 599 Reset_IncomingMsg();
Maor_T 2:9a4938a0475b 600 }
Maor_T 2:9a4938a0475b 601
Maor_T 2:9a4938a0475b 602
Maor_T 2:9a4938a0475b 603 void UltraSonic_Thread_Func(void const *args)
Maor_T 2:9a4938a0475b 604 {
Maor_T 2:9a4938a0475b 605 while(1)
Maor_T 2:9a4938a0475b 606 {
Maor_T 2:9a4938a0475b 607
Maor_T 2:9a4938a0475b 608 // lastDistance = GetDistance();
Maor_T 2:9a4938a0475b 609 usensor.start();
Maor_T 2:9a4938a0475b 610 lastDistance = usensor.get_dist_cm();
Maor_T 2:9a4938a0475b 611
Maor_T 2:9a4938a0475b 612 if(moveForward_flag)
Maor_T 2:9a4938a0475b 613 {
Maor_T 2:9a4938a0475b 614 lastDistance = Get_Min_Distance();
Maor_T 2:9a4938a0475b 615 }
Maor_T 2:9a4938a0475b 616
Maor_T 2:9a4938a0475b 617
Maor_T 2:9a4938a0475b 618 #ifdef DEBUG_ULTRASONICE
Maor_T 2:9a4938a0475b 619 pc.printf("Distance : %f\n",lastDistance );
Maor_T 2:9a4938a0475b 620 #endif
Maor_T 2:9a4938a0475b 621
Maor_T 2:9a4938a0475b 622 if(lastDistance < obstacleDistance && moveForward_flag ) { // todo : minimize code here
Maor_T 2:9a4938a0475b 623 autoDrivePartOk = false;
Maor_T 2:9a4938a0475b 624 Drive_Force_Stop();
Maor_T 2:9a4938a0475b 625 moveForward_flag = false;
Maor_T 2:9a4938a0475b 626 Servo_NoNo(1);
Maor_T 2:9a4938a0475b 627 }
Maor_T 2:9a4938a0475b 628 else if((turnRight_Flag || turnLeft_Flag) && lastDistance < obstacleDistance ) { // todo : minimize code here
Maor_T 2:9a4938a0475b 629 autoDrivePartOk = false;
Maor_T 2:9a4938a0475b 630 Drive_Stop();
Maor_T 2:9a4938a0475b 631
Maor_T 2:9a4938a0475b 632 // moveForward_flag = false;
Maor_T 2:9a4938a0475b 633 }
Maor_T 2:9a4938a0475b 634
Maor_T 2:9a4938a0475b 635
Maor_T 2:9a4938a0475b 636 Thread::wait(ULTRASONIC_CHECK_DELAY_MS);
Maor_T 2:9a4938a0475b 637 }
Maor_T 2:9a4938a0475b 638 }
Maor_T 2:9a4938a0475b 639
Maor_T 2:9a4938a0475b 640 void UltraSonic_Thread_Emergency(void const *args)
Maor_T 2:9a4938a0475b 641 {
Maor_T 2:9a4938a0475b 642 while(1)
Maor_T 2:9a4938a0475b 643 {
Maor_T 2:9a4938a0475b 644 usensor.start();
Maor_T 2:9a4938a0475b 645 lastDistance = usensor.get_dist_cm();
Maor_T 2:9a4938a0475b 646
Maor_T 2:9a4938a0475b 647 #ifdef DEBUG_ULTRASONICE
Maor_T 2:9a4938a0475b 648 pc.printf("Distance : %f\n",lastDistance );
Maor_T 2:9a4938a0475b 649 #endif
Maor_T 2:9a4938a0475b 650
Maor_T 2:9a4938a0475b 651 if(lastDistance < obstacleDistance && moveForward_flag ) { // todo : minimize code here
Maor_T 2:9a4938a0475b 652 Drive_Force_Stop();
Maor_T 2:9a4938a0475b 653 moveForward_flag = false;
Maor_T 2:9a4938a0475b 654 Servo_NoNo(1);
Maor_T 2:9a4938a0475b 655 }
Maor_T 2:9a4938a0475b 656 Thread::wait(ULTRASONIC_EMERGENCY_DELAY_MS);
Maor_T 2:9a4938a0475b 657 }
Maor_T 2:9a4938a0475b 658 }
Maor_T 2:9a4938a0475b 659
Maor_T 2:9a4938a0475b 660
Maor_T 2:9a4938a0475b 661
Maor_T 2:9a4938a0475b 662 void ExecuteNextCommand()
Maor_T 2:9a4938a0475b 663 {
Maor_T 2:9a4938a0475b 664 DriveCommand* dc = (DriveCommand*)malloc(sizeof(DriveCommand)) ;
Maor_T 2:9a4938a0475b 665 // printf("Before get from queue\n");
Maor_T 2:9a4938a0475b 666 autoDriverQueue.Get(dc);
Maor_T 2:9a4938a0475b 667 // printf("after get from queue\n");
Maor_T 2:9a4938a0475b 668 float time = dc->GetTime();
Maor_T 2:9a4938a0475b 669 char dir = dc->GetDir();
Maor_T 2:9a4938a0475b 670 // printf("AutoDrive Runing at Dir:%c, Time:%d\n",dir,time);
Maor_T 2:9a4938a0475b 671 autoDriveRun = true;
Maor_T 2:9a4938a0475b 672 autoDrivePartOk = true;
Maor_T 2:9a4938a0475b 673 switch(dir)
Maor_T 2:9a4938a0475b 674 {
Maor_T 2:9a4938a0475b 675 case 'f':
Maor_T 2:9a4938a0475b 676 Drive_Forward(vehicleSpeed);
Maor_T 2:9a4938a0475b 677 break;
Maor_T 2:9a4938a0475b 678 case 'b':
Maor_T 2:9a4938a0475b 679 Drive_Backwards(vehicleSpeed);
Maor_T 2:9a4938a0475b 680 break;
Maor_T 2:9a4938a0475b 681 case 'l':
Maor_T 2:9a4938a0475b 682 Turn_Left(turnSpeed);
Maor_T 2:9a4938a0475b 683 wait_ms(rotateLeftMillis);
Maor_T 2:9a4938a0475b 684 Drive_Forward(vehicleSpeed);
Maor_T 2:9a4938a0475b 685 break;
Maor_T 2:9a4938a0475b 686 case 'r':
Maor_T 2:9a4938a0475b 687 Turn_Right(turnSpeed);
Maor_T 2:9a4938a0475b 688 wait_ms(rotateRightMillis);
Maor_T 2:9a4938a0475b 689 Drive_Forward(vehicleSpeed);
Maor_T 2:9a4938a0475b 690 break;
Maor_T 2:9a4938a0475b 691 }
Maor_T 2:9a4938a0475b 692
Maor_T 2:9a4938a0475b 693 StopAfter(time);
Maor_T 2:9a4938a0475b 694 free(dc);
Maor_T 2:9a4938a0475b 695
Maor_T 2:9a4938a0475b 696 while(autoDriveRun)
Maor_T 2:9a4938a0475b 697 wait_ms(50);
Maor_T 2:9a4938a0475b 698
Maor_T 2:9a4938a0475b 699 if(autoDrivePartOk)
Maor_T 2:9a4938a0475b 700 bt.printf("ok%d~",autoDriveCounter);
Maor_T 2:9a4938a0475b 701 else
Maor_T 2:9a4938a0475b 702 bt.printf("fail%d~",autoDriveCounter);
Maor_T 2:9a4938a0475b 703
Maor_T 2:9a4938a0475b 704 autoDriveCounter++;
Maor_T 2:9a4938a0475b 705
Maor_T 2:9a4938a0475b 706
Maor_T 2:9a4938a0475b 707 }
Maor_T 2:9a4938a0475b 708
Maor_T 2:9a4938a0475b 709
Maor_T 2:9a4938a0475b 710
Maor_T 2:9a4938a0475b 711
Maor_T 2:9a4938a0475b 712
Maor_T 2:9a4938a0475b 713 void AutoDrive_Thread_Func(void const *args)
Maor_T 2:9a4938a0475b 714 {
Maor_T 2:9a4938a0475b 715 while(true) {
Maor_T 2:9a4938a0475b 716
Maor_T 2:9a4938a0475b 717 // todo add check for autoDriveFlag
Maor_T 2:9a4938a0475b 718 if(autoDriveFlag && autoDriverQueue.GetNumberOfItems() >0 )
Maor_T 2:9a4938a0475b 719 {
Maor_T 2:9a4938a0475b 720 // printf("Autodrive Commands before commit\n");
Maor_T 2:9a4938a0475b 721 if(!autoDriveRun) //check that there is no running command right now
Maor_T 2:9a4938a0475b 722 ExecuteNextCommand();
Maor_T 2:9a4938a0475b 723 // printf("Autodrive Commands after commit\n");
Maor_T 2:9a4938a0475b 724
Maor_T 2:9a4938a0475b 725 }
Maor_T 2:9a4938a0475b 726 else
Maor_T 2:9a4938a0475b 727 {
Maor_T 2:9a4938a0475b 728 autoDriveFlag = false;
Maor_T 2:9a4938a0475b 729 autoDriveCounter =0;
Maor_T 2:9a4938a0475b 730 }
Maor_T 2:9a4938a0475b 731
Maor_T 2:9a4938a0475b 732 //printf("Checking for Autodrive Commands\n");
Maor_T 2:9a4938a0475b 733 Thread::wait(AUTODRIVE_CHECK_DELAY_MS);
Maor_T 2:9a4938a0475b 734 }
Maor_T 2:9a4938a0475b 735 }
Maor_T 2:9a4938a0475b 736
Maor_T 2:9a4938a0475b 737
Maor_T 2:9a4938a0475b 738
Maor_T 2:9a4938a0475b 739
Maor_T 2:9a4938a0475b 740 void BT_Thread_Func(void const *args)
Maor_T 2:9a4938a0475b 741 {
Maor_T 2:9a4938a0475b 742 // while(true) {
Maor_T 2:9a4938a0475b 743 // if(globalString.length() > 0 )
Maor_T 2:9a4938a0475b 744 // {
Maor_T 2:9a4938a0475b 745 // BTCommands(globalString);
Maor_T 2:9a4938a0475b 746 // globalString = "";
Maor_T 2:9a4938a0475b 747 // }
Maor_T 2:9a4938a0475b 748 // Thread::wait(100);
Maor_T 2:9a4938a0475b 749 // }
Maor_T 2:9a4938a0475b 750 //
Maor_T 2:9a4938a0475b 751 while(true) {
Maor_T 2:9a4938a0475b 752 // if(lastCommandChar != 'x' )
Maor_T 2:9a4938a0475b 753 // {
Maor_T 2:9a4938a0475b 754 // BTCommands2(lastCommandChar);
Maor_T 2:9a4938a0475b 755 // lastCommandChar = 'x';
Maor_T 2:9a4938a0475b 756 // }
Maor_T 2:9a4938a0475b 757
Maor_T 2:9a4938a0475b 758 string cmd = Get_last_Msg();
Maor_T 2:9a4938a0475b 759 if(succesReadFlag)
Maor_T 2:9a4938a0475b 760 {
Maor_T 2:9a4938a0475b 761
Maor_T 2:9a4938a0475b 762 BTCommands(cmd);
Maor_T 2:9a4938a0475b 763 succesReadFlag = false;
Maor_T 2:9a4938a0475b 764 }
Maor_T 2:9a4938a0475b 765
Maor_T 2:9a4938a0475b 766 Thread::wait(BT_CHECK_DELAY_MS);
Maor_T 2:9a4938a0475b 767 }
Maor_T 2:9a4938a0475b 768 }
Maor_T 2:9a4938a0475b 769
ejteb 0:1704ea055c4f 770
ejteb 0:1704ea055c4f 771 int main()
ejteb 0:1704ea055c4f 772 {
Maor_T 2:9a4938a0475b 773 // mu.startUpdates();//start mesuring the distance
Maor_T 2:9a4938a0475b 774
Maor_T 2:9a4938a0475b 775 init();
Maor_T 2:9a4938a0475b 776 printf("Hello\n");
Maor_T 2:9a4938a0475b 777 Thread btThread(BT_Thread_Func);
Maor_T 2:9a4938a0475b 778 btThread.set_priority(osPriorityNormal);
Maor_T 2:9a4938a0475b 779 Thread ultraSonicThread(UltraSonic_Thread_Func);
Maor_T 2:9a4938a0475b 780 ultraSonicThread.set_priority(osPriorityNormal);
Maor_T 2:9a4938a0475b 781 Thread ultraSonicEmergencyThread(UltraSonic_Thread_Emergency);
Maor_T 2:9a4938a0475b 782 ultraSonicEmergencyThread.set_priority(osPriorityNormal);
Maor_T 2:9a4938a0475b 783 Thread AutoDriveThread(AutoDrive_Thread_Func);
Maor_T 2:9a4938a0475b 784 AutoDriveThread.set_priority(osPriorityNormal);
Maor_T 2:9a4938a0475b 785
Maor_T 2:9a4938a0475b 786
Maor_T 2:9a4938a0475b 787
Maor_T 2:9a4938a0475b 788
Maor_T 2:9a4938a0475b 789 Servo_Look_Left();
Maor_T 2:9a4938a0475b 790 wait(2);
Maor_T 2:9a4938a0475b 791 Servo_Look_Right();
Maor_T 2:9a4938a0475b 792 wait(2);
Maor_T 2:9a4938a0475b 793 Servo_Look_Center();
Maor_T 2:9a4938a0475b 794
Maor_T 2:9a4938a0475b 795
Maor_T 2:9a4938a0475b 796 // Drive_Forward(0.5);
Maor_T 2:9a4938a0475b 797
Maor_T 2:9a4938a0475b 798 // StopAfter(3.0);
Maor_T 2:9a4938a0475b 799
Maor_T 2:9a4938a0475b 800
Maor_T 2:9a4938a0475b 801 // autoDriverQueue.Put(&DriveCommand('f',120));
Maor_T 2:9a4938a0475b 802
ejteb 0:1704ea055c4f 803 while(1)
ejteb 0:1704ea055c4f 804 {
Maor_T 2:9a4938a0475b 805 // mu._checkDistance();
ejteb 1:4a5586eb1765 806 //Do something else here
Maor_T 2:9a4938a0475b 807 // mu.checkDistance(); //call checkDistance() as much as possible, as this is where //the class checks if dist needs to be called.
ejteb 0:1704ea055c4f 808 }
Maor_T 2:9a4938a0475b 809
ejteb 0:1704ea055c4f 810 }
Maor_T 2:9a4938a0475b 811
Maor_T 2:9a4938a0475b 812
Maor_T 2:9a4938a0475b 813