File content as of revision 22:fe373ba68df3:

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

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

//Team calibrated parameters for car and battery
#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  //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

//Various motor speeds
#define MAX 1.0
#define MIN 0
#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
#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;  


//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;
}


//TO DO: CLEAN UP THIS FUNCTION
void sensor_all(int arr[]) //lav en funktion? ved ikke hvad jeg skal her
     {
        m3pi.putc(0x87); // send noget sensor data
    
        for(int n=0; n < 5; n++) // forloop så sensor for data
        {
        char lowbyte = m3pi.getc();
        char hibyte  = m3pi.getc();
        arr[n] = ((lowbyte + (hibyte << 8))); // sensor value
        }
        m3pi.cls();
        m3pi.locate(0,1);
        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;
    }
}

//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();
    }
}

//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)
{
    
    return 1;
}   


int main() {
    
    /* 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[5]; //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
    

    /* 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) {
      
      /* 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
          if nothing else: line following
          
          */
          
          //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
              
              //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;
          }
          
          /*
          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_navigation (entering pit from T-intersection all the way to the charger)
        pit_execution (only when connected to charger - voltage > V_CHARGER_MIN
            add 1 to number of pitstops
            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
                save progress data to file
            leave the charger -> pass control to pit_exit
            
        pit_exit
            reverse out of the pit back to T-intersection
            continue line following away from T-intersection, recal sensors after a few seconds
            
      
        AfterPitAdmin: Calibrate sensors, save state to file, 
          
        show_results_on_display:
          
        check_sensors_for_errors:
        
        check_battery_voltage -> return true false
            compare m3pi.battery() < V_CRITICAL
      
      
      
      
      */
      
      
      // 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