Diff: main.cpp

Revision:

3:17113c72186e

Parent:

2:8887d13f967a

Child:

4:63e69557142e

--- a/main.cpp	Wed May 03 02:19:12 2017 +0000
+++ b/main.cpp	Thu May 04 19:25:25 2017 +0000
@@ -15,7 +15,7 @@
 
 Serial pc(USBTX, USBRX);
 //RawSerial  pc(USBTX, USBRX);
-Serial dev(p28,p27);    // 
+Serial dev(p28,p27);    //
 //RawSerial  dev(p28,p27); //tx, rx
 DigitalOut myled(LED1);
 DigitalOut led2(LED2);
@@ -28,19 +28,19 @@
 Motor Left(p21, p14, p13);      // green wires. pwm, fwd, rev, add ", 1" for braking
 Motor Right(p22, p12, p11);     // red wires
 // Speaker out
-AnalogOut DACout(p18);      //must(?) be p18
+//AnalogOut DACout(p18);      //must(?) be p18
 //SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
 Thread thread1;
-Thread thread2;
-Mutex BTmutex;                  //mutex for send/recv data on Bluetooth
+//Thread thread2;
+//Mutex BTmutex;                  //mutex for send/recv data on Bluetooth
 Mutex mutex;                    //other mutex for global resources
 
 //Globals
-float throttle = 0.3;
+float throttle = 0.5;
 float currIR1;
 float currIR2;
-float origHeading;
-float heading;
+//float origHeading;
+//float heading;
 
 // Calculate pitch, roll, and heading.
 // Pitch/roll calculations taken from this app note:
@@ -48,36 +48,38 @@
 // 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
+{
+    //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;        //turning to global
+    float heading;        //was global
     if (my == 0.0) {
-        mutex.lock();
+        //mutex.lock(); //heading isn't global mutexes not needed
         heading = (mx < 0.0) ? 180.0 : 0.0;
-        mutex.unlock();
-    }
-    else {
-        mutex.lock();
+        //mutex.unlock();
+    } else {
+        //mutex.lock();
         heading = atan2(mx, my)*360.0/(2.0*PI);
-        mutex.unlock();
+        //mutex.unlock();
     }
     //pc.printf("heading atan=%f \n\r",heading);
-    mutex.lock();
+    //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();
+    //mutex.unlock();
     // Convert everything from radians to degrees:
     //heading *= 180.0 / PI;
     pitch *= 180.0 / PI;
     roll  *= 180.0 / PI;
+    mutex.lock();
     //~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); 
+    //dev.printf("Magnetic Heading: %f degrees\n\r",heading);
+    mutex.unlock();
 }
 
 /*
@@ -96,46 +98,47 @@
     }
 }*/
 
-    // Driving Methods
-void forward(float speed){
+// Driving Methods
+void forward(float speed)
+{
     Left.speed(speed);
-    Right.speed(speed);    
+    Right.speed(speed);
 }
-void reverse(float speed){
+void reverse(float speed)
+{
     Left.speed(-speed);
     Right.speed(-speed);
 }
-void turnRight(float speed){
+void turnRight(float speed)
+{
     Left.speed(speed);
     Right.speed(-speed);
     //wait(0.7);
 }
-void turnLeft(float speed){
+void turnLeft(float speed)
+{
     Left.speed(-speed);
     Right.speed(speed);
     //wait(0.7);
 }
-void stop(){
+void stop()
+{
     Left.speed(0.0);
     Right.speed(0.0);
 }
 
-void IMU(){
+void IMU()
+{
     //IMU setup
     LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);   // this executes. Pins are correct. Changing them causes fault
-    IMU.begin();           
-    if (!IMU.begin()) {    
+    IMU.begin();
+    if (!IMU.begin()) {
         led2=1;
         pc.printf("Failed to communicate with LSM9DS1.\n");
     }
     IMU.calibrate(1);
     IMU.calibrateMag(0);
-    /*
-    //bluetooth setup
-    pc.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));
@@ -145,107 +148,117 @@
         IMU.readAccel();
         while(!IMU.gyroAvailable());
         IMU.readGyro();
-        BTmutex.lock();
-        pc.puts("        X axis    Y axis    Z axis\n\r");
+        //mutex.lock(); //changed from BTmutex
+        //pc.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));
+        //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));
         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();
+        //mutex.unlock(); //changed from BTmutex
         //myled = 1;
         wait(0.5);
         //myled = 0;
-        wait(0.5);   
-    }  
+        wait(0.5);
+    }
 }
 
-void avoidObstacle(){
-    currIR1 = IR1;        //get IR readings
-    currIR2 = IR2;
-    //if(currIR1 > 0.8){
+void avoidObstacle()
+{
+    //currIR1 = IR1;        //get IR readings - already received from main thread that initially decided to call avoidObstacle()
+    //currIR2 = IR2;
     stop();
     Thread::wait(300);
-    BTmutex.lock();
+    //BTmutex.lock();
     //dev.printf("Collision Detected!\n\r");
-    BTmutex.unlock();
+    //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);
+    Thread::wait(500);
     // 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;
+    while(objOnRight) {
+        mutex.lock();
+        dev.printf("Avoiding Obstacles...\n\r");
+        currIR1 = IR1; //must keep scanning IR readers to know when object is cleared
         currIR2 = IR2;
-        if(currIR2 < 0.7){ objOnRight = false;} //if IR2 drops below threshold, obstacle passed. Break out of loop
-        Thread::wait(250);
+        dev.printf("    IR1 Reading         IR2 Reading\n\r         %f          %f\n\r", currIR1, currIR2);
+        mutex.unlock();
+        if(currIR2 < 0.7) {
+            objOnRight = false;   //if IR2 drops below threshold, obstacle passed. Break out of loop
+            
+            wait(0.5);              //give robot time to drive past object
+        }
+        if(currIR1 > 0.8){          // don;t crash to anything in front
+            stop();
+            myled=led2=led3=led4=1;
+        }
+        //Thread::wait(1250);
     }
     stop();
     Thread::wait(250);
-    BTmutex.lock();
+    //BTmutex.lock();
     //dev.printf("Object passed. Turning right...\n\r");
-    turnRight(0.4);     // turn 90deg
+    turnRight(0.5);     // turn 90deg
     Thread::wait(1000);      //time to turn estimate
     stop();
     Thread::wait(1000);
-    forward(throttle);    
-        
-    //}
+    forward(throttle);
 }
 
-void defaultDrive(){        //default behavior for robot
+/*
+void defaultDrive()         //default behavior for robot //moved to main instead of being a thread
+{
     //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){
+    while(1) {
+        myled=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.puts("      Front IR reading    Right IR reading\n\r");     // print IR readings over BT
         //dev.printf("        %2f             %2f\n\r", currIR1, currIR2);
+        pc.puts("      Front IR reading    Right IR reading\n\r");     // print IR readings over BT
+        pc.printf("        %2f             %2f\n\r", currIR1, currIR2);
+
         BTmutex.unlock();
         // Forward collision handling code block
         if(currIR1 > 0.8) {     // 0.85 is threshold for collision
+            led3=1;
             avoidObstacle();    // steer around obstacle when detected
-            
-            /*PICK UP FROM HERE
-              Implement logic to control two scenarios:
-              1) Roomba has detected obstacle in front, but Right is clear. 
-            */
-                        
-        }    
+            led3=0;
+        }
         Thread::wait(400);      // for debug. IR polling too quick and floods output terminal
+        wait(0.4);
+        myled=0;
     }
 }
+*/
 
-void manualMode(){
+/*
+void manualMode() // also moved to main
+{
     bool on = true;
     char temp;
-    while(on){
+    while(on) {
         temp = dev.getc();
-        if(temp == 'A'){  // reset command
+        if(temp == 'A') { // reset command
             on = false;
-        }
-        else if(temp=='U'){
+        } else if(temp=='U') {
             led2=led3=1;
             forward(throttle);
             wait(1);
             led2=led3=0;
-        }
-        else if(temp=='L'){  // turn left
+        } else if(temp=='L') { // turn left
             myled=led2=1;   //debug
             stop();
             wait(0.3);
@@ -255,8 +268,7 @@
             wait(0.3);
             forward(throttle);
             myled=led2=0;   //debug
-        }
-        else if(temp=='R'){  // turn right
+        } else if(temp=='R') { // turn right
             led3=led4=1;
             stop();
             wait(0.3);
@@ -266,8 +278,7 @@
             wait(0.3);
             forward(throttle);
             led3=led4=0;
-        }
-        else if(temp=='X'){  // halt/brake command
+        } else if(temp=='X') { // halt/brake command
             stop();
         }
         //myled=1;
@@ -276,21 +287,31 @@
         //wait(0.5);
     }
 }
+*/
+
+/*
+void updateIRs()
+{
+    mutex.lock();
+    currIR1 = IR1; //must keep scanning IR readers to know when object is cleared
+    currIR2 = IR2;
+    mutex.unlock();
+}*/
 
 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
+    //wait to recv start command or time delay
+    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
+    char state = 'D'; //Roomba's drive state
     char temp;
     /*
     while(1){   //robot will receive a char from GUI signalling time to start
@@ -300,26 +321,104 @@
         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;
-            stop();
-            thread2.terminate();     //stop default drive
-            manualMode();       //switch to manual control
-            //once manualMode is exited, return to default
-            led4=0;
-            thread2.start(defaultDrive);
+    //thread2.start(defaultDrive); default drive inserted into main while
+    while(1) {
+        //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(state == 'D') { //default drive
+            myled=1;
+            //update current IR readings
+            //mutex.lock();//IR readings included in mutex since they are shared global variables
+            currIR1 = IR1;
+            currIR2 = IR2;
+            mutex.lock();         //prevent race conditions in BT dataoutput //changed from BTmutex
+            dev.puts("      Front IR reading    Right IR reading\n\r");     // print IR readings over BT
+            dev.printf("        %2f             %2f\n\r", currIR1, currIR2);
+            //pc.puts("      Front IR reading    Right IR reading\n\r");     // print IR readings over serial
+            //pc.printf("        %2f             %2f\n\r", currIR1, currIR2);
+            mutex.unlock(); // changed from BTmutex
+            
+            // Forward collision handling code block
+            if(currIR1 > 0.8) {     // 0.85 is threshold for collision
+                led3=1;
+                avoidObstacle();    // steer around obstacle when detected
+                led3=0;
+            }
+            Thread::wait(400);      // for debug. IR polling too quick and floods output terminal
+            wait(0.4);
+            myled=0;
+            
+            //was already ITT
+            if (dev.readable()) {
+                mutex.lock();
+                temp = dev.getc();
+                mutex.unlock();
+            }
+            if(temp == 'M') {
+                led4=1;
+                stop();
+                //thread2.terminate();     //stop default drive
+                //manualMode();       //switch to manual control
+                /*
+                while(1) {
+                    temp = dev.getc();
+                    if(temp=='U') {
+                        led2=1;
+                    }
+                }   */
+                //once manualMode is exited, return to default
+                led4=0;
+                //thread2.start(defaultDrive);
+                state = 'M';
+            }
+        }
+
+        while(state == 'M') {
+            if (dev.readable()){
+                mutex.lock();
+                temp = dev.getc();
+                mutex.unlock();
+            }
+            if(temp == 'A') { // reset command
+                state = 'D';
+            } 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.5);
+                turnLeft(0.4);
+                wait(1);
+                stop();
+                wait(0.5);
+                forward(throttle);
+                myled=led2=0;   //debug
+            } else if(temp=='R') { // turn right
+                led3=led4=1;
+                stop();
+                wait(0.3);
+                turnRight(0.5);
+                wait(0.6);
+                stop();
+                wait(0.3);
+                forward(throttle);
+                led3=led4=0;
+            } else if(temp=='X') { // halt/brake command
+                stop();
+            }
+            //myled=1;
+            //wait(0.5);
+            //myled=0;
+            //wait(0.5);
         }
     }
-    
-}
-
+}
\ No newline at end of file