Diff: main.cpp

Revision:

20:32e5cf8fe6dd

Parent:

19:c0d59019de53

Child:

21:6dce9000da11

--- a/main.cpp	Tue Dec 04 15:01:11 2018 +0000
+++ b/main.cpp	Tue Dec 04 15:26:28 2018 +0000
@@ -7,6 +7,14 @@
 #define FILESYS "robot"
 #define FILENAME "/robot/data.txt"
 
+//Team calibrated parameters for car and battery
+#define LOW_BATTERY_VOLTAGE 4.2
+#define CROSSROAD_REFL_VAL 350 //sensor reading on PC0 and PC4 at crossroad
+
+//Car sensors and LEDs
+#define PC0 0 //location of PC0 reflectance sensor
+#define PC4 4 //location of PC4 reflectance sensor
+
 // Minimum and maximum motor speeds
 #define MAX 1.0
 #define MIN 0
@@ -80,8 +88,15 @@
     return data;
 }
 
+//This function tests the car's battery state, and returs true if it needs
+//to charge. Otherwise false is returned.
 bool battery_low() {
-    return 0;   
+
+    //Get battery level from the 3pi robot
+    float voltage = m3pi.battery();
+    
+    //return battery state result
+    return (voltage <= LOW_BATTERY_VOLTAGE) ? true : false;
 }
 
 
@@ -101,8 +116,27 @@
         m3pi.printf("%d", arr[0]);
         m3pi.locate(0,0);
         m3pi.printf("%d", arr[4]);
-        }
- 
+}
+
+//Function to check whether car is just at the pit 
+bool car_at_pit_crossroad(int sensor_array[]) {
+    
+    //Read the sensor values
+    sensor_all(sensor_array);
+   
+    //Check the PC0 and PC4 sensors vs thresholds
+    if (sensor_array[PC0] > CROSSROAD_REFL_VAL &&
+        sensor_array[PC4] > CROSSROAD_REFL_VAL) {
+        
+        //Car is at the crossroad
+        return true;
+    } else {
+        //Car is NOT at the crossroad
+        return false;
+    }
+}
+
+
 int maze (void)
 {
     
@@ -134,7 +168,7 @@
     float power; //differential speed
     float speed = OPTIMAL_SPEED; //our optimal speed setting
     int sensor_val[5]; //array for reflectance sensor values
-
+    bool battery_low_flag = false;
 
     /* When the car starts, read previous progress
      * if only connected to USB, do not perform this restore process 
@@ -157,15 +191,29 @@
     //Begin main event loop    
     while (1) {
       
+      /* Check sensors 
+       * - might not be every loop: wrap this in a timer of sorts */
+      bool battery_low_flag = battery_low();
+      
+      
       /*
       switch
           if nothing else: line following
-          if low_batt: slow down and continue
-          if atPitIntersection: increment internal lap counter
+          
+          */
+          
+          //if low_batt: decelarate car (slow down) and continue to pit
+          if ( battery_low_flag ) {
+              speed = (speed > PIT_SPEED) ? speed-0.1 : PIT_SPEED;
+          }
           
-          if low_batt && atPitIntersection: Stop && execute pit navigation program
+          /*
+          if atPitIntersection: increment internal lap counter
+          */
           
+          //if low_batt && atPitIntersection: Stop && execute pit navigation program
           
+          /*
           if all_sensors == 1000 (lifted from track): 
                 show Wh and pitstops on display
                 stop motors, reset PID errors, wait 20 secs, recalibrate sensors