Diff: main/main.cpp

Revision:

5:ae417756235a

Parent:

4:38c29dbc5953

Child:

6:c10b367747a0

--- a/main/main.cpp	Thu Feb 13 17:14:39 2020 +0000
+++ b/main/main.cpp	Sat Feb 22 00:32:42 2020 +0000
@@ -29,8 +29,10 @@
 void left();
 void right();
 void back();
+//void check_goal();
 void goal();
 void simplify();
+void return_to_start();
 
 // Constants
 
@@ -38,7 +40,7 @@
 const float B = 1/10000;     // 10000
 const float C = 1.5;         // 2/3
 
-const int sensor_threshold = 500;    // replace the hard coded bits
+const int sens_thresh = 500;    // replace the hard coded bits
 const int turn_speed = 0.2;
 
 // Global Variables
@@ -47,6 +49,7 @@
 int path_length = 0;
 unsigned int *sensor;  
 float speed;
+int goal_check;
 
 // Main
 
@@ -54,7 +57,7 @@
 {
     init();
     calibrate();
-    speed = (pot_S*0.4)+0.1;   // have it so max is 0.5 and min is 0.1
+    speed = (pot_S*0.3)+0.2;   // have it so max is 0.5 and min is 0.2
     
     float proportional = 0.0;
     float prev_proportional = 0.0;
@@ -65,6 +68,7 @@
     
     while (1) {
         robot.scan();
+    
         sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
         leds = 0b0110;
         
@@ -92,20 +96,18 @@
         
         //follow_line(speed);
         
-        char turn = junction_detect();
-        turn_selector(turn);
+        // add if juntion detected, move forward until both 0 and 4 or the middle three are low, then check the type of junction and turn accordingly (lines below)
+        if (junc_detect()) {
+            char turn = turn_logic(); // rename this function something else
+            turn_selector(turn);
+        }
         
-        if ( turn != 'S' ) {              // does need 'S', also may not need 'R' as that is not technically a junction
+        if ( turn != 'S' && turn != 'R') {              // doesnt need 'S', also may not need 'R' as that is not technically a junction
             path[path_length] = turn;
             path_length ++;
         }
         
-        if ( turn == 'G' ) {
-            //char simple_path[100];
-            //simple_path = simplify( path, path_length );
-            
-            //goal(simple_path);  // make this an infinite loop that 
-        }
+ 
         
         simplify();
         
@@ -129,7 +131,7 @@
     button_enter.mode(PullUp);
     button_back.mode(PullUp);
 
-    leds = 0b0000;
+    //leds = 0b0000;
 }
 
 void calibrate()
@@ -152,15 +154,21 @@
 
 char junction_detect() 
 {
-    // add a goal detector
-    if ( sensor[0] < 500 && sensor[1] < 500 && sensor[2] < 500 && sensor[3] < 500 && sensor[4] < 500 ) {
-        return 'B';
-    } else if ( sensor[0] > 500 ) {
+    
+    // either increase the wait in main loop 50-> 20 
+    
+    //or have it so it inches forward at a junction (if junction detected, then inch forward and decide
+    
+    if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh ) {
+        return 'G';
+    }  else if ( sensor[0] > 500 ) {
         return 'L';
     } else if ( sensor[1] > 500 || sensor[2] > 500 || sensor[3] > 500 ) {
         return 'S';
     } else if ( sensor[4] > 500 ) {
         return 'R';
+    } else if ( sensor[0] < 500 && sensor[1] < 500 && sensor[2] < 500 && sensor[3] < 500 && sensor[4] < 500 ) {
+        return 'B';
     } else {
         return 'S';
     }
@@ -169,16 +177,19 @@
 void turn_selector( char turn )
 {
     switch(turn) {
-        //case 'G':
-        //    goal();
+        case 'G':
+            goal();
         case 'L':
             left();
+            break;
         case 'S':
             break;
         case 'R':
             right();
+            break;
         case 'B':
             back();
+            break;
     }
 }
         
@@ -188,7 +199,7 @@
 
     while (sensor[0] > 500) { robot.scan(); }
     
-    if (speed <= 0.25) { wait(0.1); }
+    if (speed <= 0.20) { wait(0.1); }
     
     robot.spin_left(0.2);
     wait(0.1);
@@ -201,10 +212,10 @@
 void right()
 {
     leds = 0b0011;
-    
+
     while (sensor[4] > 500) { robot.scan(); }
     
-    if (speed <= 0.25) { wait(0.1); }
+    if (speed <= 0.20) { wait(0.1); }
     
     robot.spin_right(0.2);
     wait(0.1);
@@ -217,8 +228,8 @@
 void back() 
 {
     leds = 0b1111;
-    robot.reverse(0.2);
-    wait(0.15);
+    robot.reverse(speed);
+    wait(0.1);
     robot.spin_right(0.2);
     
     while (sensor[3] < 500) { robot.scan(); }
@@ -226,11 +237,6 @@
     while (sensor[3] > 500) { robot.scan(); }
 }
 
-void goal()
-{
-
-}
-
 void simplify()
 {
     // check if the last one was a 'B'
@@ -257,4 +263,32 @@
         
         path_length -= 2;        
     }
-}  
\ No newline at end of file
+}  
+
+/*
+void check_goal() 
+{
+    if (sensor[0] > sens_thresh && sensor[1] > sens_thresh && sensor[2] > sens_thresh && sensor[3] > sens_thresh && sensor[4] > sens_thresh) {
+        goal_check ++;
+    }
+}*/
+
+void goal()
+{
+    robot.stop();
+    while(1) {
+        leds = 0b1000;
+        wait(0.2);
+        leds = 0b0100;
+        wait(0.2);
+        leds = 0b0010;
+        wait(0.2);
+        leds = 0b0001;
+        wait(0.2);
+    }
+}
+
+void return_to_start() 
+{
+    
+}
\ No newline at end of file