James Plager / Mbed 2 deprecated IMURoomba4_ThrowSumMo

ECE4180 Final Project

Dependencies:   LSM9DS1_Library Motor mbed-rtos mbed

Fork of IMURoomba4_ThrowSumMo by James Plager

main.cpp@4:63e69557142e, 2017-05-04 (annotated)

Committer:
jplager3
Date:
Thu May 04 19:45:42 2017 +0000
Revision:
4:63e69557142e
Parent:
3:17113c72186e 
Changes to timing and other stuff

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jplager3 0:c29fc80c3ca3 1 #include "mbed.h"
jplager3 0:c29fc80c3ca3 2 #include "LSM9DS1.h"
jplager3 0:c29fc80c3ca3 3 #include "rtos.h"
jplager3 0:c29fc80c3ca3 4 //#include "SDFileSystem.h"
jplager3 0:c29fc80c3ca3 5 #include "Motor.h"
jplager3 0:c29fc80c3ca3 6 //#include "wave_player.h"
jplager3 0:c29fc80c3ca3 7
jplager3 0:c29fc80c3ca3 8 #define PI 3.14159
jplager3 0:c29fc80c3ca3 9 // Earth's magnetic field varies by location. Add or subtract
jplager3 0:c29fc80c3ca3 10 // a declination to get a more accurate heading. Calculate
jplager3 0:c29fc80c3ca3 11 // your's here:
jplager3 0:c29fc80c3ca3 12 // http://www.ngdc.noaa.gov/geomag-web/#declination
jplager3 0:c29fc80c3ca3 13 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
jplager3 0:c29fc80c3ca3 14 //collab test
jplager3 0:c29fc80c3ca3 15
jplager3 0:c29fc80c3ca3 16 Serial pc(USBTX, USBRX);
jplager3 0:c29fc80c3ca3 17 //RawSerial pc(USBTX, USBRX);
jplager3 3:17113c72186e 18 Serial dev(p28,p27); //
jplager3 0:c29fc80c3ca3 19 //RawSerial dev(p28,p27); //tx, rx
jplager3 0:c29fc80c3ca3 20 DigitalOut myled(LED1);
jplager3 0:c29fc80c3ca3 21 DigitalOut led2(LED2);
jplager3 1:6b8a201f4f90 22 DigitalOut led3(LED3);
jplager3 0:c29fc80c3ca3 23 DigitalOut led4(LED4);
jplager3 0:c29fc80c3ca3 24 //IR sensors on p19(front) & p20 (right)
jplager3 0:c29fc80c3ca3 25 AnalogIn IR1(p19);
jplager3 0:c29fc80c3ca3 26 AnalogIn IR2(p20);
jplager3 0:c29fc80c3ca3 27 //L and R DC motors
jplager3 0:c29fc80c3ca3 28 Motor Left(p21, p14, p13); // green wires. pwm, fwd, rev, add ", 1" for braking
jplager3 0:c29fc80c3ca3 29 Motor Right(p22, p12, p11); // red wires
jplager3 0:c29fc80c3ca3 30 // Speaker out
jplager3 3:17113c72186e 31 //AnalogOut DACout(p18); //must(?) be p18
jplager3 0:c29fc80c3ca3 32 //SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
jplager3 0:c29fc80c3ca3 33 Thread thread1;
jplager3 3:17113c72186e 34 //Thread thread2;
jplager3 3:17113c72186e 35 //Mutex BTmutex; //mutex for send/recv data on Bluetooth
jplager3 1:6b8a201f4f90 36 Mutex mutex; //other mutex for global resources
jplager3 0:c29fc80c3ca3 37
jplager3 1:6b8a201f4f90 38 //Globals
jplager3 3:17113c72186e 39 float throttle = 0.5;
jplager3 1:6b8a201f4f90 40 float currIR1;
jplager3 1:6b8a201f4f90 41 float currIR2;
jplager3 3:17113c72186e 42 //float origHeading;
jplager3 3:17113c72186e 43 //float heading;
jplager3 0:c29fc80c3ca3 44
jplager3 0:c29fc80c3ca3 45 // Calculate pitch, roll, and heading.
jplager3 0:c29fc80c3ca3 46 // Pitch/roll calculations taken from this app note:
jplager3 0:c29fc80c3ca3 47 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
jplager3 0:c29fc80c3ca3 48 // Heading calculations taken from this app note:
jplager3 0:c29fc80c3ca3 49 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
jplager3 0:c29fc80c3ca3 50 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
jplager3 3:17113c72186e 51 {
jplager3 3:17113c72186e 52 //entire subroutine is BTmutexed already
jplager3 0:c29fc80c3ca3 53 float roll = atan2(ay, az);
jplager3 0:c29fc80c3ca3 54 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
jplager3 0:c29fc80c3ca3 55 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
jplager3 0:c29fc80c3ca3 56 mx = -mx;
jplager3 3:17113c72186e 57 float heading; //was global
jplager3 1:6b8a201f4f90 58 if (my == 0.0) {
jplager3 3:17113c72186e 59 //mutex.lock(); //heading isn't global mutexes not needed
jplager3 0:c29fc80c3ca3 60 heading = (mx < 0.0) ? 180.0 : 0.0;
jplager3 3:17113c72186e 61 //mutex.unlock();
jplager3 3:17113c72186e 62 } else {
jplager3 3:17113c72186e 63 //mutex.lock();
jplager3 0:c29fc80c3ca3 64 heading = atan2(mx, my)*360.0/(2.0*PI);
jplager3 3:17113c72186e 65 //mutex.unlock();
jplager3 1:6b8a201f4f90 66 }
jplager3 0:c29fc80c3ca3 67 //pc.printf("heading atan=%f \n\r",heading);
jplager3 3:17113c72186e 68 //mutex.lock();
jplager3 0:c29fc80c3ca3 69 heading -= DECLINATION; //correct for geo location
jplager3 0:c29fc80c3ca3 70 if(heading>180.0) heading = heading - 360.0;
jplager3 0:c29fc80c3ca3 71 else if(heading<-180.0) heading = 360.0 + heading;
jplager3 0:c29fc80c3ca3 72 else if(heading<0.0) heading = 360.0 + heading;
jplager3 3:17113c72186e 73 //mutex.unlock();
jplager3 0:c29fc80c3ca3 74 // Convert everything from radians to degrees:
jplager3 0:c29fc80c3ca3 75 //heading *= 180.0 / PI;
jplager3 0:c29fc80c3ca3 76 pitch *= 180.0 / PI;
jplager3 0:c29fc80c3ca3 77 roll *= 180.0 / PI;
jplager3 3:17113c72186e 78 mutex.lock();
jplager3 0:c29fc80c3ca3 79 //~pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
jplager3 0:c29fc80c3ca3 80 //~pc.printf("Magnetic Heading: %f degress\n\r",heading);
jplager3 3:17113c72186e 81 //dev.printf("Magnetic Heading: %f degrees\n\r",heading);
jplager3 3:17113c72186e 82 mutex.unlock();
jplager3 0:c29fc80c3ca3 83 }
jplager3 0:c29fc80c3ca3 84
jplager3 0:c29fc80c3ca3 85 /*
jplager3 0:c29fc80c3ca3 86 void dev_recv()
jplager3 0:c29fc80c3ca3 87 {
jplager3 0:c29fc80c3ca3 88 led2 = !led2;
jplager3 0:c29fc80c3ca3 89 while(dev.readable()) {
jplager3 0:c29fc80c3ca3 90 pc.putc(dev.getc());
jplager3 0:c29fc80c3ca3 91 }
jplager3 0:c29fc80c3ca3 92 }
jplager3 0:c29fc80c3ca3 93 void pc_recv()
jplager3 0:c29fc80c3ca3 94 {
jplager3 0:c29fc80c3ca3 95 led4 = !led4;
jplager3 0:c29fc80c3ca3 96 while(pc.readable()) {
jplager3 0:c29fc80c3ca3 97 dev.putc(pc.getc());
jplager3 0:c29fc80c3ca3 98 }
jplager3 0:c29fc80c3ca3 99 }*/
jplager3 0:c29fc80c3ca3 100
jplager3 3:17113c72186e 101 // Driving Methods
jplager3 3:17113c72186e 102 void forward(float speed)
jplager3 3:17113c72186e 103 {
jplager3 0:c29fc80c3ca3 104 Left.speed(speed);
jplager3 3:17113c72186e 105 Right.speed(speed);
jplager3 0:c29fc80c3ca3 106 }
jplager3 3:17113c72186e 107 void reverse(float speed)
jplager3 3:17113c72186e 108 {
jplager3 0:c29fc80c3ca3 109 Left.speed(-speed);
jplager3 0:c29fc80c3ca3 110 Right.speed(-speed);
jplager3 0:c29fc80c3ca3 111 }
jplager3 3:17113c72186e 112 void turnRight(float speed)
jplager3 3:17113c72186e 113 {
jplager3 0:c29fc80c3ca3 114 Left.speed(speed);
jplager3 0:c29fc80c3ca3 115 Right.speed(-speed);
jplager3 0:c29fc80c3ca3 116 //wait(0.7);
jplager3 0:c29fc80c3ca3 117 }
jplager3 3:17113c72186e 118 void turnLeft(float speed)
jplager3 3:17113c72186e 119 {
jplager3 0:c29fc80c3ca3 120 Left.speed(-speed);
jplager3 0:c29fc80c3ca3 121 Right.speed(speed);
jplager3 0:c29fc80c3ca3 122 //wait(0.7);
jplager3 0:c29fc80c3ca3 123 }
jplager3 3:17113c72186e 124 void stop()
jplager3 3:17113c72186e 125 {
jplager3 0:c29fc80c3ca3 126 Left.speed(0.0);
jplager3 0:c29fc80c3ca3 127 Right.speed(0.0);
jplager3 0:c29fc80c3ca3 128 }
jplager3 0:c29fc80c3ca3 129
jplager3 3:17113c72186e 130 void IMU()
jplager3 3:17113c72186e 131 {
jplager3 0:c29fc80c3ca3 132 //IMU setup
jplager3 0:c29fc80c3ca3 133 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); // this executes. Pins are correct. Changing them causes fault
jplager3 3:17113c72186e 134 IMU.begin();
jplager3 3:17113c72186e 135 if (!IMU.begin()) {
jplager3 0:c29fc80c3ca3 136 led2=1;
jplager3 0:c29fc80c3ca3 137 pc.printf("Failed to communicate with LSM9DS1.\n");
jplager3 0:c29fc80c3ca3 138 }
jplager3 0:c29fc80c3ca3 139 IMU.calibrate(1);
jplager3 0:c29fc80c3ca3 140 IMU.calibrateMag(0);
jplager3 3:17113c72186e 141
jplager3 0:c29fc80c3ca3 142 while(1) {
jplager3 0:c29fc80c3ca3 143 //myled = 1;
jplager3 0:c29fc80c3ca3 144 while(!IMU.magAvailable(X_AXIS));
jplager3 0:c29fc80c3ca3 145 IMU.readMag();
jplager3 0:c29fc80c3ca3 146 //myled = 0;
jplager3 0:c29fc80c3ca3 147 while(!IMU.accelAvailable());
jplager3 0:c29fc80c3ca3 148 IMU.readAccel();
jplager3 0:c29fc80c3ca3 149 while(!IMU.gyroAvailable());
jplager3 0:c29fc80c3ca3 150 IMU.readGyro();
jplager3 3:17113c72186e 151 //mutex.lock(); //changed from BTmutex
jplager3 3:17113c72186e 152 //pc.puts(" X axis Y axis Z axis\n\r");
jplager3 1:6b8a201f4f90 153 //dev.puts(" X axis Y axis Z axis\n\r");
jplager3 3:17113c72186e 154 //pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
jplager3 3:17113c72186e 155 //pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
jplager3 3:17113c72186e 156 //pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jplager3 1:6b8a201f4f90 157 //dev.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jplager3 0:c29fc80c3ca3 158 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
jplager3 0:c29fc80c3ca3 159 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jplager3 3:17113c72186e 160 //mutex.unlock(); //changed from BTmutex
jplager3 2:8887d13f967a 161 //myled = 1;
jplager3 0:c29fc80c3ca3 162 wait(0.5);
jplager3 2:8887d13f967a 163 //myled = 0;
jplager3 3:17113c72186e 164 wait(0.5);
jplager3 3:17113c72186e 165 }
jplager3 0:c29fc80c3ca3 166 }
jplager3 0:c29fc80c3ca3 167
jplager3 3:17113c72186e 168 void avoidObstacle()
jplager3 3:17113c72186e 169 {
jplager3 3:17113c72186e 170 //currIR1 = IR1; //get IR readings - already received from main thread that initially decided to call avoidObstacle()
jplager3 3:17113c72186e 171 //currIR2 = IR2;
jplager3 1:6b8a201f4f90 172 stop();
jplager3 1:6b8a201f4f90 173 Thread::wait(300);
jplager3 3:17113c72186e 174 //BTmutex.lock();
jplager3 1:6b8a201f4f90 175 //dev.printf("Collision Detected!\n\r");
jplager3 3:17113c72186e 176 //BTmutex.unlock();
jplager3 1:6b8a201f4f90 177 //dev.printf("Turning Left...\n\r");
jplager3 1:6b8a201f4f90 178 turnLeft(0.4); //turn 90deg
jplager3 1:6b8a201f4f90 179 Thread::wait(1000); //time to turn estimate
jplager3 1:6b8a201f4f90 180 stop();
jplager3 3:17113c72186e 181 Thread::wait(500);
jplager3 1:6b8a201f4f90 182 // turn should be complete. Drive until obstacle passed on right, then turn right again
jplager3 2:8887d13f967a 183 //BTmutex.lock();
jplager3 1:6b8a201f4f90 184 //dev.printf("Driving past obstacle.\n\r");
jplager3 2:8887d13f967a 185 //BTmutex.unlock();
jplager3 1:6b8a201f4f90 186 forward(throttle);
jplager3 1:6b8a201f4f90 187 bool objOnRight = true;
jplager3 3:17113c72186e 188 while(objOnRight) {
jplager3 3:17113c72186e 189 mutex.lock();
jplager3 3:17113c72186e 190 dev.printf("Avoiding Obstacles...\n\r");
jplager3 3:17113c72186e 191 currIR1 = IR1; //must keep scanning IR readers to know when object is cleared
jplager3 1:6b8a201f4f90 192 currIR2 = IR2;
jplager3 3:17113c72186e 193 dev.printf(" IR1 Reading IR2 Reading\n\r %f %f\n\r", currIR1, currIR2);
jplager3 3:17113c72186e 194 mutex.unlock();
jplager3 3:17113c72186e 195 if(currIR2 < 0.7) {
jplager3 3:17113c72186e 196 objOnRight = false; //if IR2 drops below threshold, obstacle passed. Break out of loop
jplager3 3:17113c72186e 197
jplager3 3:17113c72186e 198 wait(0.5); //give robot time to drive past object
jplager3 3:17113c72186e 199 }
jplager3 3:17113c72186e 200 if(currIR1 > 0.8){ // don;t crash to anything in front
jplager3 3:17113c72186e 201 stop();
jplager3 3:17113c72186e 202 myled=led2=led3=led4=1;
jplager3 3:17113c72186e 203 }
jplager3 4:63e69557142e 204 //Thread::wait(1250); //
jplager3 1:6b8a201f4f90 205 }
jplager3 1:6b8a201f4f90 206 stop();
jplager3 1:6b8a201f4f90 207 Thread::wait(250);
jplager3 3:17113c72186e 208 //BTmutex.lock();
jplager3 1:6b8a201f4f90 209 //dev.printf("Object passed. Turning right...\n\r");
jplager3 3:17113c72186e 210 turnRight(0.5); // turn 90deg
jplager3 1:6b8a201f4f90 211 Thread::wait(1000); //time to turn estimate
jplager3 1:6b8a201f4f90 212 stop();
jplager3 1:6b8a201f4f90 213 Thread::wait(1000);
jplager3 3:17113c72186e 214 forward(throttle);
jplager3 1:6b8a201f4f90 215 }
jplager3 1:6b8a201f4f90 216
jplager3 3:17113c72186e 217 /*
jplager3 3:17113c72186e 218 void defaultDrive() //default behavior for robot //moved to main instead of being a thread
jplager3 3:17113c72186e 219 {
jplager3 1:6b8a201f4f90 220 //Drive forward until object detected. Stop, turn left, then drive until IR2 says path is clear, then turn right to go around object.
jplager3 1:6b8a201f4f90 221 forward(throttle);
jplager3 3:17113c72186e 222 while(1) {
jplager3 3:17113c72186e 223 myled=1;
jplager3 1:6b8a201f4f90 224 //update current IR readings
jplager3 1:6b8a201f4f90 225 currIR1 = IR1;
jplager3 1:6b8a201f4f90 226 currIR2 = IR2;
jplager3 1:6b8a201f4f90 227 BTmutex.lock(); //prevent race conditions in BT dataoutput
jplager3 3:17113c72186e 228 //dev.puts(" Front IR reading Right IR reading\n\r"); // print IR readings over BT
jplager3 1:6b8a201f4f90 229 //dev.printf(" %2f %2f\n\r", currIR1, currIR2);
jplager3 3:17113c72186e 230 pc.puts(" Front IR reading Right IR reading\n\r"); // print IR readings over BT
jplager3 3:17113c72186e 231 pc.printf(" %2f %2f\n\r", currIR1, currIR2);
jplager3 3:17113c72186e 232
jplager3 1:6b8a201f4f90 233 BTmutex.unlock();
jplager3 1:6b8a201f4f90 234 // Forward collision handling code block
jplager3 1:6b8a201f4f90 235 if(currIR1 > 0.8) { // 0.85 is threshold for collision
jplager3 3:17113c72186e 236 led3=1;
jplager3 1:6b8a201f4f90 237 avoidObstacle(); // steer around obstacle when detected
jplager3 3:17113c72186e 238 led3=0;
jplager3 3:17113c72186e 239 }
jplager3 1:6b8a201f4f90 240 Thread::wait(400); // for debug. IR polling too quick and floods output terminal
jplager3 3:17113c72186e 241 wait(0.4);
jplager3 3:17113c72186e 242 myled=0;
jplager3 0:c29fc80c3ca3 243 }
jplager3 0:c29fc80c3ca3 244 }
jplager3 3:17113c72186e 245 */
jplager3 0:c29fc80c3ca3 246
jplager3 3:17113c72186e 247 /*
jplager3 3:17113c72186e 248 void manualMode() // also moved to main
jplager3 3:17113c72186e 249 {
jplager3 1:6b8a201f4f90 250 bool on = true;
jplager3 2:8887d13f967a 251 char temp;
jplager3 3:17113c72186e 252 while(on) {
jplager3 2:8887d13f967a 253 temp = dev.getc();
jplager3 3:17113c72186e 254 if(temp == 'A') { // reset command
jplager3 1:6b8a201f4f90 255 on = false;
jplager3 3:17113c72186e 256 } else if(temp=='U') {
jplager3 1:6b8a201f4f90 257 led2=led3=1;
jplager3 1:6b8a201f4f90 258 forward(throttle);
jplager3 1:6b8a201f4f90 259 wait(1);
jplager3 1:6b8a201f4f90 260 led2=led3=0;
jplager3 3:17113c72186e 261 } else if(temp=='L') { // turn left
jplager3 1:6b8a201f4f90 262 myled=led2=1; //debug
jplager3 1:6b8a201f4f90 263 stop();
jplager3 1:6b8a201f4f90 264 wait(0.3);
jplager3 1:6b8a201f4f90 265 turnLeft(0.4);
jplager3 1:6b8a201f4f90 266 wait(0.6);
jplager3 1:6b8a201f4f90 267 stop();
jplager3 1:6b8a201f4f90 268 wait(0.3);
jplager3 1:6b8a201f4f90 269 forward(throttle);
jplager3 1:6b8a201f4f90 270 myled=led2=0; //debug
jplager3 3:17113c72186e 271 } else if(temp=='R') { // turn right
jplager3 1:6b8a201f4f90 272 led3=led4=1;
jplager3 1:6b8a201f4f90 273 stop();
jplager3 1:6b8a201f4f90 274 wait(0.3);
jplager3 1:6b8a201f4f90 275 turnRight(0.4);
jplager3 1:6b8a201f4f90 276 wait(0.6);
jplager3 1:6b8a201f4f90 277 stop();
jplager3 1:6b8a201f4f90 278 wait(0.3);
jplager3 1:6b8a201f4f90 279 forward(throttle);
jplager3 1:6b8a201f4f90 280 led3=led4=0;
jplager3 3:17113c72186e 281 } else if(temp=='X') { // halt/brake command
jplager3 1:6b8a201f4f90 282 stop();
jplager3 1:6b8a201f4f90 283 }
jplager3 2:8887d13f967a 284 //myled=1;
jplager3 2:8887d13f967a 285 //wait(0.5);
jplager3 2:8887d13f967a 286 //myled=0;
jplager3 2:8887d13f967a 287 //wait(0.5);
jplager3 1:6b8a201f4f90 288 }
jplager3 1:6b8a201f4f90 289 }
jplager3 3:17113c72186e 290 */
jplager3 3:17113c72186e 291
jplager3 3:17113c72186e 292 /*
jplager3 3:17113c72186e 293 void updateIRs()
jplager3 3:17113c72186e 294 {
jplager3 3:17113c72186e 295 mutex.lock();
jplager3 3:17113c72186e 296 currIR1 = IR1; //must keep scanning IR readers to know when object is cleared
jplager3 3:17113c72186e 297 currIR2 = IR2;
jplager3 3:17113c72186e 298 mutex.unlock();
jplager3 3:17113c72186e 299 }*/
jplager3 1:6b8a201f4f90 300
jplager3 0:c29fc80c3ca3 301 int main()
jplager3 0:c29fc80c3ca3 302 {
jplager3 1:6b8a201f4f90 303 //bluetooth setup
jplager3 1:6b8a201f4f90 304 pc.baud(9600);
jplager3 1:6b8a201f4f90 305 dev.baud(9600);
jplager3 3:17113c72186e 306 //wait to recv start command or time delay
jplager3 3:17113c72186e 307 for(int i=0; i<3; i++) { //temp delay for a few sec
jplager3 1:6b8a201f4f90 308 myled=led2=led3=led4=1;
jplager3 1:6b8a201f4f90 309 wait(0.5);
jplager3 1:6b8a201f4f90 310 myled=led2=led3=led4=0;
jplager3 1:6b8a201f4f90 311 wait(0.5);
jplager3 3:17113c72186e 312 }
jplager3 0:c29fc80c3ca3 313 thread1.start(IMU); // start the IMU thread
jplager3 3:17113c72186e 314 char state = 'D'; //Roomba's drive state
jplager3 1:6b8a201f4f90 315 char temp;
jplager3 2:8887d13f967a 316 /*
jplager3 1:6b8a201f4f90 317 while(1){ //robot will receive a char from GUI signalling time to start
jplager3 1:6b8a201f4f90 318 temp = dev.getc();
jplager3 1:6b8a201f4f90 319 led3=1;
jplager3 1:6b8a201f4f90 320 pc.putc(temp);
jplager3 1:6b8a201f4f90 321 if (temp == 'B'){
jplager3 1:6b8a201f4f90 322 break;
jplager3 1:6b8a201f4f90 323 }
jplager3 1:6b8a201f4f90 324 if(led2 == 0) led2 = 1;
jplager3 1:6b8a201f4f90 325 else {led2 = 0;}
jplager3 1:6b8a201f4f90 326 wait(0.25);
jplager3 3:17113c72186e 327 }
jplager3 2:8887d13f967a 328 */
jplager3 1:6b8a201f4f90 329 led3=0;
jplager3 3:17113c72186e 330 //thread2.start(defaultDrive); default drive inserted into main while
jplager3 3:17113c72186e 331 while(1) {
jplager3 3:17113c72186e 332 //Drive forward until object detected. Stop, turn left, then drive until IR2 says path is clear, then turn right to go around object.
jplager3 3:17113c72186e 333 forward(throttle);
jplager3 3:17113c72186e 334 while(state == 'D') { //default drive
jplager3 3:17113c72186e 335 myled=1;
jplager3 3:17113c72186e 336 //update current IR readings
jplager3 3:17113c72186e 337 //mutex.lock();//IR readings included in mutex since they are shared global variables
jplager3 3:17113c72186e 338 currIR1 = IR1;
jplager3 3:17113c72186e 339 currIR2 = IR2;
jplager3 3:17113c72186e 340 mutex.lock(); //prevent race conditions in BT dataoutput //changed from BTmutex
jplager3 3:17113c72186e 341 dev.puts(" Front IR reading Right IR reading\n\r"); // print IR readings over BT
jplager3 3:17113c72186e 342 dev.printf(" %2f %2f\n\r", currIR1, currIR2);
jplager3 3:17113c72186e 343 //pc.puts(" Front IR reading Right IR reading\n\r"); // print IR readings over serial
jplager3 3:17113c72186e 344 //pc.printf(" %2f %2f\n\r", currIR1, currIR2);
jplager3 3:17113c72186e 345 mutex.unlock(); // changed from BTmutex
jplager3 3:17113c72186e 346
jplager3 3:17113c72186e 347 // Forward collision handling code block
jplager3 3:17113c72186e 348 if(currIR1 > 0.8) { // 0.85 is threshold for collision
jplager3 3:17113c72186e 349 led3=1;
jplager3 3:17113c72186e 350 avoidObstacle(); // steer around obstacle when detected
jplager3 3:17113c72186e 351 led3=0;
jplager3 3:17113c72186e 352 }
jplager3 3:17113c72186e 353 Thread::wait(400); // for debug. IR polling too quick and floods output terminal
jplager3 3:17113c72186e 354 wait(0.4);
jplager3 3:17113c72186e 355 myled=0;
jplager3 3:17113c72186e 356
jplager3 3:17113c72186e 357 //was already ITT
jplager3 3:17113c72186e 358 if (dev.readable()) {
jplager3 3:17113c72186e 359 mutex.lock();
jplager3 3:17113c72186e 360 temp = dev.getc();
jplager3 3:17113c72186e 361 mutex.unlock();
jplager3 3:17113c72186e 362 }
jplager3 3:17113c72186e 363 if(temp == 'M') {
jplager3 3:17113c72186e 364 led4=1;
jplager3 3:17113c72186e 365 stop();
jplager3 3:17113c72186e 366 //thread2.terminate(); //stop default drive
jplager3 3:17113c72186e 367 //manualMode(); //switch to manual control
jplager3 3:17113c72186e 368 /*
jplager3 3:17113c72186e 369 while(1) {
jplager3 3:17113c72186e 370 temp = dev.getc();
jplager3 3:17113c72186e 371 if(temp=='U') {
jplager3 3:17113c72186e 372 led2=1;
jplager3 3:17113c72186e 373 }
jplager3 3:17113c72186e 374 } */
jplager3 3:17113c72186e 375 //once manualMode is exited, return to default
jplager3 3:17113c72186e 376 led4=0;
jplager3 3:17113c72186e 377 //thread2.start(defaultDrive);
jplager3 3:17113c72186e 378 state = 'M';
jplager3 3:17113c72186e 379 }
jplager3 3:17113c72186e 380 }
jplager3 3:17113c72186e 381
jplager3 3:17113c72186e 382 while(state == 'M') {
jplager3 3:17113c72186e 383 if (dev.readable()){
jplager3 3:17113c72186e 384 mutex.lock();
jplager3 3:17113c72186e 385 temp = dev.getc();
jplager3 3:17113c72186e 386 mutex.unlock();
jplager3 3:17113c72186e 387 }
jplager3 3:17113c72186e 388 if(temp == 'A') { // reset command
jplager3 3:17113c72186e 389 state = 'D';
jplager3 3:17113c72186e 390 } else if(temp=='U') {
jplager3 3:17113c72186e 391 led2=led3=1;
jplager3 3:17113c72186e 392 forward(throttle);
jplager3 3:17113c72186e 393 wait(1);
jplager3 3:17113c72186e 394 led2=led3=0;
jplager3 3:17113c72186e 395 } else if(temp=='L') { // turn left
jplager3 3:17113c72186e 396 myled=led2=1; //debug
jplager3 3:17113c72186e 397 stop();
jplager3 3:17113c72186e 398 wait(0.5);
jplager3 3:17113c72186e 399 turnLeft(0.4);
jplager3 3:17113c72186e 400 wait(1);
jplager3 3:17113c72186e 401 stop();
jplager3 3:17113c72186e 402 wait(0.5);
jplager3 3:17113c72186e 403 forward(throttle);
jplager3 3:17113c72186e 404 myled=led2=0; //debug
jplager3 3:17113c72186e 405 } else if(temp=='R') { // turn right
jplager3 3:17113c72186e 406 led3=led4=1;
jplager3 3:17113c72186e 407 stop();
jplager3 3:17113c72186e 408 wait(0.3);
jplager3 3:17113c72186e 409 turnRight(0.5);
jplager3 3:17113c72186e 410 wait(0.6);
jplager3 3:17113c72186e 411 stop();
jplager3 3:17113c72186e 412 wait(0.3);
jplager3 3:17113c72186e 413 forward(throttle);
jplager3 3:17113c72186e 414 led3=led4=0;
jplager3 3:17113c72186e 415 } else if(temp=='X') { // halt/brake command
jplager3 3:17113c72186e 416 stop();
jplager3 3:17113c72186e 417 }
jplager3 3:17113c72186e 418 //myled=1;
jplager3 3:17113c72186e 419 //wait(0.5);
jplager3 3:17113c72186e 420 //myled=0;
jplager3 3:17113c72186e 421 //wait(0.5);
jplager3 1:6b8a201f4f90 422 }
jplager3 0:c29fc80c3ca3 423 }
jplager3 3:17113c72186e 424 }