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@1:6b8a201f4f90, 2017-05-03 (annotated)

Committer:
jplager3
Date:
Wed May 03 01:53:52 2017 +0000
Revision:
1:6b8a201f4f90
Parent:
0:c29fc80c3ca3
Child:
2:8887d13f967a
Added wireless manual control, some finesse to autonomy and other bs

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 0:c29fc80c3ca3 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 0:c29fc80c3ca3 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 0:c29fc80c3ca3 34 Thread thread2;
jplager3 1:6b8a201f4f90 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 1:6b8a201f4f90 39 float throttle = 0.3;
jplager3 1:6b8a201f4f90 40 float currIR1;
jplager3 1:6b8a201f4f90 41 float currIR2;
jplager3 1:6b8a201f4f90 42 float origHeading;
jplager3 1:6b8a201f4f90 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 1:6b8a201f4f90 51 { //entire subroutine is BTmutexed already
jplager3 0:c29fc80c3ca3 52 float roll = atan2(ay, az);
jplager3 0:c29fc80c3ca3 53 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
jplager3 0:c29fc80c3ca3 54 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
jplager3 0:c29fc80c3ca3 55 mx = -mx;
jplager3 1:6b8a201f4f90 56 //float heading; //turning to global
jplager3 1:6b8a201f4f90 57 if (my == 0.0) {
jplager3 1:6b8a201f4f90 58 mutex.lock();
jplager3 0:c29fc80c3ca3 59 heading = (mx < 0.0) ? 180.0 : 0.0;
jplager3 1:6b8a201f4f90 60 mutex.unlock();
jplager3 1:6b8a201f4f90 61 }
jplager3 1:6b8a201f4f90 62 else {
jplager3 1:6b8a201f4f90 63 mutex.lock();
jplager3 0:c29fc80c3ca3 64 heading = atan2(mx, my)*360.0/(2.0*PI);
jplager3 1:6b8a201f4f90 65 mutex.unlock();
jplager3 1:6b8a201f4f90 66 }
jplager3 0:c29fc80c3ca3 67 //pc.printf("heading atan=%f \n\r",heading);
jplager3 1:6b8a201f4f90 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 1:6b8a201f4f90 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 0:c29fc80c3ca3 78 //~pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
jplager3 0:c29fc80c3ca3 79 //~pc.printf("Magnetic Heading: %f degress\n\r",heading);
jplager3 1:6b8a201f4f90 80 dev.printf("Magnetic Heading: %f degrees\n\r",heading);
jplager3 0:c29fc80c3ca3 81 }
jplager3 0:c29fc80c3ca3 82
jplager3 0:c29fc80c3ca3 83 /*
jplager3 0:c29fc80c3ca3 84 void dev_recv()
jplager3 0:c29fc80c3ca3 85 {
jplager3 0:c29fc80c3ca3 86 led2 = !led2;
jplager3 0:c29fc80c3ca3 87 while(dev.readable()) {
jplager3 0:c29fc80c3ca3 88 pc.putc(dev.getc());
jplager3 0:c29fc80c3ca3 89 }
jplager3 0:c29fc80c3ca3 90 }
jplager3 0:c29fc80c3ca3 91 void pc_recv()
jplager3 0:c29fc80c3ca3 92 {
jplager3 0:c29fc80c3ca3 93 led4 = !led4;
jplager3 0:c29fc80c3ca3 94 while(pc.readable()) {
jplager3 0:c29fc80c3ca3 95 dev.putc(pc.getc());
jplager3 0:c29fc80c3ca3 96 }
jplager3 0:c29fc80c3ca3 97 }*/
jplager3 0:c29fc80c3ca3 98
jplager3 0:c29fc80c3ca3 99 // Driving Methods
jplager3 0:c29fc80c3ca3 100 void forward(float speed){
jplager3 0:c29fc80c3ca3 101 Left.speed(speed);
jplager3 0:c29fc80c3ca3 102 Right.speed(speed);
jplager3 0:c29fc80c3ca3 103 }
jplager3 0:c29fc80c3ca3 104 void reverse(float speed){
jplager3 0:c29fc80c3ca3 105 Left.speed(-speed);
jplager3 0:c29fc80c3ca3 106 Right.speed(-speed);
jplager3 0:c29fc80c3ca3 107 }
jplager3 0:c29fc80c3ca3 108 void turnRight(float speed){
jplager3 0:c29fc80c3ca3 109 Left.speed(speed);
jplager3 0:c29fc80c3ca3 110 Right.speed(-speed);
jplager3 0:c29fc80c3ca3 111 //wait(0.7);
jplager3 0:c29fc80c3ca3 112 }
jplager3 0:c29fc80c3ca3 113 void turnLeft(float speed){
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 0:c29fc80c3ca3 118 void stop(){
jplager3 0:c29fc80c3ca3 119 Left.speed(0.0);
jplager3 0:c29fc80c3ca3 120 Right.speed(0.0);
jplager3 0:c29fc80c3ca3 121 }
jplager3 0:c29fc80c3ca3 122
jplager3 0:c29fc80c3ca3 123 void IMU(){
jplager3 0:c29fc80c3ca3 124 //IMU setup
jplager3 0:c29fc80c3ca3 125 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); // this executes. Pins are correct. Changing them causes fault
jplager3 0:c29fc80c3ca3 126 IMU.begin();
jplager3 0:c29fc80c3ca3 127 if (!IMU.begin()) {
jplager3 0:c29fc80c3ca3 128 led2=1;
jplager3 0:c29fc80c3ca3 129 pc.printf("Failed to communicate with LSM9DS1.\n");
jplager3 0:c29fc80c3ca3 130 }
jplager3 0:c29fc80c3ca3 131 IMU.calibrate(1);
jplager3 0:c29fc80c3ca3 132 IMU.calibrateMag(0);
jplager3 1:6b8a201f4f90 133 /*
jplager3 0:c29fc80c3ca3 134 //bluetooth setup
jplager3 0:c29fc80c3ca3 135 pc.baud(9600);
jplager3 1:6b8a201f4f90 136 dev.baud(9600); */
jplager3 0:c29fc80c3ca3 137 /*pc.attach(&pc_recv, Serial::RxIrq);
jplager3 0:c29fc80c3ca3 138 dev.attach(&dev_recv, Serial::RxIrq);*/
jplager3 0:c29fc80c3ca3 139 while(1) {
jplager3 0:c29fc80c3ca3 140 //myled = 1;
jplager3 0:c29fc80c3ca3 141 while(!IMU.magAvailable(X_AXIS));
jplager3 0:c29fc80c3ca3 142 IMU.readMag();
jplager3 0:c29fc80c3ca3 143 //myled = 0;
jplager3 0:c29fc80c3ca3 144 while(!IMU.accelAvailable());
jplager3 0:c29fc80c3ca3 145 IMU.readAccel();
jplager3 0:c29fc80c3ca3 146 while(!IMU.gyroAvailable());
jplager3 0:c29fc80c3ca3 147 IMU.readGyro();
jplager3 0:c29fc80c3ca3 148 BTmutex.lock();
jplager3 0:c29fc80c3ca3 149 pc.puts(" X axis Y axis Z axis\n\r");
jplager3 1:6b8a201f4f90 150 //dev.puts(" X axis Y axis Z axis\n\r");
jplager3 0:c29fc80c3ca3 151 pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
jplager3 0:c29fc80c3ca3 152 pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
jplager3 0:c29fc80c3ca3 153 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 154 //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 155 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
jplager3 0:c29fc80c3ca3 156 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jplager3 0:c29fc80c3ca3 157 BTmutex.unlock();
jplager3 0:c29fc80c3ca3 158 myled = 1;
jplager3 0:c29fc80c3ca3 159 wait(0.5);
jplager3 0:c29fc80c3ca3 160 myled = 0;
jplager3 0:c29fc80c3ca3 161 wait(0.5);
jplager3 0:c29fc80c3ca3 162 }
jplager3 0:c29fc80c3ca3 163 }
jplager3 0:c29fc80c3ca3 164
jplager3 1:6b8a201f4f90 165 void avoidObstacle(){
jplager3 1:6b8a201f4f90 166 currIR1 = IR1; //get IR readings
jplager3 1:6b8a201f4f90 167 currIR2 = IR2;
jplager3 1:6b8a201f4f90 168 //if(currIR1 > 0.8){
jplager3 1:6b8a201f4f90 169 stop();
jplager3 1:6b8a201f4f90 170 Thread::wait(300);
jplager3 1:6b8a201f4f90 171 BTmutex.lock();
jplager3 1:6b8a201f4f90 172 //dev.printf("Collision Detected!\n\r");
jplager3 1:6b8a201f4f90 173 BTmutex.unlock();
jplager3 1:6b8a201f4f90 174 //dev.printf("Turning Left...\n\r");
jplager3 1:6b8a201f4f90 175 turnLeft(0.4); //turn 90deg
jplager3 1:6b8a201f4f90 176 Thread::wait(1000); //time to turn estimate
jplager3 1:6b8a201f4f90 177 /*
jplager3 1:6b8a201f4f90 178 while(IR2 < 0.7){ //turn left until IR2 detects an object.
jplager3 1:6b8a201f4f90 179 currIR2 = IR2;
jplager3 1:6b8a201f4f90 180 Thread::wait(300);
jplager3 1:6b8a201f4f90 181 } */
jplager3 1:6b8a201f4f90 182 stop();
jplager3 1:6b8a201f4f90 183 Thread::wait(1000);
jplager3 1:6b8a201f4f90 184 // turn should be complete. Drive until obstacle passed on right, then turn right again
jplager3 1:6b8a201f4f90 185 BTmutex.lock();
jplager3 1:6b8a201f4f90 186 //dev.printf("Driving past obstacle.\n\r");
jplager3 1:6b8a201f4f90 187 BTmutex.unlock();
jplager3 1:6b8a201f4f90 188 forward(throttle);
jplager3 1:6b8a201f4f90 189 bool objOnRight = true;
jplager3 1:6b8a201f4f90 190 while(objOnRight){
jplager3 1:6b8a201f4f90 191 currIR1 = IR1;
jplager3 1:6b8a201f4f90 192 currIR2 = IR2;
jplager3 1:6b8a201f4f90 193 if(currIR2 < 0.7){ objOnRight = false;} //if IR2 drops below threshold, obstacle passed. Break out of loop
jplager3 0:c29fc80c3ca3 194 Thread::wait(250);
jplager3 1:6b8a201f4f90 195 }
jplager3 1:6b8a201f4f90 196 stop();
jplager3 1:6b8a201f4f90 197 Thread::wait(250);
jplager3 1:6b8a201f4f90 198 BTmutex.lock();
jplager3 1:6b8a201f4f90 199 //dev.printf("Object passed. Turning right...\n\r");
jplager3 1:6b8a201f4f90 200 turnRight(0.4); // turn 90deg
jplager3 1:6b8a201f4f90 201 Thread::wait(1000); //time to turn estimate
jplager3 1:6b8a201f4f90 202 stop();
jplager3 1:6b8a201f4f90 203 Thread::wait(1000);
jplager3 1:6b8a201f4f90 204 forward(throttle);
jplager3 0:c29fc80c3ca3 205
jplager3 1:6b8a201f4f90 206 //}
jplager3 1:6b8a201f4f90 207 }
jplager3 1:6b8a201f4f90 208
jplager3 1:6b8a201f4f90 209 void defaultDrive(){ //default behavior for robot
jplager3 1:6b8a201f4f90 210 //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 211 forward(throttle);
jplager3 1:6b8a201f4f90 212 while(1){
jplager3 1:6b8a201f4f90 213 //update current IR readings
jplager3 1:6b8a201f4f90 214 currIR1 = IR1;
jplager3 1:6b8a201f4f90 215 currIR2 = IR2;
jplager3 1:6b8a201f4f90 216 BTmutex.lock(); //prevent race conditions in BT dataoutput
jplager3 1:6b8a201f4f90 217 //dev.puts(" Front IR reading Right IR reading\n\r"); // print IR readings over BT
jplager3 1:6b8a201f4f90 218 //dev.printf(" %2f %2f\n\r", currIR1, currIR2);
jplager3 1:6b8a201f4f90 219 BTmutex.unlock();
jplager3 1:6b8a201f4f90 220 // Forward collision handling code block
jplager3 1:6b8a201f4f90 221 if(currIR1 > 0.8) { // 0.85 is threshold for collision
jplager3 1:6b8a201f4f90 222 avoidObstacle(); // steer around obstacle when detected
jplager3 0:c29fc80c3ca3 223
jplager3 1:6b8a201f4f90 224 /*PICK UP FROM HERE
jplager3 1:6b8a201f4f90 225 Implement logic to control two scenarios:
jplager3 1:6b8a201f4f90 226 1) Roomba has detected obstacle in front, but Right is clear.
jplager3 1:6b8a201f4f90 227 */
jplager3 1:6b8a201f4f90 228
jplager3 1:6b8a201f4f90 229 }
jplager3 1:6b8a201f4f90 230 Thread::wait(400); // for debug. IR polling too quick and floods output terminal
jplager3 0:c29fc80c3ca3 231 }
jplager3 0:c29fc80c3ca3 232
jplager3 0:c29fc80c3ca3 233 }
jplager3 0:c29fc80c3ca3 234
jplager3 1:6b8a201f4f90 235 void manualMode(){
jplager3 1:6b8a201f4f90 236 bool on = true;
jplager3 1:6b8a201f4f90 237 while(on){
jplager3 1:6b8a201f4f90 238 char temp = dev.getc();
jplager3 1:6b8a201f4f90 239 if(temp == 'A'){ // reset command
jplager3 1:6b8a201f4f90 240 on = false;
jplager3 1:6b8a201f4f90 241 }
jplager3 1:6b8a201f4f90 242 else if(temp=='U'){
jplager3 1:6b8a201f4f90 243 led2=led3=1;
jplager3 1:6b8a201f4f90 244 forward(throttle);
jplager3 1:6b8a201f4f90 245 wait(1);
jplager3 1:6b8a201f4f90 246 led2=led3=0;
jplager3 1:6b8a201f4f90 247 }
jplager3 1:6b8a201f4f90 248 else if(temp=='L'){ // turn left
jplager3 1:6b8a201f4f90 249 myled=led2=1; //debug
jplager3 1:6b8a201f4f90 250 stop();
jplager3 1:6b8a201f4f90 251 wait(0.3);
jplager3 1:6b8a201f4f90 252 turnLeft(0.4);
jplager3 1:6b8a201f4f90 253 wait(0.6);
jplager3 1:6b8a201f4f90 254 stop();
jplager3 1:6b8a201f4f90 255 wait(0.3);
jplager3 1:6b8a201f4f90 256 forward(throttle);
jplager3 1:6b8a201f4f90 257 myled=led2=0; //debug
jplager3 1:6b8a201f4f90 258 }
jplager3 1:6b8a201f4f90 259 else if(temp=='R'){ // turn right
jplager3 1:6b8a201f4f90 260 led3=led4=1;
jplager3 1:6b8a201f4f90 261 stop();
jplager3 1:6b8a201f4f90 262 wait(0.3);
jplager3 1:6b8a201f4f90 263 turnRight(0.4);
jplager3 1:6b8a201f4f90 264 wait(0.6);
jplager3 1:6b8a201f4f90 265 stop();
jplager3 1:6b8a201f4f90 266 wait(0.3);
jplager3 1:6b8a201f4f90 267 forward(throttle);
jplager3 1:6b8a201f4f90 268 led3=led4=0;
jplager3 1:6b8a201f4f90 269 }
jplager3 1:6b8a201f4f90 270 else if(temp=='X'){ // halt/brake command
jplager3 1:6b8a201f4f90 271 stop();
jplager3 1:6b8a201f4f90 272 }
jplager3 1:6b8a201f4f90 273 }
jplager3 1:6b8a201f4f90 274 }
jplager3 1:6b8a201f4f90 275
jplager3 0:c29fc80c3ca3 276 int main()
jplager3 0:c29fc80c3ca3 277 {
jplager3 1:6b8a201f4f90 278 //bluetooth setup
jplager3 1:6b8a201f4f90 279 pc.baud(9600);
jplager3 1:6b8a201f4f90 280 dev.baud(9600);
jplager3 1:6b8a201f4f90 281 //wait to recv start command
jplager3 1:6b8a201f4f90 282 /*
jplager3 1:6b8a201f4f90 283 for(int i=0; i<3;i++){ //temp delay for a few sec
jplager3 1:6b8a201f4f90 284 myled=led2=led3=led4=1;
jplager3 1:6b8a201f4f90 285 wait(0.5);
jplager3 1:6b8a201f4f90 286 myled=led2=led3=led4=0;
jplager3 1:6b8a201f4f90 287 wait(0.5);
jplager3 1:6b8a201f4f90 288 } */
jplager3 0:c29fc80c3ca3 289 thread1.start(IMU); // start the IMU thread
jplager3 1:6b8a201f4f90 290 char temp;
jplager3 0:c29fc80c3ca3 291
jplager3 1:6b8a201f4f90 292 while(1){ //robot will receive a char from GUI signalling time to start
jplager3 1:6b8a201f4f90 293 temp = dev.getc();
jplager3 1:6b8a201f4f90 294 led3=1;
jplager3 1:6b8a201f4f90 295 pc.putc(temp);
jplager3 1:6b8a201f4f90 296 if (temp == 'B'){
jplager3 1:6b8a201f4f90 297 break;
jplager3 1:6b8a201f4f90 298 }
jplager3 1:6b8a201f4f90 299
jplager3 1:6b8a201f4f90 300 if(led2 == 0) led2 = 1;
jplager3 1:6b8a201f4f90 301 else {led2 = 0;}
jplager3 1:6b8a201f4f90 302 wait(0.25);
jplager3 1:6b8a201f4f90 303 }
jplager3 1:6b8a201f4f90 304 led3=0;
jplager3 1:6b8a201f4f90 305 thread2.start(defaultDrive);
jplager3 0:c29fc80c3ca3 306 while(1){
jplager3 1:6b8a201f4f90 307 temp = dev.getc();
jplager3 1:6b8a201f4f90 308 if(temp == 'M'){
jplager3 1:6b8a201f4f90 309 led4=1;
jplager3 1:6b8a201f4f90 310 thread2.terminate(); //stop default drive
jplager3 1:6b8a201f4f90 311 manualMode(); //switch to manual control
jplager3 1:6b8a201f4f90 312 //once manualMode is exited, return to default
jplager3 1:6b8a201f4f90 313 led4=0;
jplager3 1:6b8a201f4f90 314 thread2.start(defaultDrive);
jplager3 1:6b8a201f4f90 315 }
jplager3 0:c29fc80c3ca3 316 }
jplager3 0:c29fc80c3ca3 317
jplager3 0:c29fc80c3ca3 318 }
jplager3 0:c29fc80c3ca3 319