File content as of revision 13:ddff4bb7c24f:

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

m3pi m3pi;

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

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

// Prototypes
int PitTest(void);   // Test if to robot needs to goto pit 
void InitialMessages (void); // Prints initial message to the LCD 
void LED_Control(int ledNumber, int state) //turn ledNumber to 1=on, 0 = off

int main() {
    
    m3pi.sensor_auto_calibrate();
    
    /*Base program Variable initiation*/
    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 = MAX;

    /*Team 7 Variabels*/
    int gotoPit = 0; // wether or not the robot is heading to pit. Initialstate false. 
    int ccount; //used to count cycles
    

    
    /*Printing secret cat mission*/
    InitialMessage();
 

    
    while (1) {
        /* If cycle count divided by 100 does not have a rest. test if pit */
        if (ccount %100 == 0 && gotoPit=0){
            gotoPit = PitTest(void);
            }
    
        // 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);
        
    ccount++;
    }
}

void InitialMessage(void){
    /*Prints iniatl secret mission*/
    
    m3pi.cls();
    m3pi.locate(0,0);
    m3pi.printf("eliminate");
    m3pi.locate(0,1);
    m3pi.printf("all cats");
    wait(200.0);
    
    m3pi.cls();
    m3pi.locate(0,0);
    m3pi.printf("%f.3 ",m3pi.battery());
    m3pi.locate(0,1);
    m3pi.printf("%f.3 ",m3pi.pot_voltage());
    wait(200.0);
    m3pi.cls();
}
int PitTest(void){
/* Test the batteri voltage if the robot is not headed for pit */ 
    
    const float BATVOLTTRESHOLD = 3.0; // Treshold i volt
    int result;
    
    /*Digital outs*/
    DigitalOut led1(LED1);
    led1 = 0; // Turn off led 1 on the embed
    
    /*Test if the voltage is below the threshold if so turn on go to pit mode*/
    if (m3pi.battery() <= BATVOLTTRESHOLD ()){  
        result = 1; // Set goto pit condition
        led1 = 1; // Turn on Led 1
        m3pi.cls();
        m3pi.locate(0,0);
        m3pi.printf("Going to");
        m3pi.locate(0,1);
        m3pi.printf("PIT Party");
    }
    return result;
}

void LED_Control(int ledNumber, int state){
    //LED1 on if robot is looking for pit
    if ledNumber == 1{
        DigitalOut led1(state);
    }
    if ledNumber == 2{
        DigitalOut led1(state);
    }
    if ledNumber == 3{
        DigitalOut led1(state);
    }
    if ledNumber == 4{
        DigitalOut led1(state);
    }
}