Diff: main.cpp

Revision:

1:6b8a201f4f90

Parent:

0:c29fc80c3ca3

Child:

2:8887d13f967a

--- a/main.cpp	Tue May 02 19:22:12 2017 +0000
+++ b/main.cpp	Wed May 03 01:53:52 2017 +0000
@@ -19,6 +19,7 @@
 //RawSerial  dev(p28,p27); //tx, rx
 DigitalOut myled(LED1);
 DigitalOut led2(LED2);
+DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 //IR sensors on p19(front) & p20 (right)
 AnalogIn IR1(p19);
@@ -29,13 +30,17 @@
 // Speaker out
 AnalogOut DACout(p18);      //must(?) be p18
 //SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
-
-
 Thread thread1;
 Thread thread2;
-Mutex BTmutex;
-Mutex mutex;
+Mutex BTmutex;                  //mutex for send/recv data on Bluetooth
+Mutex mutex;                    //other mutex for global resources
 
+//Globals
+float throttle = 0.3;
+float currIR1;
+float currIR2;
+float origHeading;
+float heading;
 
 // Calculate pitch, roll, and heading.
 // Pitch/roll calculations taken from this app note:
@@ -43,27 +48,36 @@
 // Heading calculations taken from this app note:
 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
-{
+{   //entire subroutine is BTmutexed already
     float roll = atan2(ay, az);
     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
     // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
     mx = -mx;
-    float heading;
-    if (my == 0.0)
+    //float heading;        //turning to global
+    if (my == 0.0) {
+        mutex.lock();
         heading = (mx < 0.0) ? 180.0 : 0.0;
-    else
+        mutex.unlock();
+    }
+    else {
+        mutex.lock();
         heading = atan2(mx, my)*360.0/(2.0*PI);
+        mutex.unlock();
+    }
     //pc.printf("heading atan=%f \n\r",heading);
+    mutex.lock();
     heading -= DECLINATION; //correct for geo location
     if(heading>180.0) heading = heading - 360.0;
     else if(heading<-180.0) heading = 360.0 + heading;
     else if(heading<0.0) heading = 360.0  + heading;
+    mutex.unlock();
     // Convert everything from radians to degrees:
     //heading *= 180.0 / PI;
     pitch *= 180.0 / PI;
     roll  *= 180.0 / PI;
     //~pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
     //~pc.printf("Magnetic Heading: %f degress\n\r",heading);
+    dev.printf("Magnetic Heading: %f degrees\n\r",heading); 
 }
 
 /*
@@ -74,7 +88,6 @@
         pc.putc(dev.getc());
     }
 }
-
 void pc_recv()
 {
     led4 = !led4;
@@ -117,14 +130,12 @@
     }
     IMU.calibrate(1);
     IMU.calibrateMag(0);
-    
+    /*
     //bluetooth setup
     pc.baud(9600);
-    dev.baud(9600);
-
+    dev.baud(9600); */
     /*pc.attach(&pc_recv, Serial::RxIrq);
     dev.attach(&dev_recv, Serial::RxIrq);*/
-    
     while(1) {
         //myled = 1;
         while(!IMU.magAvailable(X_AXIS));
@@ -136,11 +147,11 @@
         IMU.readGyro();
         BTmutex.lock();
         pc.puts("        X axis    Y axis    Z axis\n\r");
-        dev.puts("        X axis    Y axis    Z axis\n\r");
+        //dev.puts("        X axis    Y axis    Z axis\n\r");
         pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
         pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
         pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
-        dev.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+        //dev.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
         printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
                       IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
         BTmutex.unlock();
@@ -151,74 +162,157 @@
     }  
 }
 
-void defaultDrive(){        //default behavior for robot
-//Drive forward until object detected. Stop, turn left, then drive until IR2 says path is clear, then turn right to go around object.
-    forward(0.2);
-    if(IR1 > 0.85) {     // this is threshold for collision
-        stop();
+void avoidObstacle(){
+    currIR1 = IR1;        //get IR readings
+    currIR2 = IR2;
+    //if(currIR1 > 0.8){
+    stop();
+    Thread::wait(300);
+    BTmutex.lock();
+    //dev.printf("Collision Detected!\n\r");
+    BTmutex.unlock();
+    //dev.printf("Turning Left...\n\r");
+    turnLeft(0.4);          //turn 90deg
+    Thread::wait(1000);      //time to turn estimate
+    /*
+    while(IR2 < 0.7){           //turn left until IR2 detects an object.
+        currIR2 = IR2;
+        Thread::wait(300);
+    }   */
+    stop();
+    Thread::wait(1000);
+    // turn should be complete. Drive until obstacle passed on right, then turn right again
+    BTmutex.lock();
+    //dev.printf("Driving past obstacle.\n\r");
+    BTmutex.unlock();
+    forward(throttle);
+    bool objOnRight = true;
+    while(objOnRight){
+        currIR1 = IR1;
+        currIR2 = IR2;
+        if(currIR2 < 0.7){ objOnRight = false;} //if IR2 drops below threshold, obstacle passed. Break out of loop
         Thread::wait(250);
-        // check if path to right is clear
-        if(IR2 < .4){
-            turnRight(0.3);
-            while(IR1>0.4){};   //turn until path in front is clear
-            stop();
-        }
-        else {
-            turnLeft(0.3);
-            while(IR1>0.4){};    //execute turn until front IR says path is clear
-                                // consider placing Thread::wait(??) within loop to account for IR polling?
-            stop();
-            //Thread::wait(250);
-        }
-        Thread::wait(250);
-        forward(0.2);
+    }
+    stop();
+    Thread::wait(250);
+    BTmutex.lock();
+    //dev.printf("Object passed. Turning right...\n\r");
+    turnRight(0.4);     // turn 90deg
+    Thread::wait(1000);      //time to turn estimate
+    stop();
+    Thread::wait(1000);
+    forward(throttle);    
         
-        /*PICK UP FROM HERE
-          Implement logic to control two scenarios:
-          1) Roomba has detected obstacle in front, but Right is clear. Has turned right and needs to continue driving
-          2) Roomba has detected obstacle, Right is blocked. Turn left & drive until Right is clear. Turn back to right (orig. Fwd heading) and continue.
-          2a) Consider more complex routing to circle around obstacle
-        */
-        
-        
-        /*
-        //while(IR2>0.5 && IR1<0.8){};     // drive until roomba has passed object.
-        while(IR1<0.8){};
-        stop();
-        Thread::wait(250);
-        //check that path in front is clear
-        if(IR1>0.8){        // if not clear, turn left again until front is clear
-            turnLeft(0.3);
-            while(IR1>0.4){}
-            stop();
-            Thread::wait(250);    
+    //}
+}
+
+void defaultDrive(){        //default behavior for robot
+    //Drive forward until object detected. Stop, turn left, then drive until IR2 says path is clear, then turn right to go around object.
+    forward(throttle);
+    while(1){
+        //update current IR readings
+        currIR1 = IR1;
+        currIR2 = IR2;
+        BTmutex.lock();         //prevent race conditions in BT dataoutput
+        //dev.puts("      Front IR reading    Right IR reading\n\r");     // print IR readings over BT 
+        //dev.printf("        %2f             %2f\n\r", currIR1, currIR2);
+        BTmutex.unlock();
+        // Forward collision handling code block
+        if(currIR1 > 0.8) {     // 0.85 is threshold for collision
+            avoidObstacle();    // steer around obstacle when detected
             
-        }
-        else {
-            
-        }
-                
-        
-        Thread::wait(200);
-        
-        while(IR2>0.85 ) forward(0.3);   // drive until 
-        */
-        
-            
-        
+            /*PICK UP FROM HERE
+              Implement logic to control two scenarios:
+              1) Roomba has detected obstacle in front, but Right is clear. 
+            */
+                        
+        }    
+        Thread::wait(400);      // for debug. IR polling too quick and floods output terminal
     }
     
 }
 
+void manualMode(){
+    bool on = true;
+    while(on){
+        char temp = dev.getc();
+        if(temp == 'A'){  // reset command
+            on = false;
+        }
+        else if(temp=='U'){
+            led2=led3=1;
+            forward(throttle);
+            wait(1);
+            led2=led3=0;
+        }
+        else if(temp=='L'){  // turn left
+            myled=led2=1;   //debug
+            stop();
+            wait(0.3);
+            turnLeft(0.4);
+            wait(0.6);
+            stop();
+            wait(0.3);
+            forward(throttle);
+            myled=led2=0;   //debug
+        }
+        else if(temp=='R'){  // turn right
+            led3=led4=1;
+            stop();
+            wait(0.3);
+            turnRight(0.4);
+            wait(0.6);
+            stop();
+            wait(0.3);
+            forward(throttle);
+            led3=led4=0;
+        }
+        else if(temp=='X'){  // halt/brake command
+            stop();
+        }
+    }
+}
+
 int main()
 {
+    //bluetooth setup
+    pc.baud(9600);
+    dev.baud(9600);
+    //wait to recv start command
+    /*
+    for(int i=0; i<3;i++){          //temp delay for a few sec
+        myled=led2=led3=led4=1;
+        wait(0.5);
+        myled=led2=led3=led4=0;
+        wait(0.5);
+    }   */
     thread1.start(IMU); // start the IMU thread
-    //thread2.start(defaultDrive);
+    char temp;
     
-    forward(0.3);
-    led4=1;
+    while(1){   //robot will receive a char from GUI signalling time to start
+        temp = dev.getc();
+        led3=1;
+        pc.putc(temp);
+        if (temp == 'B'){
+            break;
+        }
+        
+        if(led2 == 0) led2 = 1;
+        else {led2 = 0;}
+        wait(0.25);
+    }   
+    led3=0;
+    thread2.start(defaultDrive);
     while(1){
-        
+        temp = dev.getc();
+        if(temp == 'M'){
+            led4=1;
+            thread2.terminate();     //stop default drive
+            manualMode();       //switch to manual control
+            //once manualMode is exited, return to default
+            led4=0;
+            thread2.start(defaultDrive);
+        }
     }
     
 }