File content as of revision 87:88aebb4c7226:

#include "mbed.h"
#include "m3pi.h"
#include <cstdio>

m3pi m3pi;
Timer timer;

// DigitalOuts & Global Variabels
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
int startTime;

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

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

//  Ccount before test
#define CYCLEBEFORETEST 4500

// Textfile paths
#define PITLOGPATH "/local/pitlog.txt"
#define VOLTAGELOGPATH "/local/voltage.txt"

// Prototypes
void LED_Blink(int ledNumber); // Make ledNumber blinik

void LCD_CountDown(int num);  //LCD Coundown function
void LCD_InitialMessages(void); // Prints initial message to the LCD 

int PS_BatteryTest(void);   // Test if to robot needs to goto pit 
void PS_PitStop(void); // 
void PS_CreateLog(void); // create a log file or resets it (WIP
void PS_AddStopToLog(void); // Add one to the log 
int PS_GetNumberofPS(void);  // Get number form file
void PS_DisplayPS(void);

void TE_CreateVoltageLog(void); //
void TE_LogVoltage(int count); // test funktion that write the woltage each time the battry is checked


int main() {
    LocalFileSystem local("local"); 
    timer.start();
    startTime = timer.read();


    /*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 = 0; //used to count cycles
    
    /*Printing secret cat mission*/
    PS_DisplayPS();
    LCD_InitialMessages();
    m3pi.sensor_auto_calibrate();
    

    /*Create logs used to log the number of pitstop */
    PS_CreateLog(); //
    TE_CreateVoltageLog();

    while (1) {

        /* If cycle count divided by a constant does not have a rest. test if pit */
        if (ccount % CYCLEBEFORETEST == 0 && gotoPit == 0)
        {
            TE_LogVoltage(ccount);
            gotoPit = PS_BatteryTest();
        }
        if (gotoPit == 1)
        {
            /*Add one to the nummber allready in the pitlog*/
            PS_AddStopToLog();

            /*Run the pitstop function*/
            PS_PitStop();
        }
            
        // 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++;
    }
    
}

/**
 * LCD_InitialMessages -Prints iniatial secret mission
 */
void LCD_InitialMessages(void){

    m3pi.cls();
    m3pi.locate(0,0);
    m3pi.printf("DESTROY");
    m3pi.locate(0,1);
    m3pi.printf("**CATS**");
    wait(5.0);
    m3pi.cls();
    m3pi.locate(0,0);
    m3pi.printf("%4.4f ",m3pi.battery());
    m3pi.locate(0,1);
    m3pi.printf("%4.4f ",m3pi.pot_voltage());
    wait(10.0);
    m3pi.cls();
    m3pi.locate(0,0);
    m3pi.printf("ROBOT ON");
    m3pi.locate(0,1);
    m3pi.printf("TRACK!!");
    wait(4.0);
    LCD_CountDown(3);
    m3pi.cls();
    m3pi.locate(0,0);
    m3pi.printf("** GO **");

     
    wait (1.0);
}

/**
 * PS_DisplayPS - Display the number of pidstops from the log
 */

void PS_DisplayPS(void){
    m3pi.cls();
    m3pi.locate(0,0);
    m3pi.printf("PITSTOP:");
    m3pi.locate(0,1);
    m3pi.printf("%d", PS_GetNumberofPS() );

}

/**
 * LCD_CountDown - display a countdown
 * @num  The number to count down from-
 */
void LCD_CountDown(int num){

    for (int i=num; i>0; i--)
    {
       m3pi.cls();
       m3pi.locate(0,0); 
       m3pi.printf("** %d **", i);
       wait(1.0);
    }
}   

/**
 * PS_BatteryTest - Test the batteri voltage if the robot is not headed for pi
 * 
 *return: 0 if the battery is above the threshold- Return 1 if the robot needs to goto pit
 */
int PS_BatteryTest(void){
    
    const float BATVOLTTRESHOLD = 0.5; // Treshold i volt
    int result = 0;
  
    /*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
        LED_Blink(3);

        m3pi.cls();
        m3pi.locate(0,0);
        m3pi.printf("Going to");
        m3pi.locate(0,1);
        m3pi.printf("**PIT**");
    }
    return result;
}

/**
 * LED_Blink - Make a LED blink
 *@ledNumber - The number of the targeted LED 
 */
void LED_Blink(int ledNumber)
{
    int a = 2;
    
    switch (ledNumber){
        case(1):
            led1 = 0;
            wait(a);
            led1 = 1;
            wait(a);
            led1 = 0;
            wait(a);
            break;
        
        case(2):
    
            led2 = 0;
            wait(a);
            led2 = 1;
            wait(a);
            led2 = 0;
            wait(a);
            break;
        
        case(3):
            led3 = 0;
            wait(a);
            led3 = 1;
            wait(a);
            led3 = 0;
            wait(a);
            break;

        case(4):
            led4 = 0;
            wait(a);
            led4 = 1;
            wait(a);
            led4 = 0;
            wait(a);
            break;
        
        
        default:
            break;
    }


        
    }

/**
 * PS_PitStop - Stops the robot and starts the signal
 */
void PS_PitStop(void)
{
    m3pi.stop();       // stop all engine
    // increase counter with one
    while (1)
    {
        LED_Blink (1);      // signal in pit
    }
}

/**
 * PS_CreateLog - Create a pitstop log i none excist
 * 
 * Return: The number of ppitstops as noted in the log
 */
void PS_CreateLog(void){
    FILE *fptr;
    
    if ((fptr = fopen(PITLOGPATH,"r")) == NULL){
        fptr = fopen(PITLOGPATH,"w");
        fprintf(fptr,"%d", 0);
        fclose(fptr);
    }
    
}

/**
 * PS_AddStopToLog - Add one to the number in the pitstop log 
 */
void PS_AddStopToLog(void){
 
    FILE *fptr;
    int x, y;
    if ((fptr = fopen(PITLOGPATH,"r")) == NULL){
        printf("Error! opening file");
        // Program exits if the file pointer returns NULL.
        exit(1);
       }

    fscanf(fptr,"%d", &x);
    fclose(fptr);

    y = x+1;
    fptr = fopen(PITLOGPATH,"w");

    if(fptr == NULL)
        {
             printf("Error creating log file ");
             exit(1);
        }
    fprintf(fptr,"%d", y);
    fclose(fptr);
}

/** PS_GetNumberofPS - Return the  number i the pitstop recorded in the logfile
*
*return: An interger from the the pitlog
*/
int PS_GetNumberofPS(void){
    // 
    FILE *fptr;
    int x;
    if ((fptr = fopen(PITLOGPATH,"r")) == NULL){
        printf("Error! opening file");
        // Program exits if the file pointer returns NULL.
        exit(1);
       }
     fscanf(fptr,"%d", &x);
     //printf("Final number of pits stops %d", x);
     fclose(fptr);
     return x;
}

/** TE_CreateVoltageLog - create a voltagelog
*/
void TE_CreateVoltageLog(void){
/* Create a voltagelog file and test if it can open*/
    FILE *fptr;
    fptr = fopen(VOLTAGELOGPATH,"w");

    if(fptr == NULL)
         {
            printf("Error creating log file ");
            exit(1);
         }
    
    fclose(fptr);
}

/** TE_LogVoltage - Add an entry to the voltagelog 
*@count: The number the counter has reached in the main loop 
*/
void TE_LogVoltage(int count){

    FILE *fptr; /* voltagelog adres */
    fptr = fopen(VOLTAGELOGPATH,"a");
  
    fprintf(fptr,"%8d,%8d,%4.4f,%4.4f\n"  , timer.read()-startTime, count, m3pi.battery(),m3pi.pot_voltage()  );
    fclose(fptr);

}