File content as of revision 19:c0d59019de53:

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

m3pi m3pi;

//File name
#define FILESYS "robot"
#define FILENAME "/robot/data.txt"

// Minimum and maximum motor speeds
#define MAX 1.0
#define MIN 0
#define OPTIMAL_SPEED 0.8
#define PIT_SPEED 0.3

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

bool battery_low() {
    return 0;   
}


//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]);
        }
 
int maze (void)
{
    
    return 1;
}   

void turn_left (void)
{
    m3pi.left(0.3);
    wait (0.22);
}
    
void turn_right (void)
{
    m3pi.right (0.3);
    wait (0.22);
}

int main() {
    
    /* Define all needed variables in this section */
    performance_data car_data; //Saves the car's Wh and pitstops data

    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_val[5]; //array for reflectance sensor values


    /* 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) {
      
      /*
      switch
          if nothing else: line following
          if low_batt: slow down and continue
          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
                
          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
      
      
      
      HUSK FOR POKKER KOLLES KODESTANDARD!
      
      */
      
      
      // 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);
        
        //Marc's sensor test
        sensor_all(sensor_val); // sensor_value gets fkt value
        if (sensor_val[0] > 350 && sensor_val[4] > 350)
            {
                 
                m3pi.stop();
                turn_right();
                m3pi.stop();
                m3pi.forward(0.2);
                wait (1.1);
                m3pi.stop();
                wait (5);
                m3pi.backward(0.2);
                wait (1);
                m3pi.stop();
                turn_left();
                m3pi.stop();
            }
        /*
        sensor_value4 = sensor_all(4); // sensor_value gets fkt value
        */
            
        /*
        m3pi.printf("%d", sensor_value0); // prints one of sidesensors value
        m3pi.locate(0,1);
        m3pi.printf("%d", sensor_value4); // prints the other sidesensor value
        */

    }
}