Diff: main/main.cpp

Revision:

15:6c461501d12d

Parent:

14:87052bb35211

Child:

16:96c7dc8a1119

--- a/main/main.cpp	Mon Feb 24 06:52:04 2020 +0000
+++ b/main/main.cpp	Tue Feb 25 15:53:53 2020 +0000
@@ -61,6 +61,7 @@
 {
     init();
     calibrate();
+    
     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
@@ -108,12 +109,16 @@
     leds = 0b1111;
     robot.reset_calibration();
 
-    while (button_enter.read() == 1) {}  // keep looping waiting for Enter to be pressed
+    while (button_enter.read() == 1) {}  // wait for enter to be pressed
+    
+    wait(2.0);  
     
-    wait(2.0);  // small delay to allow hands to move away etc.
+    robot.auto_calibrate();  
+    leds = 0b0000;
     
-    robot.auto_calibrate();  // run calibration routine
-    leds = 0b0000;
+    robot.stop();
+    wait(0.05);
+    robot.scan();
 }
       
 void follow_line() 
@@ -121,6 +126,8 @@
     robot.scan();
     sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
     
+    leds = 0b0110;
+    
     proportional = robot.read_line();  // returns a value between -1,1     (-1 = PC0 or further , -1 to -0.5 = PC1 (-0.5 is directly below PC1) , -0.5 to 0 = PC2 , 0 to 0.5 = PC3 , 0.5 to 1 and further = PC4)
     derivative = proportional - prev_proportional;
     integral += proportional;
@@ -171,7 +178,7 @@
         }
         
         if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
-            wait(0.1);        // maybe change or replace w something better
+            wait(0.05);        // maybe change or replace w something better
             robot.scan();
             if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
                 goal = true;
@@ -297,13 +304,51 @@
     
     robot.lcd_clear();
     robot.lcd_print(path,100);
+    //robot.lcd_print(inv_path,100);
     
     while(1) {
-        int pointer = 0;   //path_length - 1
+        int pointer = 0;
+        
+        robot.stop();
+        
+        leds = 0b1001;
+        wait(0.2);
+        leds = 0b0110;
+        wait(0.2);
+        
+        robot.reverse(speed);
+        while(sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
+        
+        wait(0.1);
+        robot.spin_right(0.2);
+        
+        while(sensor[2] > sens_thresh) { robot.scan(); }
+        while(sensor[3] < sens_thresh) { robot.scan(); } 
+        while(sensor[3] > sens_thresh) { robot.scan(); } 
+
+        robot.stop();
+        
+        while(pointer <= path_length) {
+            follow_line();
+            
+            if ( junction_detect() ) {  // if junction found
+                turn_select(inv_path[pointer]);
+                if(inv_path[pointer] == 'S') {      // make this better
+                    robot.forward(speed);
+                    leds = 0b1010;
+                    while(sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
+                }
+                pointer++;
+            }
+        }
+        
+        back();
         
         robot.stop();
         while ( button_enter.read() == 1 ) { speed = (pot_S*0.3)+0.2; }  // keep looping waiting for Enter to be pressed (can change speed)
         
+        pointer = 0;
+        
         leds = 0b1001;
         wait(0.2);
         leds = 0b0110;
@@ -326,14 +371,6 @@
                 pointer++;
             }
         }
-        
-        robot.stop();
-        while( button_enter.read() == 1 ) {
-            leds = 0b1001;
-            wait(0.2);
-            leds = 0b0110;
-            wait(0.2);
-        }
     }
 }