Diff: main.cpp

Revision:

22:fe373ba68df3

Parent:

21:6dce9000da11

Child:

23:e1e6411e5221

--- a/main.cpp	Tue Dec 04 15:47:02 2018 +0000
+++ b/main.cpp	Tue Dec 04 16:32:40 2018 +0000
@@ -1,31 +1,33 @@
 #include "mbed.h"
 #include "m3pi.h"
 
-m3pi m3pi;
+m3pi m3pi; //declare a new car object
 
-//File name
-#define FILESYS "robot"
-#define FILENAME "/robot/data.txt"
+//Filename settings
+#define FILESYS "robot"  //name of the virtual file system on the LPC1768
+#define FILENAME "/robot/data.txt"  //filename to write backups to
 
 //Team calibrated parameters for car and battery
-#define LOW_BATTERY_VOLTAGE 4.2
+#define LOW_BATTERY_VOLTAGE 4.2  //measured voltage where charging is required
+#define CHARGE_TIME 20 //in seconds. 1200 seconds = 20 minutes
 #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
+#define ENTER_PIT 1  //direction constant to mark driven forward out of pit
+#define LEAVE_PIT -1  //direction constant to mark backing out of pit
+#define SPEED_INSIDE_PIT 0.2  //speed to drive inside the pit
+#define PIT_TRANSPORT_TIME 1  //seconds to drive pit stretch at interal pit spd
 
 //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
+//Various motor speeds
 #define MAX 1.0
 #define MIN 0
-#define OPTIMAL_SPEED 0.8
-#define PIT_SPEED 0.3
+#define OPTIMAL_SPEED 0.8  //Current targeted optimal speed
+#define PIT_SPEED 0.3  //Speed on a pit lap to avoid overshooting the pit
+#define DECELLERATION 0.1 //Rate of decelleration to avoid bumping
 
 // PID terms
 #define P_TERM 1
@@ -183,6 +185,28 @@
     }
 }
 
+//TO DO: IMPLEMENT A WH CALCULATION
+/* Begin the charging by calling when parked in pit
+ * Charges for a called number of seconds (int charge_time)
+ * Return the amount of Wh charged during each charging session
+ * Function keeps control as long as charging 
+ */
+float charge(int charge_time) {
+    
+    float wh_this_session = 0;
+    
+    for (int i = 0; i < charge_time; i++) {
+        
+        // PERFORM SOME WEIRD CALCULATION HERE
+        wait(1.0);
+    
+    } //end for
+    
+    //return the amount of Wh charged
+    return wh_this_session;
+}
+
+
 int maze (void)
 {
     
@@ -195,6 +219,7 @@
     /* Define all needed variables in this section */
     performance_data car_data; //Saves the car's Wh and pitstops data
 
+    int laps = 0; //experimantal lap counter
     float right; //right motor speed
     float left; //left motor speed
     float current_pos_of_line = 0.0; //for PID
@@ -203,7 +228,9 @@
     float power; //differential speed
     float speed = OPTIMAL_SPEED; //our optimal speed setting
     int sensor_values[5]; //array for reflectance sensor values
-    bool battery_low_flag = false;
+    bool battery_low_flag = false; //goes true when battery needs charging
+    bool pit_crossroad_flag = false; //goes true when car is at the pit crossrd
+    
 
     /* When the car starts, read previous progress
      * if only connected to USB, do not perform this restore process 
@@ -226,10 +253,11 @@
     //Begin main event loop    
     while (1) {
       
-      /* Check sensors 
+      /* Check sensors and update sensor-based flags
        * - might not be every loop: wrap this in a timer of sorts */
       battery_low_flag = battery_low();
-      
+
+      pit_crossroad_flag = car_at_pit_crossroad(sensor_values); //anden frekvens?
       
       /*
       switch
@@ -239,17 +267,37 @@
           
           //if low_batt: decellerate car (slow down) and continue to pit
           if ( battery_low_flag ) {
-              speed = (speed > PIT_SPEED) ? speed-0.1 : PIT_SPEED;
+              speed = (speed > PIT_SPEED) ? speed-DECELLERATION : PIT_SPEED;
+          }
+          
+          //if @pitcrossrd, not going to charge: increment internal lap counter
+          if ( pit_crossroad_flag ) {
+              laps++;  //might count multiple times if the loop is too fast
           }
           
-          /*
-          if atPitIntersection: increment internal lap counter
-          */
-          
           //if low_batt && atPitIntersection: Stop && execute pit navigation program
-          if ( battery_low_flag && car_at_pit_crossroad(sensor_values) ) {
+          if ( battery_low_flag && pit_crossroad_flag ) {
               //We are at the pit, and the battery is low
               
+              //Enter the pit
+              pit_navigate(ENTER_PIT);
+              
+              //Call charging function
+              //When done, it will return the amount of Wh charged
+              car_data.wh += charge(CHARGE_TIME);
+              
+              //increment the number of pitstops
+              car_data.pitstops += 1;
+              
+              //backup data to persistent file
+              write_to_file(car_data);
+              
+              //Leave the pit
+              pit_navigate(LEAVE_PIT);
+              
+              //Reset pit-related flags
+              battery_low_flag = false;
+              pit_crossroad_flag = false;
           }
           
           /*
@@ -297,7 +345,6 @@
       
       
       
-      HUSK FOR POKKER KOLLES KODESTANDARD!
       
       */
       
@@ -336,7 +383,6 @@
        // set speed 
         m3pi.left_motor(left);
         m3pi.right_motor(right);
-        
 
     } //end while
 } //end main
\ No newline at end of file