James Plager / Mbed 2 deprecated IMURoomba4

ECE4180 Final Project

Dependencies:   LSM9DS1_Library Motor mbed-rtos mbed HC_SR04_Ultrasonic_Library

Fork of IMURoomba4_ThrowSumMo by James Plager

main.cpp@6:7123768ea0c9, 2017-05-04 (annotated)

Committer:
CRaslawski
Date:
Thu May 04 23:54:17 2017 +0000
Revision:
6:7123768ea0c9
Parent:
5:ab5fd9a37d7a 
Found out some of the pc prints over serial were causing the C# app to freeze. After disabling them all the app works fine. Currently only sends chars to the app over BT. Any chars the app sends to the robot over BT are echoed to the pc serial port

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