File content as of revision 1:fbf9b4ab4bc5:

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

m3pi m3pi;

// Minimum and maximum motor speeds
#define MAX 1.0
#define MIN 0

//Our variables
#define OPTIMAL_SPEED 0.8

// PID terms
#define P_TERM 1.0
#define D_TERM 8.0
#define I_TERM 0.01  //skal være nul eller tæt på

//Print to LCD (character, line) numbers are zero indexed
void print_battery() {
    
    //measure battery voltage
    float f = m3pi.battery();
    char text[8];
    m3pi.cls();
    m3pi.locate(0,0);
    
    //convert to a string
    sprintf(text, "%4.2f", f);
    m3pi.printf(text);
}

void toggleLED(DigitalOut &led) {
    if (led == 1) {
        led = 0;
    } else {
        led = 1;
    }
}


int main(void) {
    
    //user push button p21
    DigitalIn button(p21);
    button.mode(PullUp);

    //set the LEDs as objects    
    //on the m3pi
    DigitalOut redled8(p13);
    DigitalOut redled7(p14);
    DigitalOut redled6(p15);
    DigitalOut redled5(p16);
    DigitalOut redled4(p17);
    DigitalOut redled3(p18);
    DigitalOut redled2(p19);
    DigitalOut redled1(p20);
    
    //on the mbed
    DigitalOut myled1 (LED1);
    DigitalOut myled2 (LED2);
    DigitalOut myled3 (LED3);
    DigitalOut myled4 (LED4);
    
    
    // Set all LEDs to off
    redled1 = 0;
    redled2 = 0;
    redled3 = 0;
    redled4 = 0;
    redled5 = 0;
    redled6 = 0;
    redled7 = 0;
    redled8 = 0;
    myled1 = 0;
    myled2 = 0;
    myled3 = 0;
    myled4 = 0;
    
    
    //Initialize the car
    m3pi.sensor_auto_calibrate();

    // create some control variables
    float right;
    float left;
    float current_pos_of_line = 0.0;
    float previous_pos_of_line = 0.0;
    float derivative,proportional,integral = 0;
    float power;
    float speed = OPTIMAL_SPEED;
    
    
    // Start the loop
    while (1) {
    
    // 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);
    
    print_battery();

}
    
    
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        /*
        f = m3pi.battery();
        if (f > 4.7) {
            myled1 = 1;
        } else {
            myled1 = 0;
        }
        
        
        m3pi.cls();
        m3pi.locate(0,0);
    
        //convert to a string
        sprintf(text, "%4.2f", f);
        m3pi.printf(text);
        wait(0.2);
        */
          
      
}



/* Structure

functions:

Charging Strategy
poll voltage -> return voltage
Charging limits
Low Batt level
Enough charge level

Control -> inherits from m3pi
pause line follower
resume line follower
speed up (home stretch)


Class: CarData


Class: PitData
Total time in pit
Total stops in pit

Methods:
Update Wh
Update pit stops
Print pitstops to screen





*/