Diff: main/main.cpp

Revision:

18:991658b628fc

Parent:

17:77b8515a9568

Child:

19:4c08275cb3c9

--- a/main/main.cpp	Wed Mar 11 11:03:59 2020 +0000
+++ b/main/main.cpp	Thu Mar 12 22:06:17 2020 +0000
@@ -21,9 +21,15 @@
 AnalogIn pot_S(p20);
 
 // Sensors
-QTRSensorsRC encoders;
+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();
@@ -45,19 +51,23 @@
 
 const int sens_thresh = 500;    // replace the hard coded bits
 const int turn_speed = 0.2;
+const float pi = 3.14159265;
+const float wheel_radius = 1.5; // 1.5cm
 
 // Global Variables
 
 char path[100];
 char inv_path[100];
 int path_length = 0;
-unsigned int *sensor;  
-unsigned int enc_vals[2];  
+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
 
@@ -70,52 +80,97 @@
     
     float dt = 1/50;           // updating 50 times a second
     
+    bool check = true;
+    
     while (1) {
-        
-//        encoders.read(enc_vals);
-//        robot.lcd_clear();
-//        
-//        char buffer[10];
-//        sprintf(buffer,"%d",enc_vals[1]);
-//        robot.lcd_goto_xy(0,0);
-//        robot.lcd_print("buffer",10); 
-//        
-//        if (enc_vals[1] > 500) {
-//            leds = 0b0011;
-//        }else if (enc_vals[1] < 500){
-//            leds = 0b1100;
-//        }else{
-//            leds = 0b0000;
-//        }
+        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) { 
+            
+            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;
+        }
+        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);
+        }
         
-        follow_line();
+        if (button_enter.read() == 0) {
+            check = not check;
+        }  // wait for enter to be pressed
         
-        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();
+//        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();
         
         wait(dt);
     }
 }
 
+void read_encoders() 
+{
+        enc_R.output();    // Set the I/O line to an output
+        enc_L.output();
+        enc_R.mode(PullUp);
+        enc_L.mode(PullUp);
+        
+        wait_us(100);         // 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
+        enc_L = 1;
+        
+        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
+        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);
+        encoder[1] = t_L.read_us();        
+        t_L.stop();
+        t_L.reset();
+}
+
 void init()
 {
     robot.init();
-    
-    PinName pins[2] = {p25,p26};    // choose digital pins that are free (maybe steal P and I pot pins 15&16) try 25 and 26 first
-    encoders.init(pins,2);
 
     button_A.mode(PullUp);
     button_B.mode(PullUp);
@@ -138,13 +193,10 @@
     
     robot.auto_calibrate();  
     
-    
     robot.stop();
     wait(0.05);
     robot.scan();
     
-    encoders.calibrate();
-    
     leds = 0b0000;
 }
       
@@ -176,6 +228,10 @@
     } else {
         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
 }
 
 bool junction_detect()