File content as of revision 30:ad77e3ee288b:

#include "mbed.h"
#include "m3pi.h"

/*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

//Filename settings
#define FILESYS "robot"  //name of the virtual file system on the LPC1768
#define FILENAME "/robot/data.txt"  //filename to write backups to

//Calibrated parameters for sensors
#define CROSSROAD_REFL_VAL 350 //sensor reading on PC0 and PC4 at crossroad

//Pit control
#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 internal pit spd

//Charging data
#define POLL_BATT_CYCLE 5 //Poll the battery every 3 loops
#define LOW_BATTERY_VOLTAGE 4.6  //measured voltage where charging is required
#define VOLTAGE_AT_PIT 4.7 //min initial voltage across charger when car enter the pit
#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
#define MAX 1.0
#define MIN 0
#define OPTIMAL_SPEED 0.8  //Current targeted optimal speed
#define PIT_SPEED 0.5  //Speed on a pit lap to avoid overshooting the pit
#define DECELLERATION 0.1 //Rate of decelleration to avoid bumping

//Calibrated values for performing 90 degree turns
#define TURN_SPEED 0.3
#define TURN_TIME 0.22

// PID terms
#define P_TERM 1
#define D_TERM 8
#define I_TERM 0

//Forward declare our type for storing car performance date
//It can now be used as a type by the name performance_data
typedef struct pd {
    float wh;
    int pitstops;
} performance_data;  

//Prototypes: Storing and getting file data
performance_data read_from_file();
void write_to_file(performance_data data);

//Prototypes: Sensing
bool battery_low();
void sensor_all(int arr[]);
bool car_at_pit_crossroad(int sensor_array[]);

//Prototypes: Controlling
void turn_left(void);
void turn_right(void);
void pit_navigate(int direction);

//Prototypes: Charging
float charge(int charge_time);

//Prototypes: Displaying
void show_voltage(char* message);
void show_car_data(performance_data data);


int main(void) {
    
    /* 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
    float previous_pos_of_line = 0.0; //for PID
    float derivative,proportional,integral = 0; //for PID
    float power; //differential speed
    float speed = OPTIMAL_SPEED; //our optimal speed setting
    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
    
    DigitalOut pit_light(LED1);  //Blue LED on mbed, lights up when in pit
    DigitalOut low_batt_light(p20);  //Red LED on expansion board
    
//    int mod_batt = 0; //modded counter to control how often sensors are polled
    
    //define the button: user push button on p21
    DigitalIn button(p21);
    button.mode(PullUp);  //1 if not pressed, 0 if pressed    

    /* When the car starts, read previous progress
     * if only connected to USB, do not perform this restore process 
     */
     
    // If robot is on this returns a value, otherwise it's just connected USB
    if (m3pi.battery() > 0) {  
        car_data = read_from_file(); //restore data from file
    }

    //Info to user
    m3pi.locate(0,1);
    m3pi.printf("Line PID");

    //Calibrate before starting <- might be moved somewhere else
    m3pi.sensor_auto_calibrate();

    //Begin main event loop    
    while (1) {
      
      //TEST ONLY
      //show_voltage();
      
      /* Check sensors and update sensor-based flags
       * - might not be every loop: wrap this in a timer of sorts */
      
      //if (mod_batt % (POLL_BATT_CYCLE + 1) == 0) {
          battery_low_flag = battery_low();
      //    low_batt_light = battery_low_flag;
      //    mod_batt = 0; //reset the counter
      //}      
      //mod_batt++;

      pit_crossroad_flag = car_at_pit_crossroad(sensor_values); //anden frekvens?
      
      //Detect button press... Do we need to debounce the button?
  //    if (button == 0) {
  //        button_pressed_flag = true;
  //    }
      
      
      /*
      switch
          if nothing else: line following
          
          */
          
          //if the button is pressed, show some stats on the display
    /*      if ( button_pressed_flag ) {
              
              //call the function to print car data
              show_car_data(car_data);
              
              //reset the flag
              button_pressed_flag = false;
          }
     */     
          //if low_batt: decellerate car (slow down) and continue to pit
          if ( battery_low_flag ) {
              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 low_batt && atPitIntersection: Stop && execute pit navigation program
          if ( battery_low_flag && pit_crossroad_flag ) {
              //We are at the pit and the battery is low -> go in
              
              //turn on the pitlight
              pit_light = 1;
              
              //Enter the pit
              pit_navigate(ENTER_PIT);
              
              //Check whether the car has connected to the charger
              //and show voltage at the same time
              for (int i = 0; i < 6; i++) {
                  show_voltage("Waiting");
                  wait(0.5);
              }
              
              if ( m3pi.battery() >= VOLTAGE_AT_PIT ) {
                  //we have connected to the charging poles
                  
                  //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);
                  
                  //Reset pit-related flags
                  battery_low_flag = false;
                  pit_crossroad_flag = false;
                  
                  show_car_data(car_data);
              }
              
              //Leave the pit
              pit_navigate(LEAVE_PIT);
              
              //reset PID variables
              proportional = 0;
              derivative = 0;
              integral = 0;
              previous_pos_of_line = 0; //denne skal også nulstilles
              
//              m3pi.forward(0.1);
//              wait(0.3);
//              m3pi.stop();
//              m3pi.sensor_auto_calibrate();
              
              pit_light = 0;
              
          }
          
          /*
          if all_sensors == 1000 (lifted from track): 
                show Wh and pitstops on display
                stop motors, reset PID errors, wait 20 secs, recalibrate sensors
                
          if all_sensors < 90 (driven off track):
                stop and ask for help? Flash LEDs? turn back and continue? Reset PID errors
      
      Periodically (not necessarily every lap - maybe every 2-5th lap):
        Save to file -> possibly only after pit
        Check battery voltage -> every couple of minutes
        Check light sensors for drift -> recalibrate (optional)
        
      */
      
      
      
      /* Separate functions
        pit_execution (only when connected to charger - voltage > V_CHARGER_MIN
            charge and wait for signal from the external charging circuit
                during charging: integrate  v(t) dt, add to cumulative sum
            when fully charged (or signal goes high):
                Calculate delta_Wh ( integrate{ v(t) dt} * u * 1/3600 ) 
                Calculate total_Wh
        
      
        AfterPitAdmin: Calibrate sensors
        show_results_on_display
        check_sensors_for_errors
      */
      
      
      // Get the position of the line.
        current_pos_of_line = m3pi.line_position();        
        proportional = current_pos_of_line;
        
        // Compute the derivative
        derivative = current_pos_of_line - previous_pos_of_line;
        
        // Compute the integral
        integral += proportional;
        
        // Remember the last position.
        previous_pos_of_line = current_pos_of_line;
        
        // Compute the power
        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
        
        // Compute new speeds   
        right = speed+power;
        left  = speed-power;
        
        // limit checks
        if (right < MIN)
            right = MIN;
        else if (right > MAX)
            right = MAX;
            
        if (left < MIN)
            left = MIN;
        else if (left > MAX)
            left = MAX;
            
       // set speed 
        m3pi.left_motor(left);
        m3pi.right_motor(right);

    } //end while
} //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("Charging");
        
        // 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(char* message) {
    m3pi.cls();
    m3pi.locate(0,0);
    m3pi.printf("%s", message);
    m3pi.locate(0,1);
    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
    
}