Diff: main/main.cpp

Revision:

12:d80399686f32

Parent:

11:c3299aca7d8f

Child:

13:bd271266e161

--- a/main/main.cpp	Mon Feb 24 05:36:03 2020 +0000
+++ b/main/main.cpp	Mon Feb 24 06:07:03 2020 +0000
@@ -60,7 +60,7 @@
 {
     init();
     calibrate();
-    speed = (pot_S*0.3)+0.2;   // have it so max is 0.5 and min is 0.2
+    speed = (pot_S*0.3)+0.2;   // have it so max is 0.5 and min is 0.2 (this lowest doesnt work)
     
     float dt = 1/50;           // updating 50 times a second
     
@@ -151,7 +151,7 @@
     if (sensor[0] > sens_thresh || sensor[4] > sens_thresh) {
         if (sensor[0] > sens_thresh) {
             left = true;
-            while ( sensor[0] > sens_thresh && (sensor[1] > sens_thresh && sensor[2] > sens_thresh && sensor[3] > sens_thresh) ) {
+            while ( sensor[0] > sens_thresh && (sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh) ) {
                 robot.forward(speed);
                 robot.scan();
                 if (sensor[4] > sens_thresh) {
@@ -161,8 +161,10 @@
             
             robot.scan();
             
-            if ( sensor[0] > sens_thresh && (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) ) {
-                if ( sensor[0] > sens_thresh && (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) ) {
+            if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
+                wait(0.05);
+                robot.scan();
+                if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
                     goal = true;
                 }
             }
@@ -176,7 +178,7 @@
         
         else if (sensor[4] > sens_thresh) {
             right = true;
-            while ( sensor[4] > sens_thresh && (sensor[1] > sens_thresh && sensor[2] > sens_thresh && sensor[3] > sens_thresh) ) {
+            while ( sensor[4] > sens_thresh && (sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh) ) {
                 robot.forward(speed);
                 robot.scan();
                 if (sensor[0] > sens_thresh) {
@@ -186,6 +188,16 @@
             
             robot.scan();
             
+            if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
+                wait(0.05);
+                robot.scan();
+                if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
+                    goal = true;
+                }
+            }
+            
+            robot.scan();
+            
             if ( sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh ) {
                 straight = true;
             }
@@ -307,8 +319,9 @@
     
     while(1) {
         int pointer = 0;   //path_length - 1
+        
         robot.stop();
-        while (button_enter.read() == 1) {}  // keep looping waiting for Enter to be pressed
+        while (button_enter.read() == 1) { speed = (pot_S*0.3)+0.2; }  // keep looping waiting for Enter to be pressed (can change speed)
         
         leds = 0b1001;
         wait(0.2);
@@ -319,9 +332,6 @@
         leds = 0b0110;
         wait(0.2);
         
-        //robot.reverse(speed);
-    //    back();
-        
         while(pointer <= path_length) {
             follow_line();
             
@@ -338,13 +348,9 @@
         
         robot.stop();
         while(button_enter.read() == 1) {
-            leds = 0b1000;
-            wait(0.2);
-            leds = 0b0100;
+            leds = 0b1001;
             wait(0.2);
-            leds = 0b0010;
-            wait(0.2);
-            leds = 0b0001;
+            leds = 0b0110;
             wait(0.2);
         }
     }
@@ -354,7 +360,7 @@
 {
     // only call once then can use infinitely
     for( int i = 0; i <= path_length; i++ ){
-        inv_path[i] = path[path_length-i];
+        inv_path[i] = path[path_length-1-i];
         if( inv_path[i] == 'L' ) { inv_path[i] = 'R'; }
         if( inv_path[i] == 'R' ) { inv_path[i] = 'L'; }
     }