File content as of revision 12:d80399686f32:

#include "main.h"

// API
m3pi robot;

// LEDs
BusOut leds(LED4,LED3,LED2,LED1);

// Buttons 
DigitalIn button_A(p18);
DigitalIn button_B(p17);
DigitalIn button_X(p21);
DigitalIn button_Y(p22);
DigitalIn button_enter(p24);
DigitalIn button_back(p23);

// Potentiometers
AnalogIn pot_P(p15);
AnalogIn pot_I(p16);
AnalogIn pot_D(p19);
AnalogIn pot_S(p20);

// Prototypes
void init();
void calibrate();
void follow_line();
char junction_detect();
void turn_selector( char turn );
void left();
void right();
void back();
void goal();
void simplify();
void invert_path();

// Constants

const float A = 0.5;         // 20
const float B = 1/10000;     // 10000
const float C = 1.5;         // 2/3

const int sens_thresh = 500;    // replace the hard coded bits
const int turn_speed = 0.2;

// Global Variables

char path[100];
char inv_path[100];
int path_length = 0;
unsigned int *sensor;  
float speed;
float proportional = 0.0;
float prev_proportional = 0.0;
float integral = 0.0;
float derivative = 0.0;

// Main

int main()
{
    init();
    calibrate();
    speed = (pot_S*0.3)+0.2;   // have it so max is 0.5 and min is 0.2 (this lowest doesnt work)
    
    float dt = 1/50;           // updating 50 times a second
    
    while (1) {

        follow_line();

        char turn = junction_detect(); // rename this function something else
        turn_selector(turn);
        if ( turn != 'S' ) {              // doesnt need 'S', also may not need 'R' as that is not technically a junction
            path[path_length] = turn;
            path_length ++;
        }
        
        simplify();
        
        robot.lcd_clear();
        robot.lcd_print(path,100);
    
        //robot.display_data();
        
        wait(dt);
    }
}

void init()
{
    robot.init();

    button_A.mode(PullUp);
    button_B.mode(PullUp);
    button_X.mode(PullUp);
    button_Y.mode(PullUp);
    button_enter.mode(PullUp);
    button_back.mode(PullUp);

    leds = 0b0000;
}

void calibrate()
{
    leds = 0b1111;
    robot.reset_calibration();

    while (button_enter.read() == 1) {}  // keep looping waiting for Enter to be pressed
    
    wait(2.0);  // small delay to allow hands to move away etc.
    
    robot.auto_calibrate();  // run calibration routine
    leds = 0b0000;
}
      
void follow_line() 
{
    robot.scan();
    sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
    
    proportional = robot.read_line();  // returns a value between -1,1     (-1 = PC0 or further , -1 to -0.5 = PC1 (-0.5 is directly below PC1) , -0.5 to 0 = PC2 , 0 to 0.5 = PC3 , 0.5 to 1 and further = PC4)
    derivative = proportional - prev_proportional;
    integral += proportional;
    prev_proportional = proportional;
    
    // calculate motor correction
    float motor_correction = proportional*A + integral*B + derivative*C;
    
    // make sure the correction is never greater than the max speed as the motor will reverse
    if( motor_correction > speed ) {
        motor_correction = speed;
    }
    if( motor_correction < -speed ) {
        motor_correction = -speed;
    }

    if( proportional < 0 ) {
        robot.motors(speed+motor_correction,speed);
    } else {
        robot.motors(speed,speed-motor_correction);
    }
}

char junction_detect() 
{
    bool left = false;
    bool right = false;
    bool straight = false;
    bool goal = false;
    
    if (sensor[0] > sens_thresh || sensor[4] > sens_thresh) {
        if (sensor[0] > sens_thresh) {
            left = true;
            while ( sensor[0] > sens_thresh && (sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh) ) {
                robot.forward(speed);
                robot.scan();
                if (sensor[4] > sens_thresh) {
                    right = true;
                }
            }
            
            robot.scan();
            
            if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
                wait(0.05);
                robot.scan();
                if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
                    goal = true;
                }
            }
            
            robot.scan();
            
            if ( sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh ) {
                straight = true;
            }
        }  
        
        else if (sensor[4] > sens_thresh) {
            right = true;
            while ( sensor[4] > sens_thresh && (sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh) ) {
                robot.forward(speed);
                robot.scan();
                if (sensor[0] > sens_thresh) {
                    left = true;
                }
            }
            
            robot.scan();
            
            if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
                wait(0.05);
                robot.scan();
                if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
                    goal = true;
                }
            }
            
            robot.scan();
            
            if ( sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh ) {
                straight = true;
            }
        }  
        
    } else if (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) {
        return 'B';
    }
    
    if (goal) {
        return 'G';
    } else if (left) {
        return 'L';
    } else if (straight) {
        return 'S';
    } else if (right) {
        return 'R';
    } else {
        return 'S';
    }
}


void turn_selector( char turn )
{
    switch(turn) {
        case 'G':
            goal();
        case 'L':
            left();
            break;
        case 'S':
            break;
        case 'R':
            right();
            break;
        case 'B':
            back();
            break;
    }
}
        
void left()
{
    leds = 0b1100;

    while (sensor[0] > 500) { robot.scan(); }
    
    robot.spin_left(0.2);
    wait(0.1);
    
    while (sensor[1] < 500) { robot.scan(); }
    
    while (sensor[1] > 500) { robot.scan(); }
}

void right()
{
    leds = 0b0011;

    while (sensor[4] > 500) { robot.scan(); }
    
    robot.spin_right(0.2);
    wait(0.1);
    
    while (sensor[3] < 500) { robot.scan(); }
    
    while (sensor[3] > 500) { robot.scan(); }
}

void back() 
{
    leds = 0b1111;
    robot.reverse(speed);
    wait(0.1);
    robot.spin_right(0.2);
    
    while (sensor[3] < 500) { robot.scan(); }
    
    while (sensor[3] > 500) { robot.scan(); }
}

void simplify()
{
    // check if the last one was a 'B'
    // if it was, iterate over the last three turns and check the total angle change
    // replace the three turns with the new single turn
    
    if( path[path_length-2] == 'B' && path_length >= 3) {
        int angle_change = 0;
        
        for (int i = 1; i <= 3; i++) {
            if (path[path_length - i] == 'L') { angle_change += 270; }
            else if (path[path_length - i] == 'R') { angle_change += 90; }
            else if (path[path_length - i] == 'B') { angle_change += 180; }
        }
        
        angle_change = angle_change % 360;
        
        if (angle_change == 0) { path[path_length - 3] = 'S'; }
        else if (angle_change == 90) { path[path_length - 3] = 'R'; }
        else if (angle_change == 180) { path[path_length - 3] = 'B'; }
        else if (angle_change == 270) { path[path_length - 3] = 'L'; }
        
        for (int i = 1; i <= 2; i++) { path[path_length - i] = NULL; }   // clear the other turns
        
        path_length -= 2;        
    }
}  

void goal()
{ 
    invert_path();
    
    leds = 0b0000;
    
    robot.lcd_clear();
    robot.lcd_print(path,100);
    
    while(1) {
        int pointer = 0;   //path_length - 1
        
        robot.stop();
        while (button_enter.read() == 1) { speed = (pot_S*0.3)+0.2; }  // keep looping waiting for Enter to be pressed (can change speed)
        
        leds = 0b1001;
        wait(0.2);
        leds = 0b0110;
        wait(0.2);
        leds = 0b1001;
        wait(0.2);
        leds = 0b0110;
        wait(0.2);
        
        while(pointer <= path_length) {
            follow_line();
            
            if (sensor[0] > sens_thresh || sensor[4] > sens_thresh) {  // if junction found
                turn_selector(path[pointer]);
                if(path[pointer] == 'S') {      // make this better
                    robot.forward(speed);
                    leds = 0b1010;
                    while(sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
                }
                pointer++;
            }
        }
        
        robot.stop();
        while(button_enter.read() == 1) {
            leds = 0b1001;
            wait(0.2);
            leds = 0b0110;
            wait(0.2);
        }
    }
}

void invert_path()
{
    // only call once then can use infinitely
    for( int i = 0; i <= path_length; i++ ){
        inv_path[i] = path[path_length-1-i];
        if( inv_path[i] == 'L' ) { inv_path[i] = 'R'; }
        if( inv_path[i] == 'R' ) { inv_path[i] = 'L'; }
    }
}