Revision 5:ab5fd9a37d7a, committed 2017-05-04

Comitter:

CRaslawski

Date:

Thu May 04 20:23:10 2017 +0000

Parent:

4:63e69557142e

Child:

6:7123768ea0c9

Commit message:

replace front IR sensor with more reliable sonar. All instances of currIR1 are now replaced by "sonar". "sonar" is in feet. Change global variable "sonarThresh" to control the range that the Roomba reacts to

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HC_SR04_Ultrasonic_Library.lib	Thu May 04 20:23:10 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/ejteb/code/HC_SR04_Ultrasonic_Library/#e0f9c9fb4cf3

--- 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;