Diff: main.cpp

Revision:

5:ab5fd9a37d7a

Parent:

4:63e69557142e

Child:

6:7123768ea0c9

--- a/main.cpp	Thu May 04 19:45:42 2017 +0000
+++ b/main.cpp	Thu May 04 20:23:10 2017 +0000
@@ -4,6 +4,7 @@
 //#include "SDFileSystem.h"
 #include "Motor.h"
 //#include "wave_player.h"
+#include "ultrasonic.h"
 
 #define PI 3.14159
 // Earth's magnetic field varies by location. Add or subtract
@@ -30,6 +31,10 @@
 // Speaker out
 //AnalogOut DACout(p18);      //must(?) be p18
 //SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
+ultrasonic mu(p29, p30, .1, 1);    //Set the trigger pin to D8 (p29) and the echo pin to D9 (p30)
+                                        //have updates every .1 seconds and a timeout after 1
+                                        //second, and call dist when the distance changes
+
 Thread thread1;
 //Thread thread2;
 //Mutex BTmutex;                  //mutex for send/recv data on Bluetooth
@@ -37,8 +42,10 @@
 
 //Globals
 float throttle = 0.5;
-float currIR1;
+//float currIR1;
 float currIR2;
+float sonar;
+float sonarThresh = 0.5;
 //float origHeading;
 //float heading;
 
@@ -188,16 +195,17 @@
     while(objOnRight) {
         mutex.lock();
         dev.printf("Avoiding Obstacles...\n\r");
-        currIR1 = IR1; //must keep scanning IR readers to know when object is cleared
+        //currIR1 = IR1; //must keep scanning IR readers to know when object is cleared
+        sonar = mu.getCurrentDistance()*0.00328084;
         currIR2 = IR2;
-        dev.printf("    IR1 Reading         IR2 Reading\n\r         %f          %f\n\r", currIR1, currIR2);
+        dev.printf("    IR1 Reading         IR2 Reading\n\r         %f          %f\n\r", sonar, 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
+        if(sonar < sonarThresh){          // don;t crash to anything in front
             stop();
             myled=led2=led3=led4=1;
         }
@@ -303,6 +311,7 @@
     //bluetooth setup
     pc.baud(9600);
     dev.baud(9600);
+    mu.startUpdates();//start measuring the distance from Sonar
     //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;
@@ -335,17 +344,19 @@
             myled=1;
             //update current IR readings
             //mutex.lock();//IR readings included in mutex since they are shared global variables
-            currIR1 = IR1;
+            //currIR1 = IR1; //replaced with sonar
+            sonar = mu.getCurrentDistance()*0.00328084;
             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);
+            //dev.printf("        %2f             %2f\n\r", currIR1, currIR2);
+            dev.printf("        %2f             %2f\n\r", sonar, currIR2); //changed
             //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
+            if(sonar < sonarThresh) {     // 0.85 is threshold for collision
                 led3=1;
                 avoidObstacle();    // steer around obstacle when detected
                 led3=0;