Diff: main/main.cpp

Revision:

19:4c08275cb3c9

Parent:

18:991658b628fc

Child:

20:5cf6a378801d

diff -r 991658b628fc -r 4c08275cb3c9 main/main.cpp
--- a/main/main.cpp	Thu Mar 12 22:06:17 2020 +0000
+++ b/main/main.cpp	Tue Mar 17 21:08:10 2020 +0000
@@ -49,10 +49,8 @@
 const float B = 1/10000;     // 10000
 const float C = 1.5;         // 2/3
 
-const int sens_thresh = 500;    // replace the hard coded bits
+const int sens_thresh = 500;    // >500 = black
 const int turn_speed = 0.2;
-const float pi = 3.14159265;
-const float wheel_radius = 1.5; // 1.5cm
 
 // Global Variables
 
@@ -80,61 +78,62 @@
     
     float dt = 1/50;           // updating 50 times a second
     
-    bool check = true;
+//    bool check = true;       // used for encoder testing
     
     while (1) {
-        speed = (pot_S);   // have it so max is 0.5 and min is 0.2 (this lowest doesnt work)
-        read_encoders();     
-        if (encoder[0] > 3100) { 
+        // encoder testing
+//        speed = (pot_S);   // have it so max is 0.5 and min is 0.2 (this lowest doesnt work)
+//        read_encoders();     
+//        if (encoder[0] > 1100) { 
+//            
+//            leds[1] = 1;
+//        }
+//        else { 
+//            leds[1] = 0;
+//            dist_est_1 += 1;
+//        }  // going to have to reset these dist estimates every junction (in the if (junc_detect()) statement)
+//        if (encoder[1] > 1100) { // black
+//             
+//            leds[2] = 1;
+//        }
+//        else { 
+//            leds[2] = 0;
+//            dist_est_2 += 1;
+//        } 
+//        robot.forward(speed);
+//        if (check) {
+//            char buffer[5];
+//            sprintf(buffer,"%d", dist_est_1);
+//            robot.lcd_clear();
+//            robot.lcd_print(buffer,5);
+//        } else {
+//            char buffer2[5];
+//            sprintf(buffer2,"%d", dist_est_2);
+//            robot.lcd_clear();
+//            robot.lcd_print(buffer2,5);
+//        }
+//        
+//        if (button_enter.read() == 0) {
+//            check = not check;
+//        }  // wait for enter to be pressed
+//        
+        follow_line();
+        
+        if ( junction_detect() ) {
+            char turn = junction_logic(); 
+            turn_select(turn);
             
-            leds[1] = 1;
-        }
-        else { 
-            leds[1] = 0;
-            dist_est_1 += 1;
-        }  // going to have to reset these dist estimates every junction (in the if (junc_detect()) statement)
-        if (encoder[1] > 3100) { // black
-             
-            leds[2] = 1;
+            path[path_length] = turn;
+            path_length ++;
         }
-        else { 
-            leds[2] = 0;
-            dist_est_2 += 1;
-        } 
-        robot.forward(speed);
-        if (check) {
-            char buffer[5];
-            sprintf(buffer,"%d", dist_est_1);
-            robot.lcd_clear();
-            robot.lcd_print(buffer,5);
-        } else {
-            char buffer2[5];
-            sprintf(buffer2,"%d", dist_est_2);
-            robot.lcd_clear();
-            robot.lcd_print(buffer2,5);
-        }
+
+        
+        simplify();
         
-        if (button_enter.read() == 0) {
-            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 ++;
-//        }
-//
-//        
-//        simplify();
-//        
-//        robot.lcd_clear();
-//        robot.lcd_print(path,100);
-//    
-//        //robot.display_data();
+        robot.lcd_clear();
+        robot.lcd_print(path,100);
+    
+        //robot.display_data();
         
         wait(dt);
     }
@@ -147,7 +146,7 @@
         enc_R.mode(PullUp);
         enc_L.mode(PullUp);
         
-        wait_us(100);         // Must be atleast 10us for the 10 nF capacitor to charge
+        wait_us(10);         // Must be atleast 10us for the 10 nF capacitor to charge
         enc_R.mode(PullNone);
         enc_L.mode(PullNone);   
         enc_R = 1;            // Drive the line high
@@ -155,14 +154,23 @@
         
         t_R.start();
         enc_R.input();        // Make the I/O line an input (high impedance)
-        while (enc_R == 1 || t_R.read_us() < 3000);  // replace 2000 with a hard variable
+        while (enc_R == 1 || t_R.read_us() < 1000);  // replace 1000 with a hard variable (1000 = 1ms = 1kHz sampling) (might be able to drop this further
+        // sampling time is required to be this high for times when there is no reflectance but we only care about high reflectance
+        
+        // maybe i could wait a few microseconds, see if the encoder is still high, if high then no reflectance, if low, the high reflectance
+        // this would increase sampling time
+        
+        // also, the fact that the waits are in the same loop means that the loop will run at different speeds depending on whether a sensor is triggered or not
+        // if both are triggered it will run fast, otherwise it will have to wait 1000+ us for each sensor
+        
+        // this therefore needs to be done in parallel and also must not affect the time of other operations in the main loop
         encoder[0] = t_R.read_us();   // Measure the time for the capacitor to discharge by waiting for the I/O line to go low
         t_R.stop();
         t_R.reset();
         
         t_L.start();
         enc_L.input();
-        while (enc_L == 1 || t_L.read_us() < 3000);
+        while (enc_L == 1 || t_L.read_us() < 1000);
         encoder[1] = t_L.read_us();        
         t_L.stop();
         t_L.reset();
@@ -229,9 +237,9 @@
         robot.motors(speed,speed-motor_correction);
     }
     
-    read_encoders();     
-    if (encoder[0] > 3100) { dist_est_1 += 1; }  // going to have to reset these dist estimates every junction (in the if (junc_detect()) statement)
-    if (encoder[1] > 3100) { dist_est_2 += 1; }  // might not need to actually use 2pir/3 could just add arbitrary numbers
+//    read_encoders();     
+//    if (encoder[0] > 3100) { dist_est_1 += 1; }  // going to have to reset these dist estimates every junction (in the if (junc_detect()) statement)
+//    if (encoder[1] > 3100) { dist_est_2 += 1; }  // might not need to actually use 2pir/3 could just add arbitrary numbers
 }
 
 bool junction_detect() 
@@ -315,28 +323,28 @@
 {
     leds = 0b1100;
 
-    while (sensor[0] > 500) { robot.scan(); }
+    while (sensor[0] > sens_thresh) { robot.scan(); }
     
-    robot.spin_left(0.2);
+    robot.spin_left(turn_speed);
     wait(0.1);
     
-    while (sensor[1] < 500) { robot.scan(); }
+    while (sensor[1] < sens_thresh) { robot.scan(); }
     
-    while (sensor[1] > 500) { robot.scan(); }
+    while (sensor[1] > sens_thresh) { robot.scan(); }
 }
 
 void right()
 {
     leds = 0b0011;
 
-    while (sensor[4] > 500) { robot.scan(); }
+    while (sensor[4] > sens_thresh) { robot.scan(); }
     
-    robot.spin_right(0.2);
+    robot.spin_right(turn_speed);
     wait(0.1);
     
-    while (sensor[3] < 500) { robot.scan(); }
+    while (sensor[3] < sens_thresh) { robot.scan(); }
     
-    while (sensor[3] > 500) { robot.scan(); }
+    while (sensor[3] > sens_thresh) { robot.scan(); }
 }
 
 void back() 
@@ -344,11 +352,11 @@
     leds = 0b1111;
     robot.reverse(speed);
     wait(0.1);
-    robot.spin_right(0.2);
+    robot.spin_right(turn_speed);
     
-    while (sensor[3] < 500) { robot.scan(); }
+    while (sensor[3] < sens_thresh) { robot.scan(); }
     
-    while (sensor[3] > 500) { robot.scan(); }
+    while (sensor[3] > sens_thresh) { robot.scan(); }
 }
 
 void simplify()
@@ -403,7 +411,7 @@
         while(sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
         wait(0.05);
         
-        robot.spin_right(0.2);
+        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(); }