Diff: main.cpp

Revision:

9:37bb5f0e2975

Parent:

8:cde09ae38be7

Child:

10:059154e51621

--- a/main.cpp	Thu Mar 16 01:28:27 2017 +0000
+++ b/main.cpp	Thu Mar 16 03:39:30 2017 +0000
@@ -63,27 +63,43 @@
     waver.play(wave_file2);
     fclose(wave_file2);  
 }
+void drive(float speed){
+    Left.speed(speed);
+    Right.speed(speed);    
+}
+void reverse(float speed){
+    Left.speed(-speed);
+    Right.speed(-speed);
+}
+void turnRight(float speed){
+    Left.speed(speed);
+    Right.speed(-speed);
+    //wait(0.7);
+}
+void turnLeft(float speed){
+    Left.speed(-speed);
+    Right.speed(speed);
+    //wait(0.7);
+}
+void stop(){
+    Left.speed(0.0);
+    Right.speed(0.0);
+}
+
 
 int main() {
-    Left.speed(0.0);
-    Right.speed(0.0);
+    Left.speed(0.0);        //attempt at forcing Left to chill on startup. ineffective.
+    Right.speed(0.0);       //Right does not have the issue. Is chill on startup
     //thread1.start(IR_thread); // read in IR data
     thread2.start(LCD_thread1);   
     //thread3.start(Motor_thread);
     thread4.start(sound_thread);
     
-    while(1){
-        //motor testing
-        //motorL.write(.8);
-        
-        for(float i=0.5;i>0.0;i-=0.1){
-            Left.speed(i);
-            Right.speed(-i);
-            wait(0.75);
-        }
-        
+    float turnDirectionSelector = 0.3;
+    drive(0.3);    // begin driving fwd    
+    while(1){        
         //IR testing
-        if(IR>0.3 && IR<0.5) {
+        if(IR>0.3 && IR<0.5) {              // scan for object
             led1=1;
             led2=led3=led4=0;
         } else if (IR>0.5 && IR< 0.7) {
@@ -93,26 +109,26 @@
             led1=led2=led3=1;
             led4=0;
         } else if(IR>0.9){ //collision threshold seems to be the same from IR=0.8 to IR=0.95
+                            // in this block, "collision" detected. Back up, execute turn, and proceed driving fwd.
             led1=led2=led3=led4=1; 
             fclose(wave_file);
             thread4.terminate();
             thread4.start(beep);
-            /* reverse motors and turn Roomba here
-             *
-             *
-             *
-             *
-             */
-            //motorL.write(.5);
-            // write reverse values to the motors for N rotations
-            // execute turn by stopping one wheel, and turning the other (trial and error to find exact rotation?)
-            // jump back to normal behavior
-            wait(5); //wait for beeping to finish
+            // reverse motors and turn Roomba here
+            stop();
+            wait(0.4);
+            reverse(0.3);
+            wait(2);
+            turnDirectionSelector*=-1;      // will alternate turning left/right upon collision
+            turnRight(turnDirectionSelector);
+            wait(1);    //2s is a bit more than 180deg @ 0.3 speed
+            stop();
+            
+            wait(2); //wait for beeping to finish
             fclose(wave_file2);
             thread4.terminate();
             thread4.start(sound_thread);
+            drive(0.4);
         } else led1=led2=led3=led4=0;
     }
-    // use mutex to lock getc(), printf(), scanf()
-    // don't unlock until you've checked that it's readable()  
 }
\ No newline at end of file