Diff: main/main.cpp

Revision:

20:5cf6a378801d

Parent:

19:4c08275cb3c9

Child:

21:54ea75f7984f

--- a/main/main.cpp	Tue Mar 17 21:08:10 2020 +0000
+++ b/main/main.cpp	Tue Mar 24 14:20:32 2020 +0000
@@ -24,62 +24,38 @@
 DigitalInOut enc_L(p26); //connected to digital P26
 DigitalInOut enc_R(p25); //connected to digital P25
 
-// Timer
-Timer t_L;
-Timer t_R;
-
-// Prototypes
-void read_encoders();
-void init();
-void calibrate();
-void follow_line();
-bool junction_detect();
-char junction_logic();
-void turn_select( char turn );
-void left();
-void right();
-void back();
-void goal();
-void simplify();
-void invert_path();
-
-// Constants
-
-const float A = 0.5;         // 20
-const float B = 1/10000;     // 10000
-const float C = 1.5;         // 2/3
-
-const int sens_thresh = 500;    // >500 = black
-const int turn_speed = 0.2;
-
-// Global Variables
-
-char path[100];
-char inv_path[100];
-int path_length = 0;
-unsigned int *sensor;
-float speed;
-float proportional = 0.0;
-float prev_proportional = 0.0;
-float integral = 0.0;
-float derivative = 0.0;
-int encoder[2];
-int dist_est_1 = 0;
-int dist_est_2 = 0;
-
 // Main
 
 int main()
 {
     init();
+    
+    bool loop_check;
+    
+    robot.lcd_goto_xy(0,0);
+    robot.lcd_print("A=simple", 10);
+    robot.lcd_goto_xy(0,1);
+    robot.lcd_print("B=looped", 10);
+    
+    while(button_A.read() == 1 && button_B.read() == 1) {}
+    
+    if (button_B.read()) { loop_check = true; }   // non-looped
+    if (button_A.read()) { loop_check = false; }  // looped
+
+    robot.lcd_clear();
+    robot.lcd_goto_xy(0,0);
+    robot.lcd_print("  ENTER   ", 10);
+    robot.lcd_goto_xy(0,1);
+    robot.lcd_print("= start   ", 10);
+    
     calibrate();
     
+    robot.lcd_clear();
+    
     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
-    
-//    bool check = true;       // used for encoder testing
-    
+
     while (1) {
         // encoder testing
 //        speed = (pot_S);   // have it so max is 0.5 and min is 0.2 (this lowest doesnt work)
@@ -117,23 +93,11 @@
 //            check = not check;
 //        }  // wait for enter to be pressed
 //        
-        follow_line();
-        
-        if ( junction_detect() ) {
-            char turn = junction_logic(); 
-            turn_select(turn);
-            
-            path[path_length] = turn;
-            path_length ++;
+        if (loop_check == true) {
+            non_looped();
+        } else {
+            looped();
         }
-
-        
-        simplify();
-        
-        robot.lcd_clear();
-        robot.lcd_print(path,100);
-    
-        //robot.display_data();
         
         wait(dt);
     }
@@ -207,6 +171,48 @@
     
     leds = 0b0000;
 }
+
+void non_looped() 
+{
+    follow_line();
+    
+    if ( junction_detect() ) {
+        char turn = junction_logic(); 
+        turn_select(turn);
+        
+        path[path_length] = turn;
+        path_length ++;
+    }
+
+    
+    simplify();
+    
+    robot.lcd_clear();
+    robot.lcd_print(path,100);
+
+    //robot.display_data();
+}
+
+void looped()
+{
+      // follow line until reaching a junction, determine its type and coordinates
+//    T_R.start();
+//    follow_line();
+//    
+//    if ( junction_detect() ) {
+//        time = T_R;
+//        T_R.stop();
+//        if (time > ) 
+//    }
+//
+//    
+//    simplify();
+//    
+//    robot.lcd_clear();
+//    robot.lcd_print(path,100);
+
+    //robot.display_data();
+}
       
 void follow_line() 
 {
@@ -244,9 +250,9 @@
 
 bool junction_detect() 
 {
-    if ( sensor[0] > sens_thresh || sensor[4] > sens_thresh ) {
+    if ( sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH ) {
         return true;
-    } else if ( sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh ) {
+    } else if ( sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH ) {
         return true;
     } else {
         return false;
@@ -260,29 +266,29 @@
     bool right = false;
     bool goal = false;
     
-    if (sensor[0] > sens_thresh || sensor[4] > sens_thresh) {
-        while ( (sensor[0] > sens_thresh || sensor[4] > sens_thresh) && (sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh) ) {
+    if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
+        while ( (sensor[0] > SENS_THRESH || 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 ) { left = true; }
-            if ( sensor[4] > sens_thresh ) { right = true; }
+            if ( sensor[0] > SENS_THRESH ) { left = true; }
+            if ( sensor[4] > SENS_THRESH ) { right = true; }
         }
         
-        if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
+        if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
             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 ) {
+            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 ) {
+        if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
             straight = true;
         }
         
-    } else if (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) {
+    } else if (sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH) {
         return 'B';
     }
     
@@ -323,28 +329,28 @@
 {
     leds = 0b1100;
 
-    while (sensor[0] > sens_thresh) { robot.scan(); }
+    while (sensor[0] > SENS_THRESH) { robot.scan(); }
     
-    robot.spin_left(turn_speed);
+    robot.spin_left(0.2);
     wait(0.1);
     
-    while (sensor[1] < sens_thresh) { robot.scan(); }
+    while (sensor[1] < SENS_THRESH) { robot.scan(); }
     
-    while (sensor[1] > sens_thresh) { robot.scan(); }
+    while (sensor[1] > SENS_THRESH) { robot.scan(); }
 }
 
 void right()
 {
     leds = 0b0011;
 
-    while (sensor[4] > sens_thresh) { robot.scan(); }
+    while (sensor[4] > SENS_THRESH) { robot.scan(); }
     
-    robot.spin_right(turn_speed);
+    robot.spin_right(TURN_SPEED);
     wait(0.1);
     
-    while (sensor[3] < sens_thresh) { robot.scan(); }
+    while (sensor[3] < SENS_THRESH) { robot.scan(); }
     
-    while (sensor[3] > sens_thresh) { robot.scan(); }
+    while (sensor[3] > SENS_THRESH) { robot.scan(); }
 }
 
 void back() 
@@ -352,11 +358,11 @@
     leds = 0b1111;
     robot.reverse(speed);
     wait(0.1);
-    robot.spin_right(turn_speed);
+    robot.spin_right(TURN_SPEED);
     
-    while (sensor[3] < sens_thresh) { robot.scan(); }
+    while (sensor[3] < SENS_THRESH) { robot.scan(); }
     
-    while (sensor[3] > sens_thresh) { robot.scan(); }
+    while (sensor[3] > SENS_THRESH) { robot.scan(); }
 }
 
 void simplify()
@@ -394,7 +400,6 @@
     leds = 0b0000;
     
     robot.lcd_clear();
-    //robot.lcd_print(path,100);
     robot.lcd_print(inv_path,100);
     
     while(1) {
@@ -408,13 +413,13 @@
         wait(0.2);
         
         robot.reverse(speed);
-        while(sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
+        while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
         wait(0.05);
         
-        robot.spin_right(turn_speed);
-        while(sensor[2] > sens_thresh) { robot.scan(); }
-        while(sensor[3] < sens_thresh) { robot.scan(); } 
-        while(sensor[3] > sens_thresh) { robot.scan(); } 
+        robot.spin_right(TURN_SPEED);
+        while(sensor[2] > SENS_THRESH) { robot.scan(); }
+        while(sensor[3] < SENS_THRESH) { robot.scan(); } 
+        while(sensor[3] > SENS_THRESH) { robot.scan(); } 
 
         robot.stop();
         
@@ -427,7 +432,7 @@
                 if(inv_path[pointer] == 'S') {      // make this better
                     robot.forward(speed);
                     leds = 0b1010;
-                    while(sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
+                    while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
                 }
                 pointer++;
             }
@@ -436,8 +441,16 @@
         back();
         
         robot.stop();
+        robot.lcd_goto_xy(0,0);
+        robot.lcd_print("  ENTER   ", 10);
+        robot.lcd_goto_xy(0,1);
+        robot.lcd_print("=restart", 10);
+        
         while ( button_enter.read() == 1 ) { speed = (pot_S*0.3)+0.2; }  // keep looping waiting for Enter to be pressed (can change speed)
         
+        robot.lcd_clear();
+        robot.lcd_print(path,100);
+        
         pointer = 0;
         
         leds = 0b1001;
@@ -458,7 +471,7 @@
                 if(path[pointer] == 'S') {      // make this better
                     robot.forward(speed);
                     leds = 0b1010;
-                    while(sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
+                    while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
                 }
                 pointer++;
             }