Diff: main.cpp

Revision:

21:6dce9000da11

Parent:

20:32e5cf8fe6dd

Child:

22:fe373ba68df3

--- a/main.cpp	Tue Dec 04 15:26:28 2018 +0000
+++ b/main.cpp	Tue Dec 04 15:47:02 2018 +0000
@@ -11,6 +11,12 @@
 #define LOW_BATTERY_VOLTAGE 4.2
 #define CROSSROAD_REFL_VAL 350 //sensor reading on PC0 and PC4 at crossroad
 
+//Pit control
+#define ENTER_PIT 1
+#define LEAVE_PIT -1
+#define SPEED_INSIDE_PIT 0.2
+#define PIT_TRANSPORT_TIME 1
+
 //Car sensors and LEDs
 #define PC0 0 //location of PC0 reflectance sensor
 #define PC4 4 //location of PC4 reflectance sensor
@@ -136,6 +142,46 @@
     }
 }
 
+//Turn the car 90 degrees left
+void turn_left (void)
+{
+    m3pi.left(0.3);
+    wait (0.22);
+}
+
+//Turn the car 90 degrees right    
+void turn_right (void)
+{
+    m3pi.right (0.3);
+    wait (0.22);
+}
+
+
+//Safely navigate the car from the pit crossroad and all the way to the charger.
+//direction = 1 to enter pit... direction = -1 to leave the pit.
+//takes control all the way from track to pit OR reverse.
+//Error handling: If a wrong code is passed, the function does nothing quietly.
+void pit_navigate(int direction) {
+    
+    //Car must enter pit forward
+    if (direction == ENTER_PIT) {
+        m3pi.stop();
+        turn_right();
+        m3pi.stop();
+        m3pi.forward(SPEED_INSIDE_PIT);
+        wait(PIT_TRANSPORT_TIME);
+        m3pi.stop();
+    }
+    
+    //Car must leave pit backward
+    if (direction == LEAVE_PIT) {   
+        m3pi.backward(SPEED_INSIDE_PIT);
+        wait(PIT_TRANSPORT_TIME);
+        m3pi.stop();
+        turn_left();
+        m3pi.stop();
+    }
+}
 
 int maze (void)
 {
@@ -143,17 +189,6 @@
     return 1;
 }   
 
-void turn_left (void)
-{
-    m3pi.left(0.3);
-    wait (0.22);
-}
-    
-void turn_right (void)
-{
-    m3pi.right (0.3);
-    wait (0.22);
-}
 
 int main() {
     
@@ -167,7 +202,7 @@
     float derivative,proportional,integral = 0; //for PID
     float power; //differential speed
     float speed = OPTIMAL_SPEED; //our optimal speed setting
-    int sensor_val[5]; //array for reflectance sensor values
+    int sensor_values[5]; //array for reflectance sensor values
     bool battery_low_flag = false;
 
     /* When the car starts, read previous progress
@@ -193,7 +228,7 @@
       
       /* Check sensors 
        * - might not be every loop: wrap this in a timer of sorts */
-      bool battery_low_flag = battery_low();
+      battery_low_flag = battery_low();
       
       
       /*
@@ -202,7 +237,7 @@
           
           */
           
-          //if low_batt: decelarate car (slow down) and continue to pit
+          //if low_batt: decellerate car (slow down) and continue to pit
           if ( battery_low_flag ) {
               speed = (speed > PIT_SPEED) ? speed-0.1 : PIT_SPEED;
           }
@@ -212,6 +247,10 @@
           */
           
           //if low_batt && atPitIntersection: Stop && execute pit navigation program
+          if ( battery_low_flag && car_at_pit_crossroad(sensor_values) ) {
+              //We are at the pit, and the battery is low
+              
+          }
           
           /*
           if all_sensors == 1000 (lifted from track): 
@@ -298,33 +337,6 @@
         m3pi.left_motor(left);
         m3pi.right_motor(right);
         
-        //Marc's sensor test
-        sensor_all(sensor_val); // sensor_value gets fkt value
-        if (sensor_val[0] > 350 && sensor_val[4] > 350)
-            {
-                 
-                m3pi.stop();
-                turn_right();
-                m3pi.stop();
-                m3pi.forward(0.2);
-                wait (1.1);
-                m3pi.stop();
-                wait (5);
-                m3pi.backward(0.2);
-                wait (1);
-                m3pi.stop();
-                turn_left();
-                m3pi.stop();
-            }
-        /*
-        sensor_value4 = sensor_all(4); // sensor_value gets fkt value
-        */
-            
-        /*
-        m3pi.printf("%d", sensor_value0); // prints one of sidesensors value
-        m3pi.locate(0,1);
-        m3pi.printf("%d", sensor_value4); // prints the other sidesensor value
-        */
 
-    }
-}
\ No newline at end of file
+    } //end while
+} //end main
\ No newline at end of file