File content as of revision 21:54ea75f7984f:

#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);

// Sensors
DigitalInOut enc_L(p26); //connected to digital P26
DigitalInOut enc_R(p25); //connected to digital P25

// Main

int main()
{
    init();
    
    bool loop_check;
    
    robot.lcd_goto_xy(0,0);
    robot.lcd_print("A=simple", 10);
    robot.lcd_goto_xy(0,1);
    robot.lcd_print("B=looped", 10);
    
    while(button_A.read() == 1 && button_B.read() == 1) {}
    
    if (button_B.read()) { loop_check = true; }   // non-looped
    if (button_A.read()) { loop_check = false; }  // looped

    robot.lcd_clear();
    robot.lcd_goto_xy(0,0);
    robot.lcd_print("  ENTER   ", 10);
    robot.lcd_goto_xy(0,1);
    robot.lcd_print("= start   ", 10);
    
    calibrate();
    
    robot.lcd_clear();
    
    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) {
        
        if (loop_check == true) {
            non_looped();
        } else {
            looped();
        }
        
        wait(dt);
    }
}

void read_encoders() 
{
        enc_R.output();    // Set the I/O line to an output
        enc_L.output();
        enc_R.mode(PullUp);
        enc_L.mode(PullUp);
        
        wait_us(10);         // Must be atleast 10us for the 10 nF capacitor to charge
        enc_R.mode(PullNone);
        enc_L.mode(PullNone);   
        enc_R = 1;            // Drive the line high
        enc_L = 1;
        
        t_R.start();
        enc_R.input();        // Make the I/O line an input (high impedance)
        while (enc_R == 1 || t_R.read_us() < 1000);  // replace 1000 with a hard variable (1000 = 1ms = 1kHz sampling) (might be able to drop this further
        // sampling time is required to be this high for times when there is no reflectance but we only care about high reflectance
        
        // maybe i could wait a few microseconds, see if the encoder is still high, if high then no reflectance, if low, the high reflectance
        // this would increase sampling time
        
        // also, the fact that the waits are in the same loop means that the loop will run at different speeds depending on whether a sensor is triggered or not
        // if both are triggered it will run fast, otherwise it will have to wait 1000+ us for each sensor
        
        // this therefore needs to be done in parallel and also must not affect the time of other operations in the main loop
        encoder[0] = t_R.read_us();   // Measure the time for the capacitor to discharge by waiting for the I/O line to go low
        t_R.stop();
        t_R.reset();
        
        t_L.start();
        enc_L.input();
        while (enc_L == 1 || t_L.read_us() < 1000);
        encoder[1] = t_L.read_us();        
        t_L.stop();
        t_L.reset();
}

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) {}  // wait for enter to be pressed
    
    wait(2.0);  
    
    robot.auto_calibrate();  
    
    robot.stop();
    wait(0.05);
    robot.scan();
    
    leds = 0b0000;
}

void non_looped() 
{
    follow_line();
    
    if ( junction_detect() ) {
        char turn = junction_logic(); 
        turn_select(turn);
        
        path[path_length] = turn;
        path_length ++;
    }

    
    simplify();
    
    robot.lcd_clear();
    robot.lcd_print(path,100);

    //robot.display_data();
}

void looped()
{
      // follow line until reaching a junction, determine its type and coordinates
    if (t_restart){                // only start the timer if it isnt already started
        t_coord.start();
        t_restart = false;
    }
    
    follow_line();
    
    if ( junction_detect() ) {
                
        int dist_est = t_coord.read();
        t_coord.stop();
        t_coord.reset();
        
        //auto dist_est = time - (time%1); // round the value (nearest second)
        
        if (dir == 'N'){ curr_coords[1] += dist_est; }  // y coord
        if (dir == 'E'){ curr_coords[0] += dist_est; }  // x coord
        if (dir == 'S'){ curr_coords[1] -= dist_est; }
        if (dir == 'W'){ curr_coords[0] -= dist_est; }
        
        // check that the coordinates are not already in the list, if not add the point, if it is already return the point number and increment the explored
        point[total_points] = total_points+1; // numbered 1 -> x
        
        coords_x[total_points] = curr_coords[0];
        coords_y[total_points] = curr_coords[1];
        
        node_logic();
        
        // always try left first if there is a left. if node has already been explored once, then dont go left, go straight, if no straight then right
        
        total_points += 1;
        // assign new node with new coordinates
        t_restart = true; //restart the timer
            
    }
    
    // simplify();
    
    robot.lcd_clear();
    robot.lcd_print(path,100);

    //robot.display_data();
}

void node_logic()
{
    bool north = false;
    bool south = false;
    bool east = false;
    bool west = false;
    
    bool left = false;
    bool straight = false;
    bool right = false;
    
    int total = 1;   // starts at 1 because path entered on is counted
    
    if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
        while ( (sensor[0] > SENS_THRESH || 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; }
            if ( sensor[4] > SENS_THRESH ) { right = true; }
        }
        
        if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
            wait(0.05);        // maybe change or replace w something better
            robot.scan();
        }
        
        robot.scan();
        
        if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
            straight = true;
        }
    }
    
    if (left) { total += 1; }
    if (straight) { total += 1; }
    if (right) { total += 1; }
    
    type[total_points] = total; 
    
    int angle = 0;
    
    if (dir == 'E') { angle = 90;}
    else if (dir == 'S') { angle = 180;}
    else if (dir == 'W') { angle = 270;}
    
    if (left) { 
        angle += 270;
        angle = angle % 360;
        if (angle == 0) { north = true; }
        if (angle == 180) { south = true; }
        if (angle == 90) { east = true; }
        if (angle == 270) { west = true; }
        angle -= 270;
        angle = angle % 360;
    }
    
    if (right) { 
        angle += 90;
        angle = angle % 360;
        if (angle == 0) { north = true; }
        if (angle == 180) { south = true; }
        if (angle == 90) { east = true; }
        if (angle == 270) { west = true; }
        angle -= 90;
        angle = angle % 360;
    }
      
    if (straight) { 
        if (angle == 0) { north = true; }
        if (angle == 180) { south = true; }
        if (angle == 90) { east = true; }
        if (angle == 270) { west = true; }
    }
    
    int turn_count = type[total_points] - explored[total_points];
    
    if (west == false) { turn_count += 1; }
    if (north == false) { turn_count += 1; }
    if (east == false) { turn_count += 1; }
    if (south == false) { turn_count += 1; }
    
    turn_count = turn_count%4;
    
    char do_turn = turn_priority[turn_count];
    
    if (dir == 'E') { angle = 90;}
    else if (dir == 'S') { angle = 180;}
    else if (dir == 'W') { angle = 270;}
    
    unsigned int result = 0;
    
    if (do_turn == 'N') { dir = 'N'; result = 0; }
    else if (do_turn == 'S') { dir = 'S'; result = 180; }
    else if (do_turn == 'E') { dir = 'E'; result = 90; }
    else if (do_turn == 'W') { dir = 'W'; result = 270; }
    
    result = result - angle;
    
    if (result == 0) { turn_select('S'); }
    else if (result == 90) { turn_select('R'); }
    else if (result == 180) { turn_select('B'); }
    else if (result == 270) { turn_select('L'); }
    // figure out a way to check what directions correlate to which turns based on direction currently facing.
}

void choose_turn() 
{
    // compares the type to the explored (type - explored), then uses that as an index for the struct of the turn order priority (turn_order[3] = {'E' , 'N', 'W', 'S'} 
    // also takes into account current direction (if facing north, it wont take the south path for example)
    // do this in point type
    
}

void follow_line() 
{
    robot.scan();
    sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
    
    leds = 0b0110;
    
    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);
    }
    
//    read_encoders();     
//    if (encoder[0] > 3100) { dist_est_1 += 1; }  // going to have to reset these dist estimates every junction (in the if (junc_detect()) statement)
//    if (encoder[1] > 3100) { dist_est_2 += 1; }  // might not need to actually use 2pir/3 could just add arbitrary numbers
}

bool junction_detect() 
{
    if ( sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH ) {
        return true;
    } else if ( sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH ) {
        return true;
    } else {
        return false;
    }
}

char junction_logic() 
{
    bool straight = false;
    bool left = false;
    bool right = false;
    bool goal = false;
    
    if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
        while ( (sensor[0] > SENS_THRESH || 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; }
            if ( sensor[4] > SENS_THRESH ) { right = true; }
        }
        
        if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
            wait(0.05);        // maybe change or replace w something better
            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_select( 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] > SENS_THRESH) { robot.scan(); }
    
    robot.spin_left(0.2);
    wait(0.1);
    
    while (sensor[1] < SENS_THRESH) { robot.scan(); }
    
    while (sensor[1] > SENS_THRESH) { robot.scan(); }
}

void right()
{
    leds = 0b0011;

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

void back() 
{
    leds = 0b1111;
    robot.reverse(speed);
    wait(0.1);
    robot.spin_right(TURN_SPEED);
    
    while (sensor[3] < SENS_THRESH) { robot.scan(); }
    
    while (sensor[3] > SENS_THRESH) { 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(inv_path,100);
    
    while(1) {
        int pointer = 0;
        
        robot.stop();
        
        leds = 0b1001;
        wait(0.2);
        leds = 0b0110;
        wait(0.2);
        
        robot.reverse(speed);
        while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
        wait(0.05);
        
        robot.spin_right(TURN_SPEED);
        while(sensor[2] > SENS_THRESH) { robot.scan(); }
        while(sensor[3] < SENS_THRESH) { robot.scan(); } 
        while(sensor[3] > SENS_THRESH) { robot.scan(); } 

        robot.stop();
        
        while(pointer <= path_length) {
            follow_line();
            
            if ( junction_detect() ) {  // if junction found
                char na = junction_logic();   // aids turing fluidity (char not necessary therefore could clean up a bit) 
                turn_select(inv_path[pointer]);
                if(inv_path[pointer] == 'S') {      // make this better
                    robot.forward(speed);
                    leds = 0b1010;
                    while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
                }
                pointer++;
            }
        }
        
        back();
        
        robot.stop();
        robot.lcd_goto_xy(0,0);
        robot.lcd_print("  ENTER   ", 10);
        robot.lcd_goto_xy(0,1);
        robot.lcd_print("=restart", 10);
        
        while ( button_enter.read() == 1 ) { speed = (pot_S*0.3)+0.2; }  // keep looping waiting for Enter to be pressed (can change speed)
        
        robot.lcd_clear();
        robot.lcd_print(path,100);
        
        pointer = 0;
        
        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 ( junction_detect() ) {  // if junction found
                char na = junction_logic();   // aids turing fluidity (char not necessary therefore could clean up a bit) 
                turn_select(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++;
            }
        }
    }
}

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