File content as of revision 30:d62f122e8d60:

 #include "main.h"
#include <math.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()  
{
    if( first ) {   // init the start node on first loop run only
        first = false;
        curr_coords[0] = 0;
        curr_coords[1] = 0;
        dir = 'N';
        total_points = 0;
        point[total_points] = total_points;
        coords_x[total_points] = curr_coords[0];
        coords_y[total_points] = curr_coords[1];
        type[total_points] = 0b1000;              // start is always 1 exit type in the north direction
        explored[total_points] = 0b1000;          // not sure if i set this as explored already or whether i do that at the next junction
        looped_path[total_points] = 0;            // start node is '0'
    }
    
      // 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() ) {
        path_length++;                                  // increment the path position index
        
//        float time = t_coord.read();
//        int dist_est = floor(time);
//        t_coord.stop();
//        t_coord.reset();
//        t_restart = true; //restart the timer next loop
        
        if (dir == 'N'){ curr_coords[1] += 1; }  // y coord
        if (dir == 'E'){ curr_coords[0] += 1; }  // x coord
        if (dir == 'S'){ curr_coords[1] -= 1; }
        if (dir == 'W'){ curr_coords[0] -= 1; }
        
        // 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
        if (coord_check()) {  // coord_check returns true if curr_coords coordinates are not present in coords_x and y
            total_points++;
            node_logic(); // determines what junction type we are at updates the explored (path entered on) and type arrays accordingly
            
//            if(goal_node) { point[total_points] = 100; goal_node = false; }  // 100 will be the indicator for the goal node that we can visit once mapping is complete
//            else { point[total_points] = total_points; } // numbered 0 -> total_points
            point[total_points] = total_points;
            
            coords_x[total_points] = curr_coords[0];
            coords_y[total_points] = curr_coords[1];
            
        }
        
        // use current coords to find which point to place in path
        update_index();
        looped_path[path_length] = point[curr_index]; //returns an int of which point we are at what its called
        choose_turn(); //looks at the point we are at, examines the type vs explored and makes the appropriate turn also updates the explored
        
        if (retrace) {
            // maybe do backtrack before choose turn? and put the retrace check in node logic
            // back_track();  // if no node exists with unexplored paths, set complete to true true, else return to a previous node by going back through the path, increment the path length and add nodes appropriately and return false 
        }
        
        if (complete) {
            // once complete, have it return to start node and add those turns to path return_to_start();
            looped_goal();
        }
        
        // check_explored(); // iterates through all existing points, if all explored match type, then mapping is complete
        // if not, make a func that traverses back through the bath until reaching that node, then explore the unexplored path
        // i.e. the function will take the node ID integer as an argument and go backwards through the path until reaching that node (appending each node along the way to the end of the path)
        
        
        
        // needs a function that checks if current node has any paths left to explore, if not, then it must return via the path to a node that isnt fully explored and continue from there
        
    }
    
//    robot.lcd_clear();
//    char xcors[100];
//    char ycors[100];
//    
//    for( int a = 0; a <= total_points; a += 1 )
//    {
//        char x[1];
//        char y[1];
//        sprintf(x,"%d",coords_x[a]);
//        sprintf(y,"%d",coords_y[a]);
//        xcors[a] = x[0];
//        ycors[a] = y[0];
//    }
//    robot.lcd_goto_xy(0,0);
//    robot.lcd_print(xcors,100);
//    robot.lcd_goto_xy(0,1);
//    robot.lcd_print(ycors,100);
    
    //robot.display_data();
}

bool coord_check()
{
    bool result = true;
    //returns true if the current coords dont match a previous point
    for(int i = 0; i <= total_points; i++) {
        if(curr_coords[0] == coords_x[i] && curr_coords[1] == coords_y[i]) { 
            result = false; 
        }
    }
    
    return result;
}

void update_index() // update index (pointer to current point/type/coords/explored)
{
    // checks the curr_coords againts the coords array, returns the index to relate to the point array
    for(int i = 0; i <= total_points; i++) {
        if(curr_coords[0] == coords_x[i] && curr_coords[1] == coords_y[i]) { 
            curr_index = i; 
        }
    }
}

int path_to_point_index( int path_point )
{
    for(int i = 0; i <= total_points; i++) {
        if(path_point == point[i]) { 
            return i;
        } 
    }
    
    return curr_index;  // default 
}

void node_logic()
{
    // is done when a new node is discovered, needs to update the nodes type and the path explored upon entry
    
    // first determine what turns are available relative to the robots current direction (left, straight etc.)
    // convert these relative available turns into available absolute diections (N,E etc.)
    // set _type  to the appropriate value based on available directions (including entry direction = opposite of current)
    // set _explored entry path as 1
    // set type[total_points] = _type; & explored[total_points] = _explored;
    
    bool north = false;
    bool south = false;
    bool east = false;
    bool west = false;
    
    bool left = false;
    bool straight = false;
    bool right = false;
    //bool goal = false;
    
    int _type = 0b0000;
    int _explored = 0b0000;
    
    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;
        }
    }
    
    robot.stop(); 
    wait(1);
    
//    if(goal) {
//        if( dir == 'N' ) { south = true;       _explored |= 0b0001; }  // sets the direction opposite to entry direction as an explored path 
//        else if ( dir == 'E' ) { west = true;  _explored |= 0b1000; }
//        else if ( dir == 'S' ) { north = true; _explored |= 0b0100; }
//        else if ( dir == 'W' ) { east = true;  _explored |= 0b0010; }
//        
//        if ( west  ) { _type |= 0b1000; }
//        if ( north ) { _type |= 0b0100; }
//        if ( east  ) { _type |= 0b0010; }
//        if ( south ) { _type |= 0b0001; }
//        
//        goal_node = true;
//        
//        robot.reverse(speed);
//        wait(0.1);
//        
//        // set a variable so that a different value is set in the point array
//    }
    
//    else {
        int angle = 0;
        int reset_ang = 0;
        
        if (dir == 'E') { angle = 90; reset_ang = 90; }
        else if (dir == 'S') { angle = 180; reset_ang = 180; }
        else if (dir == 'W') { angle = 270; reset_ang = 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 = reset_ang;
        }
        
        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 = reset_ang;
        }
          
        if (straight) { 
            if (angle == 0) { north = true; }
            if (angle == 180) { south = true; }
            if (angle == 90) { east = true; }
            if (angle == 270) { west = true; }
        }
        
        if( dir == 'N' ) { south = true;       _explored |= 0b0001; }  // sets the direction opposite to entry direction as an explored path 
        else if ( dir == 'E' ) { west = true;  _explored |= 0b1000; }
        else if ( dir == 'S' ) { north = true; _explored |= 0b0100; }
        else if ( dir == 'W' ) { east = true;  _explored |= 0b0010; }
        
        if ( west  ) { _type |= 0b1000; }
        if ( north ) { _type |= 0b0100; }
        if ( east  ) { _type |= 0b0010; }
        if ( south ) { _type |= 0b0001; }
//    }   
     
    type[total_points] = _type;      // maybe update_index and use curr_index instead of total_points
    explored[total_points] = _explored;
    
}

void choose_turn() 
{
    // look at cuurent coords, find what node we are at
    // looks at the type vs the explored and does the turn that is equivalent to the first 1 in type that is a 0 in explored (WNES priority)
    // sets the explored of the current node to 1 in whatever path is chosen
    // also update dir
    
    int unexp_paths = type[curr_index] & ~( explored[curr_index] );  // produces a binary of 1's in the available unexplored paths
    
    if( unexp_paths == 0b0000 ) { 
        retrace = true; 
        while(1) {
            robot.stop();
        }
    }
    
    else {
        retrace = false;
        
        int curr_angle = 0;
        if ( dir == 'E' ) { curr_angle = 90;}
        else if ( dir == 'S' ) { curr_angle = 180;}
        else if ( dir == 'W' ) { curr_angle = 270;}
        
        int desired_angle = 0;
        if ( (unexp_paths & 0b1000) == 0b1000) { 
            desired_angle = 270; 
            dir = 'W'; 
            explored[curr_index] |= 0b1000;
        }
        else if ( (unexp_paths & 0b0100) == 0b0100) { 
            desired_angle = 0;
            dir = 'N';
            explored[curr_index] |= 0b0100;
        }
        else if ( (unexp_paths & 0b0010) == 0b0010) { 
            desired_angle = 90; 
            dir = 'E';
            explored[curr_index] |= 0b0010;
        }
        else if ( (unexp_paths & 0b0001) == 0b0001) { 
            desired_angle = 180;
            dir = 'S';
            explored[curr_index] |= 0b0001;
        }
         
        int turn_angle = (desired_angle - curr_angle + 360) % 360;
        
        if( turn_angle == 0)  { turn_select('S'); } 
        else if( turn_angle == 90)  { turn_select('R'); } 
        else if( turn_angle == 180)  { turn_select('B'); } 
        else if( turn_angle == 270)  { turn_select('L'); } 
    }
}

void back_track()
{
    // find the closest previous node with unexplored paths and go back through the path until reaching that node, updating the path and directions appropriately, then choose turn
    // also if no nodes have unexplored paths set complete to true
    
    bool check = false;
    int pointer1;
    int pointer2;
    int new_dir;
    int count = path_length; // to be added to path length at the end
    
    for(int i = total_points; i >= 0; i--) {    // start from the most recently discovered node
        if( explored[i] != type[i] ) {
            check = true;
            pointer1 = i;
            break;
        }
    }
    
    if( check == false ) { complete = true; }
    
    else {
            // compare current coordinates to previous node coordinates
            // determine which direction to turn based on diretion currently facing
            // do the appropriate turn
            // follow line
            // detect junction
            // update current coords to this nodes coords
            // update the current direction, path length and path
            
        for(int j = count - 1; j >= pointer1; j--) { // starts at the previous node to current
             
            // convert looped_path[j] into an index for that nodes coords
            pointer2 = path_to_point_index(looped_path[j]); 
            
            // then do coords - curr coords
            if(coords_y[pointer2] != curr_coords[1]) { 
                // if its positive, go north, negative, go south
                if( (coords_y[pointer2] - curr_coords[1]) > 0 ) { new_dir = 0; }
                else { new_dir = 2; }
            } else if(coords_x[pointer2] != curr_coords[0]) { 
                // if its positive, go east, negative, go west
                if( (coords_x[pointer2] - curr_coords[0]) > 0 ) { new_dir = 1; }
                else { new_dir = 3; }
            }
            
            int curr_angle = 0;
            if ( dir == 'E' ) { curr_angle = 90;}
            else if ( dir == 'S' ) { curr_angle = 180;}
            else if ( dir == 'W' ) { curr_angle = 270;}
            
            // change so north = 0
            // east = 1
            // south = 2, etc. then i can just add 90*dir to the current angle and modulo 360 then divide by 4
            
            curr_angle = ((new_dir*90) - curr_angle + 360) % 360;
            
            if( curr_angle == 0 ) { turn_select('S'); }
            else if( curr_angle == 90 ) { turn_select('R'); }
            else if( curr_angle == 180 ) { 
//                robot.reverse(speed); 
//                wait(0.1); 
                turn_select('B');
            }
            else if( curr_angle == 270 ) { turn_select('L'); }
            
            bool junc = false;
            
            while(junc == false) {
                follow_line();
                if( junction_detect() ){ 
                    junc = true; 
                }
            }
            
            curr_coords[0] = coords_x[pointer2];
            curr_coords[1] = coords_y[pointer2];
            
            // dir = new_dir;  // use if dir is changed to int 0123
            
            if (new_dir == 0) { dir = 'N'; }
            else if (new_dir == 1) { dir = 'E'; }
            else if (new_dir == 2) { dir = 'S'; }
            else if (new_dir == 3) { dir = 'W'; }
            
            path_length++;
            looped_path[path_length] = point[pointer2];

        }
        
        choose_turn();   // if back_track is placed before choose turn this can be removed  
    }         
}

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 looped_goal() 
{
    while(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++ ){
        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]; }
    }
}