File content as of revision 13:b6480275c445:

#include "rtos.h"
#include "PID.h"
#include "PololuQik2.h"
#include "QEI.h"
#include "mbed.h"
#include "HCSR04.h"
#include "stdio.h"
#include "LPC17xx.h"
#include "Sharp.h"

#define PIN_TRIGGERL    (p12)
#define PIN_ECHOL       (p11)
#define PIN_TRIGGERR    (p29)
#define PIN_ECHOR       (p30)
#define PULSE_PER_REV  (1192)
#define WHEEL_CIRCUM   (12.56637)
#define DIST_PER_PULSE (0.01054225722682)
#define MTRS_TO_INCH   (39.3701)
#define MAX_SPEED      (0.3*127)
#define PPR            (4331/4)
#define LEFT           (1)
#define RIGHT          (0)
#define STRAIGHT       (2)
#define FORWARD        (1)
#define BACKWARD       (0)
#define TOOLS          (0)
#define MID            (1)
#define RIGS           (2)
#define MID2           (3)
#define RETURN         (4)
#define FAR            (1)


float range, range2, pid_return;
void errFunction(void);
bool cRc;

//Hardware Initialization
Serial bt(p13,p14);
Serial pc(USBTX,USBRX);
HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
PID pid1(15.0,0.0,4.0,0.02);
PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
Sharp IR(p20);
//InterruptIn encoder(p29);


//Functions
void test();
void wall_follow(int side, int direction, int section, float location);
void wall_follow_to_mid(int side, int direction, int section);
void wall_follow2(int side, int direction, int section, float location, int rig);
void leftTurn(void);
void slightleft(void);
void slightright(void);
void rightTurn(void);
void slightMove(int direction, float pulses);
void us_distance(void);
void tools_section(float* location, float &current);
void mid_section(float* location, float &current, int* direction);
void mid_section2(float* location, float &current, int* direction);
void rig_section(float* location, float &current, int* direction, int rig);
void tools_section_return(float* location, float &current);
void mid_section_return(float* location, float &current, int* direction);
void mid_section2_return(float* location, float &current, int* direction);
void rig_section_return(float* location, float &current, int* direction);
void overBump(int section);
void alignWithWall(int section);
void UntilWall(int dir);


//Variables

int main(void)
{
    float location[3], current=4;
    int direction[3];
    double distance;

    pc.baud(115200);
    pc.baud(115200);
    motors.begin();

    pc.printf("START\r\n");
    //Go to tools
    slightMove(FORWARD,3200);

    wait(2);
    slightMove(FORWARD,3200);

    while(1);
    tools_section(location, current);
    mid_section(location, current, direction);
    mid_section2(location, current, direction);
    rig_section(location, current, direction, 3);
    // fire putting out
    wait(2);
    //
    rig_section_return(location, current, direction);
    mid_section2_return(location, current, direction);
    mid_section_return(location, current, direction);
    tools_section_return(location,current);

    /*while(1) {
        pc.printf("IR %f\r\n", IR.getDistance());
        /*rangeFinderLeft.startMeas();
        rangeFinderRight.startMeas();
        wait_ms(38);
        rangeFinderLeft.getMeas(range);
        rangeFinderRight.getMeas(range);
        pc.printf("leftRange = %f\n\r", range);
        pc.printf("rightRange = %f\n\r", range);

        wait_ms(200);
    }*/

//wall_follow2(LEFT,FORWARD,MID,0);
    //leftTurn();
    //rightTurn();


}

void test( ){
    /*
    float set=6, loc=0;
    int dir=1;

    pid1.reset();

    leftEncoder.reset();
    rightEncoder.reset();

    while(loc < 10) {
        loc = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

        pid1.setInputLimits(0, set);
        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
        pid1.setSetPoint(set);
        if(side) {
            rangeFinderLeft.startMeas();
            wait_ms(20);
            rangeFinderLeft.getMeas(range);
        } else {
            rangeFinderRight.startMeas();
            wait_ms(20);
            rangeFinderRight.getMeas(range);
            pc.printf("%d\r\n",range);
        }

        if(range > 15) {
            //pc.printf("wavegap %f\r\n",wavegap);
            // AT WAVE OPENING!!!!
            motors.setMotor1Speed(dir*0.25*127);//left
            motors.setMotor0Speed(dir*0.25*127);//right
        } else {

            pid1.setProcessValue(range);
            pid_return = pid1.compute();

            if(pid_return > 0) {
                if(side) {
                    motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
                    motors.setMotor1Speed(dir*MAX_SPEED);//left
                } else {
                    motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
                    motors.setMotor0Speed(dir*MAX_SPEED);//right
                }
            } else if(pid_return < 0) {
                if(side) {
                    motors.setMotor0Speed(dir*MAX_SPEED);//right
                    motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
                } else {
                    motors.setMotor1Speed(dir*MAX_SPEED);//left
                    motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
                }
            } else {
                motors.setMotor0Speed(dir*MAX_SPEED);//right
                motors.setMotor1Speed(dir*MAX_SPEED);//left
            }
        }
    }

    //STOP
    motors.setMotor0Speed(dir*-0.3*127); //right
    motors.setMotor1Speed(dir*-0.3*127); //left
    wait_ms(10);
    motors.stopBothMotors(0);
    */
}

void errFunction(void)
{
    //Nothing
}

void us_distance(void)
{
    pc.printf("Ultra Sonic\n\r");
    rangeFinderLeft.startMeas();
    wait_us(20);
    if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
        pc.printf("Range = %f\n\r", range);
    }
}

void wall_follow(int side, int direction, int section, float location)
{
    float set=6, loc=0;
    int dir=1;

    pid1.reset();

    if(direction == BACKWARD) dir=-1;
    if(section == TOOLS)set= 10;

    leftEncoder.reset();
    rightEncoder.reset();

    while(loc + location< 66.5) {
        loc = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

        pid1.setInputLimits(0, set);
        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
        pid1.setSetPoint(set);
        if(side) {
            rangeFinderLeft.startMeas();
            wait_ms(20);
            rangeFinderLeft.getMeas(range);
        } else {
            rangeFinderRight.startMeas();
            wait_ms(20);
            rangeFinderRight.getMeas(range);
            pc.printf("%d\r\n",range);
        }

        if(range > 15) {
            //pc.printf("wavegap %f\r\n",wavegap);
            // AT WAVE OPENING!!!!
            motors.setMotor1Speed(dir*0.28*127);//left
            motors.setMotor0Speed(dir*0.25*127);//right
        } else {

            pid1.setProcessValue(range);
            pid_return = pid1.compute();

            if(pid_return > 0) {
                if(side) {
                    motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
                    motors.setMotor1Speed(dir*MAX_SPEED);//left
                } else {
                    motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
                    motors.setMotor0Speed(dir*MAX_SPEED);//right
                }
            } else if(pid_return < 0) {
                if(side) {
                    motors.setMotor0Speed(dir*MAX_SPEED);//right
                    motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
                } else {
                    motors.setMotor1Speed(dir*MAX_SPEED);//left
                    motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
                }
            } else {
                motors.setMotor0Speed(dir*MAX_SPEED);//right
                motors.setMotor1Speed(dir*MAX_SPEED);//left
            }
        }
    }

    //STOP
    motors.setMotor0Speed(dir*-0.3*127); //right
    motors.setMotor1Speed(dir*-0.3*127); //left
    wait_ms(10);
    motors.stopBothMotors(0);
}

void wall_follow_to_mid(int side, int direction, int section)
{
    float location, set=6;
    int dir=1;

    pid1.reset();

    if(direction == BACKWARD) dir=-1;
    if(section == TOOLS)set= 10;

    leftEncoder.reset();
    rightEncoder.reset();

    location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

    while(location< 34) {
        location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

        pid1.setInputLimits(0, set);
        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
        pid1.setSetPoint(set);
        if(side) {
            rangeFinderLeft.startMeas();
            wait_ms(20);
            rangeFinderLeft.getMeas(range);
        } else {
            rangeFinderRight.startMeas();
            wait_ms(20);
            rangeFinderRight.getMeas(range);
            pc.printf("%d\r\n",range);
        }

        if(range > 15) {
            //pc.printf("wavegap %f\r\n",wavegap);
            // AT WAVE OPENING!!!!
            motors.setMotor1Speed(dir*0.28*127);//left
            motors.setMotor0Speed(dir*0.25*127);//right
        } else {

            pid1.setProcessValue(range);
            pid_return = pid1.compute();

            if(pid_return > 0) {
                if(side) {
                    motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
                    motors.setMotor1Speed(dir*MAX_SPEED);//left
                } else {
                    motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
                    motors.setMotor0Speed(dir*MAX_SPEED);//right
                }
            } else if(pid_return < 0) {
                if(side) {
                    motors.setMotor0Speed(dir*MAX_SPEED);//right
                    motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
                } else {
                    motors.setMotor1Speed(dir*MAX_SPEED);//left
                    motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
                }
            } else {
                motors.setMotor0Speed(dir*MAX_SPEED);//right
                motors.setMotor1Speed(dir*MAX_SPEED);//left
            }
        }
    }

    //STOP
    motors.setMotor0Speed(dir*-0.3*127); //right
    motors.setMotor1Speed(dir*-0.3*127); //left
    wait_ms(10);
    motors.stopBothMotors(0);
}

/* MODIFIED WALL_FOLLOW FOR NAVIGATION */

void wall_follow2(int side, int direction, int section, float location, int rig)
{
    int dir=1, limit=80, lowlim=5;
    float set=6, loc=0, Rigloc=0;
    bool SeeWaveGap = false;

    if(rig == 1) Rigloc= 16;
    else if(rig == 2) Rigloc= 45;
    else if(rig== 3) Rigloc = 70;

    pid1.reset();

    if(direction == BACKWARD) {
        dir=-1;
        limit = 100;
    }
    if(section == TOOLS) {
        set= 6;
        limit = 86;
    }
    if(section == RETURN) lowlim=15;

    leftEncoder.reset();
    rightEncoder.reset();

    //pc.printf("before %f\r\n", location);

    pc.printf("dir*loc+location %f\r\n",dir*loc + location );
    pc.printf("limit %d \r\n", limit);

    while((dir*loc + location <= limit) && (dir*loc + location >= lowlim)) {

        loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
        //pc.printf("loc %f \r\n", loc);

        pid1.setInputLimits(0.0, set);
        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
        pid1.setSetPoint(set);

        if(side) {
            rangeFinderLeft.startMeas();
            wait_ms(20);
            rangeFinderLeft.getMeas(range);
        } else {
            rangeFinderRight.startMeas();
            wait_ms(20);
            rangeFinderRight.getMeas(range);
        }

        if(section == RIGS) {
            rangeFinderLeft.startMeas();
            wait_ms(20);
            rangeFinderLeft.getMeas(range2);

            if(range2< 20) {
                if( abs(dir*loc + location - Rigloc) < 10) {
                    //STOP
                    motors.setMotor0Speed(dir*-0.25*127); //right
                    motors.setMotor1Speed(dir*-0.25*127); //left
                    wait_ms(5);
                    motors.stopBothMotors(0);
                    break;
                }
            }
        }


        //pc.printf("wall follow 2 range %f\r\n",range);
        //pc.printf("loc+location = %f\r\n", loc+location);
        if(range > 20 ) {
            if(section == RIGS || section == RETURN) {
                motors.setMotor0Speed(dir*0.25*127); //right
                motors.setMotor1Speed(dir*0.25*127); //left
            } else {
                if(!SeeWaveGap) {
                    SeeWaveGap=true;
                } else {
                    //STOP
                    motors.setMotor0Speed(dir*-0.25*127); //right
                    motors.setMotor1Speed(dir*-0.25*127); //left
                    wait_ms(5);
                    motors.stopBothMotors(0);

                    pc.printf("wavegap\r\n");
                    // AT WAVE OPENING!!!!
                    break;
                }
            }
        } else {
            SeeWaveGap = false;
            pid1.setProcessValue(range);
            pid_return = pid1.compute();
            //pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);

            if(pid_return > 0) {
                if(side) {
                    motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
                    motors.setMotor1Speed(dir*MAX_SPEED);//left
                } else {
                    motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
                    motors.setMotor0Speed(dir*MAX_SPEED);//right
                }
            } else if(pid_return < 0) {
                if(side) {
                    motors.setMotor0Speed(dir*MAX_SPEED);//right
                    motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
                } else {
                    motors.setMotor1Speed(dir*MAX_SPEED);//left
                    motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
                }
            } else {
                motors.setMotor0Speed(dir*MAX_SPEED);
                motors.setMotor1Speed(dir*MAX_SPEED);
            }
        }
    }

    //STOP
    motors.setMotor0Speed(dir*-0.3*127); //right
    motors.setMotor1Speed(dir*-0.3*127); //left
    wait_ms(5);
    motors.stopBothMotors(0);
}


void alignWithWall(int section)
{
    float usValue = 0;

    if(section == TOOLS) {
        pc.printf("tools section align\r\n");
        // turn at an angle
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(-1.2*MAX_SPEED); //right
        motors.setMotor1Speed(0.4*MAX_SPEED); //left
        while(rightEncoder.getPulses()>-1000);
        motors.stopBothMotors(0);

        //go backwards toward wall
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(-0.25*127); //right
        motors.setMotor1Speed(-0.25*127); //left
        while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
        motors.stopBothMotors(0);

        // turn left towards wall
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(MAX_SPEED); //right
        motors.setMotor1Speed(-MAX_SPEED); //left
        while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20);

        motors.stopBothMotors(0);

        // turning left
        motors.setMotor0Speed(0.9*MAX_SPEED); //right
        motors.setMotor1Speed(-0.9*MAX_SPEED); //left

    } else if( section == RIGS) {
        // check distance to wall
        rangeFinderRight.startMeas();
        wait_ms(20);
        rangeFinderRight.getMeas(range);

        if(range < 4 || range > 20) return;

        // turn at an angle
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor1Speed(-1.2*MAX_SPEED); //left
        motors.setMotor0Speed(0.4*MAX_SPEED); //right
        while(abs(leftEncoder.getPulses())<1000);
        motors.stopBothMotors(0);

        //go backwards toward wall
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(-0.25*127); //right
        motors.setMotor1Speed(-0.25*127); //left
        while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
        motors.stopBothMotors(0);

        // turn left towards wall
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(-MAX_SPEED); //right
        motors.setMotor1Speed(MAX_SPEED); //left
        while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20);

        motors.stopBothMotors(0);

        // turning left
        motors.setMotor0Speed(-0.9*MAX_SPEED); //right
        motors.setMotor1Speed(0.9*MAX_SPEED); //left
    } else {
        pc.printf("in mid section align\r\n");
        // turn right towards wall
        rightTurn();
        // turning left towards wall
        motors.setMotor0Speed(0.9*MAX_SPEED); //right
        motors.setMotor1Speed(-0.9*MAX_SPEED); //left
    }

    usValue = 0;
    while(1) {
        if(section == RIGS) {
            rangeFinderRight.startMeas();
            wait_ms(20);
            rangeFinderRight.getMeas(range);
        } else {
            rangeFinderLeft.startMeas();
            wait_ms(20);
            rangeFinderLeft.getMeas(range);
        }
        pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
        if(range > usValue && usValue != 0 && range < 25) {
            break;
        } else {
            usValue = range;
        }
    }
    motors.stopBothMotors(0);
}

void rightTurn(void)
{
    motors.begin();
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(-0.5*127);//right
    motors.setMotor1Speed(0.5*127);//left
    while(abs(leftEncoder.getPulses())<950 || abs(rightEncoder.getPulses())<950);
    motors.stopBothMotors(0);
}

void leftTurn(void)
{
    motors.begin();
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.5*127);// right
    motors.setMotor1Speed(-0.5*127);// left
    while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
    motors.stopBothMotors(0);
}

void slightleft(void)
{

    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.5*127);// right
    motors.setMotor1Speed(-0.5*127);// left
    while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90);
    motors.stopBothMotors(0);
}

void slightright(void)
{

    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(-0.4*127);// right
    motors.setMotor1Speed(0.4*127);// left
    while(abs(leftEncoder.getPulses())<90 || abs(rightEncoder.getPulses())<90);
    motors.stopBothMotors(0);
}

void slightMove(int direction, float pulses)
{
    int dir=1;

    if(direction == BACKWARD) dir= -1;

    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(dir*0.22*127); //right
    motors.setMotor1Speed(dir*0.25*127); //left
    while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses);
/*
    motors.setMotor0Speed(dir*-0.25*127); //right
    motors.setMotor1Speed(dir*-0.25*127); //left
    wait_ms(10);
*/
    motors.stopBothMotors(127);

}

void UntilWall(int dir)
{

    if(dir == BACKWARD) dir=-1;

    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(dir*0.2*127); //right
    motors.setMotor1Speed(dir*0.2*127); //left

    range = 30;

    while(range > 20) {
        rangeFinderRight.startMeas();
        wait_ms(20);
        rangeFinderRight.getMeas(range);
    }

    motors.setMotor0Speed(dir*-0.2*127); //right
    motors.setMotor1Speed(dir*-0.2*127); //left
    wait_ms(5);
    motors.stopBothMotors(0);
}

void overBump(int section)
{
    int preLeft=5000, preRight=5000, out=0;

    motors.begin();
    // slight backwards
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(-0.25*127); //right
    motors.setMotor1Speed(-0.25*127); //left
    while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
    motors.stopBothMotors(0);

    pc.printf("slight backwards\r\n");
    wait_ms(200);

    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.3*127); //right
    motors.setMotor1Speed(0.3*127); //left
    while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getDistance() >15 ) {
        /*preLeft=leftEncoder.getPulses();
        preRight=rightEncoder.getPulses();
        wait_ms(200);
        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;*/
    }

    pc.printf("forward \r\n");
    wait_ms(200);
    /*
       motors.stopBothMotors(0);
       motors.begin();

       leftEncoder.reset();
       rightEncoder.reset();
       motors.setMotor0Speed(0.3*127); //right
       motors.setMotor1Speed(0.3*127); //left

       while(!out) {
           preLeft=leftEncoder.getPulses();
           preRight=rightEncoder.getPulses();

           rangeFinderLeft.startMeas();
           rangeFinderRight.startMeas();
           wait_ms(20);
           rangeFinderLeft.getMeas(range);
           rangeFinderRight.getMeas(range2);
           if(range < 10 || range2 < 10) out=1;

           if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
               motors.setMotor0Speed(0.4*127); //right
               motors.setMotor1Speed(0.4*127); //left
               wait_ms(50);
               out=1;
           }
           if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1;
       }
       */

    motors.stopBothMotors(0);
    motors.begin();

    preLeft=preRight=5000 ;
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(.25*127); //right
    motors.setMotor1Speed(.25*127); //left

    if(section == TOOLS) {
        while(IR.getDistance() > 10 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {

            if(IR.getDistance() > 38) break;

            preLeft=leftEncoder.getPulses();
            preRight=rightEncoder.getPulses();
            wait_ms(200);
        }
    } else if(section == MID || section == MID2) {
        if(section == MID2) while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400));
        while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {

            if(IR.getDistance() > 38) break;

            preLeft=leftEncoder.getPulses();
            preRight=rightEncoder.getPulses();
            wait_ms(200);
        }

    } else {
        while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100);

        leftEncoder.reset();
        rightEncoder.reset();

        motors.setMotor0Speed(-.15*127); //right
        motors.setMotor1Speed(-.15*127); //left
        while((abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && preLeft!=0 ) {
            preLeft = leftEncoder.getPulses();
            preRight = rightEncoder.getPulses();
            wait_ms(200);
            if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
        }

        leftEncoder.reset();
        rightEncoder.reset();

        motors.setMotor0Speed(0.25*127); //right
        motors.setMotor1Speed(0.25*127); //left
        while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));

        motors.stopBothMotors(0);

        return;
    }

    leftEncoder.reset();
    rightEncoder.reset();

    motors.setMotor0Speed(-.25*127); //right
    motors.setMotor1Speed(-.25*127); //left
    while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));

    motors.stopBothMotors(0);
    wait_ms(20);
    motors.begin();

}

void tools_section(float* location, float &current)
{
    slightMove(FORWARD,3200);
    current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

    wait(2);
    slightMove(FORWARD,3200);
    current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
    /*
    wall_follow_to_mid(LEFT,FORWARD, TOOLS);
    // current position in reference to the starting position
    current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

    wait(2);

    wall_follow(LEFT,FORWARD, TOOLS, current);
    // current position in reference to the starting position
    current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
    */
    //////////////////////////////// determine tool
    wait(2);
    ///////////////////////////////////////////////////////////////////////////////////////
    // Move Forward
    slightMove(FORWARD, 100);

    //////////////////////////////////////////Tool aquiring
    wait(2);
    //////////////////////////////////////////////////////////////////// After tool is aquired

    alignWithWall(TOOLS);
    pc.printf("align\r\n");
    wait_ms(100);

    //wall_follow2(LEFT,FORWARD,MID, current);
    //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);

    rangeFinderLeft.startMeas();
    wait_ms(20);
    rangeFinderLeft.getMeas(range);

    if(range < 20) {
        wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
        pc.printf("wall follow\r\n");
        location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        current= location[0];
        pc.printf("current %f \r\n",current);
        // go backwards
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(-MAX_SPEED); //right
        motors.setMotor1Speed(-MAX_SPEED); //left
        while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
        // hard stop
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(MAX_SPEED); //right
        motors.setMotor1Speed(MAX_SPEED); //left
        while(abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses())< 10);
        motors.stopBothMotors(0);

        wait_ms(100);
        leftTurn();
        overBump(TOOLS);
    } else {
        pc.printf("else greater than 20\r\n");
        location[0]= current;
        leftTurn();
        overBump(TOOLS);
    }

    pc.printf("First Wavegap = %f\r\n",location[0]);
}

void mid_section(float* location, float &current, int* direction)
{
    motors.begin();

    if(IR.getDistance() > 38) {
        direction[0]= STRAIGHT;
        overBump(MID);
        return;
    }
    pc.printf("before align with wall \r\n");
    alignWithWall(MID);
    wait_ms(100);

    pc.printf("mid section current = %f\r\n",current);
    wall_follow2(LEFT,FORWARD,MID, current,0);
    current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    pc.printf("after wf2 current = %f\r\n",current);

    wait_ms(500);
    rangeFinderLeft.startMeas();
    wait_ms(20);
    rangeFinderLeft.getMeas(range);

    if(range > 20 ) {
        direction[0]= RIGHT;
        location[1]= current;
        slightMove(FORWARD,75);
        //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    } else {
        direction[0]= LEFT;
        wall_follow2(LEFT,BACKWARD,MID,current,0);
        location[1]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        current= location[1];

        if(location[1] < 18) {
            slightMove(FORWARD, 50);
            current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        }

    }

    pc.printf("wavegap2 = %f\r\n",location[1]);
    leftTurn();

    wait_ms(100);

    overBump(MID);

}

void mid_section2(float* location, float &current, int* direction)
{
    motors.begin();

    pc.printf("mid section 2\r\n");

    if(IR.getDistance() > 38) {
        direction[1]= STRAIGHT;
        overBump(RIGS);
        return;
    }

    alignWithWall(MID);
    pc.printf("midsection 2 alignt with wall mid \r\n");

    wall_follow2(LEFT,FORWARD,MID, current,0);
    current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);

    wait_ms(500);

    pc.printf("midseection 2 after wf2 %f",current);
    rangeFinderLeft.startMeas();
    wait_ms(20);
    rangeFinderLeft.getMeas(range);

    if(range > 20 ) {
        direction[1]= RIGHT;
        location[2]= current;
        slightMove(FORWARD,75);
        //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    } else {
        direction[1]= LEFT;
        wall_follow2(LEFT,BACKWARD,MID,current,0);
        location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        current=location[2];
        //slightMove(FORWARD,500);
    }

    leftTurn();
    overBump(RIGS);
    pc.printf("overbump rigs\r\n");
}

void rig_section(float* location, float &current, int* direction, int rig)
{
    float loc;

    if(rig == 1) loc= 15;
    else if(rig == 2) loc= 45;
    else loc = 75;

    rightTurn();
    slightright();

    if(current > loc) {
        pc.printf("RIG section %f\r\n",current);
        wall_follow2(RIGHT, BACKWARD, RIGS, current, rig);
        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    } else {
        pc.printf("RIG section %f\r\n",current);
        wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    }
}

void tools_section_return(float* location, float &current)
{
    if(location[0] > 16) {
        leftTurn();
        wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0);
    }
    motors.stopBothMotors(0);

}

void mid_section_return(float* location, float &current, int* direction)
{
    if(direction[0] == RIGHT) {
        leftTurn();
        alignWithWall(MID);
        wall_follow2(LEFT, BACKWARD, MID, current,0);
        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        rightTurn();
    } else if(direction[0] == LEFT) {
        leftTurn();
        wall_follow2(RIGHT, FORWARD, MID, current,0);
        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        rightTurn();
    }
    //ELSE and GO FORWARD
    overBump(RIGS);
}

void mid_section2_return(float* location, float &current, int* direction)
{
    if(direction[1] == RIGHT) {
        leftTurn();
        wall_follow2(LEFT, BACKWARD, MID, current,0);
        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        rightTurn();
    } else if(direction[1] == LEFT) {
        leftTurn();
        wall_follow2(RIGHT, FORWARD, MID, current,0);
        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        rightTurn();
    }
    //ELSE and GO FORWARD
    overBump(MID);
}

void rig_section_return(float* location, float &current, int* direction)
{
    alignWithWall(RIGS);
    if(location[2] > current) {
        wall_follow2(RIGHT, FORWARD, MID, current,0);
        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    } else {
        wall_follow2(RIGHT, BACKWARD, MID, current,0);
        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    }
    rightTurn();
    overBump(MID2);
}