Diff: main/main.cpp

Revision:

17:77b8515a9568

Parent:

16:96c7dc8a1119

Child:

18:991658b628fc

--- a/main/main.cpp	Tue Feb 25 16:15:58 2020 +0000
+++ b/main/main.cpp	Wed Mar 11 11:03:59 2020 +0000
@@ -20,6 +20,9 @@
 AnalogIn pot_D(p19);
 AnalogIn pot_S(p20);
 
+// Sensors
+QTRSensorsRC encoders;
+
 // Prototypes
 void init();
 void calibrate();
@@ -49,6 +52,7 @@
 char inv_path[100];
 int path_length = 0;
 unsigned int *sensor;  
+unsigned int enc_vals[2];  
 float speed;
 float proportional = 0.0;
 float prev_proportional = 0.0;
@@ -67,7 +71,23 @@
     float dt = 1/50;           // updating 50 times a second
     
     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;
+//        }
+        
         follow_line();
         
         if ( junction_detect() ) {
@@ -93,6 +113,9 @@
 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);
@@ -114,11 +137,15 @@
     wait(2.0);  
     
     robot.auto_calibrate();  
-    leds = 0b0000;
+    
     
     robot.stop();
     wait(0.05);
     robot.scan();
+    
+    encoders.calibrate();
+    
+    leds = 0b0000;
 }
       
 void follow_line()