Diff: main.cpp

Revision:

26:857158816262

Parent:

25:17e9e410795c

Child:

27:a19b4916169b

--- a/main.cpp	Wed Dec 05 12:34:41 2018 +0000
+++ b/main.cpp	Wed Dec 05 13:06:33 2018 +0000
@@ -1,10 +1,12 @@
 #include "mbed.h"
 #include "m3pi.h"
 
-/*TEST PARAMS SET CURRENTLY
- * Battery voltage: 4.3
- * Charging time: 60 seconds
+/*TEST PARAMS SET CURRENTLY - MUST CHANGE FOR FINAL
+ * Battery voltage: 4.6
+ * Charging time: 20 seconds
+ * "Fake charging" 1 Wh per second
  * Shows the battery voltage at each loop of the infinite loop
+ * 
  */
 
 m3pi m3pi; //declare a new car object
@@ -13,9 +15,7 @@
 #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.3  //measured voltage where charging is required
-#define CHARGE_TIME 60 //in seconds. 1200 seconds = 20 minutes
+//Calibrated parameters for sensors
 #define CROSSROAD_REFL_VAL 350 //sensor reading on PC0 and PC4 at crossroad
 
 //Pit control
@@ -24,9 +24,16 @@
 #define SPEED_INSIDE_PIT 0.2  //speed to drive inside the pit
 #define PIT_TRANSPORT_TIME 1  //seconds to drive pit stretch at internal pit spd
 
+//Charging data
+#define LOW_BATTERY_VOLTAGE 4.6  //measured voltage where charging is required
+#define CHARGE_TIME 20 //in seconds. 1200 seconds = 20 minutes
+#define CHARGE_AMPS 1.0 //constant amps being delivered through charger
+#define CHARGE_DELTA_T 1.0 //seconds polls voltage and integrates in 1 second steps
+
 //Car sensors and LEDs
 #define PC0 0 //location of PC0 reflectance sensor
 #define PC4 4 //location of PC4 reflectance sensor
+#define NUM_REFL_SENSORS 5 //number of reflectance sensors
 #define CALIBRATED_SENS_VALS 0x87 //command to 3pi to get calibrated values
 
 //Various motor speeds
@@ -52,214 +59,29 @@
     int pitstops;
 } performance_data;  
 
-
-//Prototype
+//Prototypes: Storing and getting file data
+performance_data read_from_file();
 void write_to_file(performance_data data);
 
-
-//Function to write car performance data to persistent file
-void write_to_file(performance_data data) {
-
-    //Define file system
-    LocalFileSystem local(FILESYS);
-    
-    //Open a file (or create a new) with write access
-    //"w": write "r":read "a":append (add @end)
-    //"w": will overwrite any existing data, so we just store one line.
-    FILE *fp = fopen(FILENAME, "w");
-    
-    //write performance data to the file
-    fprintf(fp, "%f %d", data.wh, data.pitstops);
-    
-    //close the file and release the memory when done.
-    fclose(fp);  
-
-}
-
-//Function to read car performance data from persistent file
-performance_data read_from_file() {
+//Prototypes: Sensing
+bool battery_low();
+void sensor_all(int arr[]);
+bool car_at_pit_crossroad(int sensor_array[]);
 
-    //Declare the variable which will ultimately be returned
-    performance_data data;
-    
-    //Define file system on mbed LPC1768
-    LocalFileSystem local(FILESYS);
-    
-    //Try to open the file as read (it might not exist)
-    FILE *fp = fopen(FILENAME, "r");
-    
-    //Test whether the file exists
-    if (fp != NULL) {
-        
-        //file exists, so read from it
-        
-        //Read stored data from the file and put in struct
-        // &(struct.member) and &struct.member yield same address
-        fscanf(fp, "%f %d", &(data.wh), &(data.pitstops) );
-    
-        //close file and release mem.
-        fclose(fp);  
-        
-    } else {
-        
-        //file doesn't exist, so just return zeros (no performance yet)
-        data.wh = 0.0;
-        data.pitstops = 0;
-    }   
-    
-    //return the data object to the caller
-    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() {
+//Prototypes: Controlling
+void turn_left(void);
+void turn_right(void);
+void pit_navigate(int direction);
 
-    //Get battery level from the 3pi robot
-    float voltage = m3pi.battery();
-    
-    //return battery state result
-    return (voltage <= LOW_BATTERY_VOLTAGE) ? true : false;
-}
-
-
-//Get the values from 3pi read from reflectance sensors PC0-PC4
-//The function returns in the array that is provided as function argument
-void sensor_all(int arr[]) {
-    
-    m3pi.putc(CALIBRATED_SENS_VALS); // request 3pi for calibrated values
+//Prototypes: Charging
+float charge(int charge_time);
 
-    //Pick up the received data
-    for(int n = PC0; n <= PC4; n++) { // loop through all sensors
-        char lowbyte = m3pi.getc(); //LSB: we receive the data "little endian"
-        char hibyte  = m3pi.getc(); //get the MSB
-        arr[n] = ((lowbyte + (hibyte << 8))); // make 2 byte int (8 bits / byte)
-    }
-}
-
-//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;
-    }
-}
+//Prototypes: Displaying
+void show_voltage();
+void show_car_data(performance_data data);
 
 
-//Turn the car 90 degrees left
-void turn_left (void)
-{
-    m3pi.left(TURN_SPEED);
-    wait(TURN_TIME);
-}
-
-//Turn the car 90 degrees right    
-void turn_right (void)
-{
-    m3pi.right (TURN_SPEED);
-    wait(TURN_TIME);
-}
-
-
-//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();
-    }
-}
-
-//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++) {
-        
-        show_voltage();
-        
-        // PERFORM SOME WEIRD CALCULATION HERE
-        
-        //For now just put in a dummy number
-        wh_this_session += 1;
-        
-        wait(1.0); //wait one second
-    
-    } //end for
-    
-    //return the amount of Wh charged
-    return wh_this_session;
-}
-
-//Show the battery voltage in the display
-void show_voltage() {
-    m3pi.printf("%07.2fV", m3pi.battery() );
-}
-
-
-//display values from the performance data struct in the display
-void show_car_data(performance_data data) {
- 
-     /* Format:
-            xxx.xxWh
-            0000xxPs
-    
-    Formats are specified according to guesstimates with the following calcs
-     Wh = u * i * t = 6 V * 0.5 A * 1/3h * 20 times = 20 Wh
-     Bilforbrug: ca. 800 mA -> over 20 timer -> 16000 mAh -> 16 Ah
-     16 Ah * 5 V = 80 Wh.
-     -> go for something between 20 - 80 Wh, with two decimals
-    */    
-
-    
-    //clear screen
-    m3pi.cls();
-    
-    //print Wh in first line
-    m3pi.locate(0,0);
-    m3pi.printf("%06.2fWh", data.wh); //6 wide in total, 2 after comma, pad with zero
-    
-    //print Pitstops in second line
-    m3pi.locate(0,1);
-    m3pi.printf("%06dPs", data.pitstops); //6 wide in total, padded with zeros
-    
-}
-
-
-int main() {
+int main(void) {
     
     /* Define all needed variables in this section */
     performance_data car_data; //Saves the car's Wh and pitstops data
@@ -272,7 +94,7 @@
     float derivative,proportional,integral = 0; //for PID
     float power; //differential speed
     float speed = OPTIMAL_SPEED; //our optimal speed setting
-    int sensor_values[5]; //array for reflectance sensor values
+    int sensor_values[NUM_REFL_SENSORS]; //array for reflectance sensor values
     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
     bool button_pressed_flag = false; //goes true when the button has been prs
@@ -307,7 +129,7 @@
     while (1) {
       
       //TEST ONLY
-      show_voltage();
+      //show_voltage();
       
       /* Check sensors and update sensor-based flags
        * - might not be every loop: wrap this in a timer of sorts */
@@ -450,4 +272,207 @@
         m3pi.right_motor(right);
 
     } //end while
-} //end main
\ No newline at end of file
+} //end main
+
+//Function to write car performance data to persistent file
+void write_to_file(performance_data data) {
+
+    //Define file system
+    LocalFileSystem local(FILESYS);
+    
+    //Open a file (or create a new) with write access
+    //"w": write "r":read "a":append (add @end)
+    //"w": will overwrite any existing data, so we just store one line.
+    FILE *fp = fopen(FILENAME, "w");
+    
+    //write performance data to the file
+    fprintf(fp, "%f %d", data.wh, data.pitstops);
+    
+    //close the file and release the memory when done.
+    fclose(fp);  
+
+}
+
+//Function to read car performance data from persistent file
+performance_data read_from_file() {
+
+    //Declare the variable which will ultimately be returned
+    performance_data data;
+    
+    //Define file system on mbed LPC1768
+    LocalFileSystem local(FILESYS);
+    
+    //Try to open the file as read (it might not exist)
+    FILE *fp = fopen(FILENAME, "r");
+    
+    //Test whether the file exists
+    if (fp != NULL) {
+        
+        //file exists, so read from it
+        
+        //Read stored data from the file and put in struct
+        // &(struct.member) and &struct.member yield same address
+        fscanf(fp, "%f %d", &(data.wh), &(data.pitstops) );
+    
+        //close file and release mem.
+        fclose(fp);  
+        
+    } else {
+        
+        //file doesn't exist, so just return zeros (no performance yet)
+        data.wh = 0.0;
+        data.pitstops = 0;
+    }   
+    
+    //return the data object to the caller
+    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() {
+
+    //Get battery level from the 3pi robot
+    float voltage = m3pi.battery();
+    
+    //return battery state result
+    return (voltage <= LOW_BATTERY_VOLTAGE) ? true : false;
+}
+
+
+//Get the values from 3pi read from reflectance sensors PC0-PC4
+//The function returns in the array that is provided as function argument
+void sensor_all(int arr[]) {
+    
+    m3pi.putc(CALIBRATED_SENS_VALS); // request 3pi for calibrated values
+
+    //Pick up the received data
+    for(int n = PC0; n < NUM_REFL_SENSORS; n++) { // loop through all sensors
+        char lowbyte = m3pi.getc(); //LSB: we receive the data "little endian"
+        char hibyte  = m3pi.getc(); //get the MSB
+        arr[n] = ((lowbyte + (hibyte << 8))); // make 2 byte int (8 bits / byte)
+    }
+}
+
+//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;
+    }
+}
+
+//Turn the car 90 degrees left
+void turn_left(void) {
+    m3pi.left(TURN_SPEED);
+    wait(TURN_TIME);
+}
+
+//Turn the car 90 degrees right    
+void turn_right(void) {
+    m3pi.right (TURN_SPEED);
+    wait(TURN_TIME);
+}
+
+
+//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();
+    }
+}
+
+//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;
+    float timestep = CHARGE_DELTA_T; //length of time between polls and integral
+    int timesteps = (int) (charge_time / timestep);
+    
+    for (int i = 0; i < timesteps; i++) {
+        
+        show_voltage();
+        
+        // PERFORM SOME WEIRD CALCULATION HERE
+        
+        //For now just put in a dummy number
+        wh_this_session += timestep;
+        
+        wait(timestep); //wait one second
+    
+    } //end for
+    
+    //return the amount of Wh charged
+    return wh_this_session;
+}
+
+
+//Show the battery voltage in the display
+void show_voltage() {
+    m3pi.printf("%07.2fV", m3pi.battery() );
+}
+
+
+//display values from the performance data struct in the display
+void show_car_data(performance_data data) {
+ 
+     /* Format:
+            xxx.xxWh
+            0000xxPs
+    
+    Formats are specified according to guesstimates with the following calcs
+     Wh = u * i * t = 6 V * 0.5 A * 1/3h * 20 times = 20 Wh
+     Bilforbrug: ca. 800 mA -> over 20 timer -> 16000 mAh -> 16 Ah
+     16 Ah * 5 V = 80 Wh.
+     -> go for something between 20 - 80 Wh, with two decimals
+    */    
+
+    
+    //clear screen
+    m3pi.cls();
+    
+    //print Wh in first line
+    m3pi.locate(0,0);
+    m3pi.printf("%06.2fWh", data.wh); //6 wide in total, 2 after comma, pad with zero
+    
+    //print Pitstops in second line
+    m3pi.locate(0,1);
+    m3pi.printf("%06dPs", data.pitstops); //6 wide in total, padded with zeros
+    
+}