File content as of revision 8:77a57909aa15:

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

/* Navigation Definitions */
#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 FORWARD        (1)
#define BACKWARD       (0)
#define TOOLS          (0)
#define MID            (1)
#define RIGS           (2)
#define FIRST_WAVE     (0)
#define FAR            (1)

//States
#define START                       0
#define OILRIG1_POS                 1
#define OILRIG2_POS                 2
#define GOTO_TOOLS                  3
#define IDENTIFY_TOOLS              4
#define AQUIRE_TOOL1                5
#define AQUIRE_TOOL2                6
#define AQUIRE_TOOL3                7
#define NAVIGATE_WAVES_ROW1         8
#define NAVIGATE_WAVES_ROW2         9
#define NAVIGATE_WAVES_ROW3         10
#define NAVIGATE_TO_SQUARE_RIG      11
#define NAVIGATE_TO_TRIANGLE_RIG    12
#define NAVIGATE_TO_CIRCLE_RIG      13
#define RIG_ALIGN                   14
#define INSERT_TOOL                 15
#define END                         16



//Servo Static Positions
#define STORE_POSITION          0
#define OIL_RIG1                1
#define OIL_RIG2                2
#define OIL_RIG3                3
#define DRIVE_POSITION_NOTOOL   4
#define TOOL_1                  5
#define TOOL_2                  6
#define TOOL_3                  7
#define DRIVE_POSITION_TOOL     8
#define ORIENT_TOOL             9

//Rig definitions
#define SQUARE                  1
#define TRIANGLE                2
#define CIRCLE                  3

//*********************//
//******* TODO ********//
//*********************//
//Oil Rig distance thresholds
#define OILRIG1_MAX     3000
#define OILRIG1_MIN     1000
#define OILRIG2_MAX     3000
#define OILRIG2_MIN     1000
#define OILRIG3_MAX     3000
#define OILRIG3_MIN     1000

//for servo normalization
#define MIN_SERVO_PULSE     900
#define MAX_SERVO_PULSE     2100
#define SERVO_MAX_ANGLE     180


DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);

void errFunction(void);
bool cRc;

Serial pc(USBTX,USBRX);                     //USB Comm
Adafruit_PWMServoDriver pwm(p28,p27);       //pwm(SDA,SCL) - Servo Control PWM
DigitalOut ServoOutputDisable(p8);          //Servo Control Output Enable/Disable
extern Serial lrf;                         //Laser Range Finder    lrf(p13,p14)
//Hardware Initialization
//Serial bt(p13,p14);
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);




/***************
local servo functions
****************/
void servoBegin(void);
void initServoDriver(void);
void setServoPulse(uint8_t n, int angle);
void setServoPulseNo_delay(uint8_t n, int angle);
void servoPosition(int set);
void setGripper(int open);
int fire_checker(int rig);


//Navigation Functions
float wall_follow(int side, int direction, int section);
void wall_follow2(int side, int direction, int section, float location);
void wall_follow3(int &currentLocation, int &WaveOpening);
void leftTurn(void);
void slightleft(void);
void rightTurn(void);
void us_distance(void);
void to_tools_section(float* location, float &current);
void from_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 overBump(int section);
void alignWithWall(int section);
void ledtoggle(void);


/************
Main Variables
*************/
int state = START;
int fire = 0;
int tool_needed = 0;
int shape_detected = 0;
float range, range2, pid_return;


/************
Variables for Servos
*************/
int servoNum, servoAngle, outputDisabled, posNum, testVal;
int currentPosition[7];

typedef struct {
    int arm_position_name;      //for organization only (STORE, OILRIG1, OILRIG2...)
    int base_rotate;
    int base_arm;
    int big_arm;
    int claw_arm;
    int claw_rotate;
    int claw_open;
} Coord;

/********************
Static Arm Positions
*********************/

Coord Arm_Table[] = {

    // POSITION ODER:
    // base_rotate, base_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open

//increase in number 5 rotates gripper

    {STORE_POSITION, 85, 5, 5, 175, 100, 0},              // storing position
    {OIL_RIG1, 163, 90, 90, 48, 100, 0},                 // point laser at oilrig1
    {OIL_RIG2, 144, 90, 90, 47, 100, 0},                // point laser at oilrig2
    {OIL_RIG3, 130, 90, 90, 50, 100, 0},                // point laser at oilrig2
    {DRIVE_POSITION_NOTOOL, 55, 70, 102, 74, 0, 0},   // Drive through course
    {TOOL_1, 95, 64, 97, 79, 0, 0},                  // Look over first tool
    {TOOL_2, 75, 70, 102, 74, 0, 0},                  // Look over second tool
    {TOOL_3, 55, 70, 102, 74, 0, 0},                  // Look over third tool
    {DRIVE_POSITION_TOOL, 55, 70, 102, 74, 0, 0},     // Drive with tool loaded
    {ORIENT_TOOL, 135, 60, 75, 60, 90, 90},             // position tool to be inserted
};


/* Variables for imageprocessing and distance */
int x_coord;
int y_coord;
int area;
int shape;

/* Variables for distance sensor*/
int distLaser;
int fire_detected = 0;
int fire_not_detected = 0;


int main()
{

    /*****************
     INITIALIZATIONS
    *******************/
    float location[3], current=0;
    int direction[3];
    double distance;

    pc.baud(115200);
    //Laser Range Finder Initialization
    lrf_baudCalibration();
    motors.begin();

    //Servo initialization
    initServoDriver();
    servoBegin();   //initiates servos to start position
    //ServoOutputDisable = 0;

    /*******************
    WHILE LOOP FOR TESTING
    ********************/
    while(1) {
        pc.scanf("%d %d", &servoNum, &servoAngle);
        if(servoAngle > 175) {
            servoAngle = 175;
        }
        if(servoNum > 5 ) {
            servoNum = 0;
            servoAngle = 90;
        }
        setServoPulse(servoNum, servoAngle);
        
        //ledtoggle();

        //pc.scanf("%d", &posNum);
        //servoPosition(posNum);
        //wait(3);
        //shape_detected = shapeDetection();
        //distLaser = getDistance();
        //pc.printf("Distance %d", distLaser);

    }

    /********************************
    MAIN WHILE LOOP FOR COMPETITION
    *********************************/
    while(1) {
        switch (state) {

                /**************************************************
                *           STAGE 0
                *
                *          - START OF THE COMETITION
                *
                **************************************************/
            case START :
                myled1 = 1;
                wait(5);
                myled1 = 0;
                state = OILRIG1_POS;
                break;


                /**************************************************
                *           STAGE 1
                *
                *          - DETERMINE OIL RIG ON FIRE
                *
                **************************************************/
            case OILRIG1_POS:                   //aims arm at square oil rig
                servoPosition(OIL_RIG1);        //position arm to point at first oilrig
                wait(2);                        //wait for servos to settle
                fire = fire_checker(OIL_RIG1);  //determines if oil rig is on fire

                //determines what tool is needed
                if (fire == 1) {
                    tool_needed = SQUARE;
                    state = GOTO_TOOLS;
                } else {
                    state = OILRIG2_POS;
                }
                break;

            case OILRIG2_POS:
                servoPosition(OIL_RIG2);    //position arm to point at first oilrig
                wait(1);                    //wait for servos to settle
                fire = fire_checker(OIL_RIG2);
                if (fire == 1) {
                    tool_needed = TRIANGLE;
                    state = GOTO_TOOLS;
                } else {
                    tool_needed = CIRCLE;
                    state = GOTO_TOOLS;
                }
                break;

                /**************************************************
                *           STAGE 2
                *
                *          - TRAVEL TO TOOLS
                *
                **************************************************/
            case GOTO_TOOLS:
                servoPosition(DRIVE_POSITION_NOTOOL);   //drive position without a tool
                wait(1);                                //wait for servos to settle

//****************************************************//

                to_tools_section(location, current);

                state = IDENTIFY_TOOLS;
                break;
                while(1) {}
                /**************************************************
                *           STAGE 3
                *
                *           - Determine order of tools
                *           - Aquire appropriate tool
                *
                **************************************************/
            case IDENTIFY_TOOLS:
                servoPosition(TOOL_1);              //arm/camera looks over tool
                wait(1);                            //wait for servos to settle
                centerCamWithTool();                //centers camera over tool
                shape_detected = shapeDetection();  //determines the shape

                //either goes to aquire the tool or look at the next shape
                if(shape_detected == tool_needed) {
                    state = AQUIRE_TOOL1;
                } else {
                    servoPosition(TOOL_2);
                    wait(1);                        //wait for servos to settle
                    centerCamWithTool();
                    shape_detected = shapeDetection();
                    if (shape_detected == tool_needed) {
                        state = AQUIRE_TOOL2;
                    } else {
                        servoPosition(TOOL_3);
                        wait(1);                            //wait for servos to settle
                        centerCamWithTool();
                        state = AQUIRE_TOOL3;
                    }
                }
                break;
            case AQUIRE_TOOL1:
                //*********************//
                //******* TODO ********//
                //*********************//
                // CODE TO GRAB TOOL1
                servoPosition(DRIVE_POSITION_TOOL);     //arm position for driving with the tool
                state  = NAVIGATE_WAVES_ROW1;
                break;
            case AQUIRE_TOOL2:
                //*********************//
                //******* TODO ********//
                //*********************//
                // CODE TO GRAB TOOL2
                servoPosition(DRIVE_POSITION_TOOL);     //arm position for driving with the tool
                state  = NAVIGATE_WAVES_ROW1;
                break;
            case AQUIRE_TOOL3:
                //*********************//
                //******* TODO ********//
                //*********************//
                // CODE TO GRAB TOOL3
                servoPosition(DRIVE_POSITION_TOOL);     //arm position for driving with the tool
                state  = NAVIGATE_WAVES_ROW1;
                break;


                /**************************************************
                *           STAGE 4
                *
                *           - Navigate through the ocean
                *
                **************************************************/

            case NAVIGATE_WAVES_ROW1:
                //*********************//
                //******* TODO ********//
                //*********************//
                // CODE TO NAVIGATE ROW1
                state = NAVIGATE_WAVES_ROW2;
                break;

            case NAVIGATE_WAVES_ROW2:
                //*********************//
                //******* TODO ********//
                //*********************//
                // CODE TO NAVIGATE ROW2
                state = NAVIGATE_WAVES_ROW3;
                break;

            case NAVIGATE_WAVES_ROW3:
                //*********************//
                //******* TODO ********//
                //*********************//
                // CODE TO NAVIGATE ROW3

                //goes to appropriate rig
                if(shape_detected == 1) {
                    state = NAVIGATE_TO_SQUARE_RIG;
                } else if(shape_detected == 2) {
                    state = NAVIGATE_TO_TRIANGLE_RIG;
                } else {
                    state = NAVIGATE_TO_CIRCLE_RIG;
                }
                break;

                /**************************************************
                *           STAGE 5
                *
                *           - Travel to appropriate rig
                *
                **************************************************/
            case NAVIGATE_TO_SQUARE_RIG:
                //NAVIGATION CODE HERE
                state = RIG_ALIGN;
                break;
            case NAVIGATE_TO_TRIANGLE_RIG:
                //NAVIGATION CODE HERE
                state = RIG_ALIGN;
                break;
            case NAVIGATE_TO_CIRCLE_RIG:
                //NAVIGATION CODE HERE
                state = RIG_ALIGN;
                break;

                /**************************************************
                *           STAGE 6
                *
                *           - Align with appropriate rig
                *
                **************************************************/
            case RIG_ALIGN:

                //*********************//
                //******* TODO ********//
                //*********************//
                // CODE TO ALIGN ROBOT WITH RIG

                servoPosition(ORIENT_TOOL);
                wait(1);                        //wait for servos to settle
                state = INSERT_TOOL;
                break;

                /**************************************************
                *           STAGE 7
                *
                *           - Insert Tool
                *           - Extenguish fire
                *           - win contest
                *
                **************************************************/

            case INSERT_TOOL:
                //*********************//
                //******* TODO ********//
                //*********************//
                // CODE TO INSERT TOOL
                break;

                /**************************************************
                *           STAGE 8
                *
                *           - END COMPETITION
                *
                **************************************************/
            case END:
                servoPosition(STORE_POSITION);
                myled1 = 1;
                wait(.2);
                myled2 = 1;
                wait(.2);
                myled3 = 1;
                wait(.2);
                myled4 = 1;
                wait(.2);
                break;
            default:

                break;
        }
    }


}



/************

Servo Functions

**************/

void setServoPulse(uint8_t n, int angle)
{
    int pulse;
    pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE -  MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
    float pulselength = 20000;   // 20,000 us per second
    int i = currentPosition[n];
    pc.printf("ServoNumber: %d  Begining Pulse: %d\n\r",n,currentPosition[n]);
    int pulse2;
    if(currentPosition[n] < pulse) {
        for(i; i < pulse; i++) {
            pulse2 = 4094 * i / pulselength;
            pwm.setPWM(n, 0, pulse2);
            wait_ms(3);
        }
    } else if (currentPosition[n] > pulse) {
        for(i; i > pulse; i--) {
            pulse2 = 4094 * i / pulselength;
            pwm.setPWM(n, 0, pulse2);
            wait_ms(3);
        }
    }
    currentPosition[n] = i;
    pc.printf("\nEND: pulse: %d,  angle: %d\n\n", i, angle);
}

void initServoDriver(void)
{
    pwm.begin();
    //pwm.setPWMFreq(100);  //This dosen't work well because of uncertain clock speed. Use setPrescale().
    pwm.setPrescale(140);    //This value is decided for 20ms interval.
    pwm.setI2Cfreq(400000); //400kHz

}

void servoBegin(void)
{
    pc.printf("Setting Initial Position\n\r");
    setServoPulseNo_delay(3, 175);
    wait(2);
    setServoPulseNo_delay(2, 0);
    wait(2);
    setServoPulseNo_delay(1, 10);
    wait(2);
    setServoPulseNo_delay(0, 85);
    wait(1);
    setServoPulseNo_delay(4, 100);
    wait(1);
    setServoPulseNo_delay(5, 0);
    setGripper(1);
}

void setServoPulseNo_delay(uint8_t n, int angle)
{
    int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE -  MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
    float pulselength = 20000;   // 20,000 us per second
    currentPosition[n] = pulse;
    //pc.printf("ServoNumber: %d    Pulsewidth: %d  Angle: %d \n\r",n,pulse, angle);
    pulse = 4094 * pulse / pulselength;
    pwm.setPWM(n, 0, pulse);

}

void setGripper(int open)
{
    if (open) {
        pc.printf("Gripper Open\r");
        setServoPulseNo_delay(6, 10);
    } else {
        pc.printf("Gripper  Closed\n\r");
        setServoPulseNo_delay(6, 170);
    }
}

void servoPosition(int set)
{
    //moves to current position
    setServoPulse(3, Arm_Table[set].claw_arm);
    setServoPulse(1, Arm_Table[set].base_arm);
    setServoPulse(2, Arm_Table[set].big_arm);
    setServoPulse(0, Arm_Table[set].base_rotate);
    setServoPulse(4, Arm_Table[set].claw_rotate);
    setServoPulse(5, Arm_Table[set].claw_open);
}


int fire_checker(int rig)
{
    switch (rig) {

        case 1:
            for (int i = 0; i<5; i++) {
                distLaser = getDistance();
                if ((distLaser < OILRIG1_MAX)
                        || (distLaser > OILRIG1_MIN)) {
                    fire_detected++;
                } else {
                    fire_not_detected++;
                }
            }
        case  2:
            for (int i = 0; i<5; i++) {
                distLaser = getDistance();
                if ((distLaser < OILRIG2_MAX)
                        || (distLaser > OILRIG2_MIN)) {
                    fire_detected++;
                } else {
                    fire_not_detected++;
                }
            }
        case 3:
            for (int i = 0; i<5; i++) {
                distLaser = getDistance();
                if ((distLaser < OILRIG3_MAX)
                        || (distLaser > OILRIG3_MIN)) {
                    fire_detected++;
                } else {
                    fire_not_detected++;
                }
            }
        default:
            for (int i = 0; i<5; i++) {
                distLaser = getDistance();
                if ((distLaser < OILRIG1_MAX)
                        || (distLaser > OILRIG1_MIN)) {
                    fire_detected++;
                } else {
                    fire_not_detected++;
                }
            }
    }

    if (fire_detected > fire_not_detected) {
        return 1;
    } else {
        return 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);
    }
}

float wall_follow(int side, int direction, int section)
{
    float location, wavegap=0, set=5;
    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< 73) {
        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(38);
            rangeFinderLeft.getMeas(range);
        } else {
            rangeFinderRight.startMeas();
            wait_ms(38);
            rangeFinderRight.getMeas(range);
            pc.printf("%d\r\n",range);
        }

        if(range > 20) {
            wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
            //pc.printf("wavegap %f\r\n",wavegap);
            // AT WAVE OPENING!!!!
            motors.setMotor1Speed(dir*0.4*127);//left
            motors.setMotor0Speed(dir*0.4*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
            }
        }
    }
    return wavegap;
}

/* MODIFIED WALL_FOLLOW FOR NAVIGATION */

void wall_follow2(int side, int direction, int section, float location)
{
    int SeeWaveGap = false, dir=1;
    float set=5, loc=0;

    pid1.reset();

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

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

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

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

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


        /*************CHECK FOR WAVE OPENING*****************/
        /* If after 20 ms the ultrasonic still sees 20+ cm */
        /*      then robot is at wave opening               */

        //pc.printf("wall follow 2 range %f\r\n",range);
        //pc.printf("loc+location = %f\r\n", loc+location);
        if(range > 20) {
            motors.stopBothMotors();
            pc.printf("wavegap\r\n");
            // AT WAVE OPENING!!!!
            break;
        }

        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);
        }
    }
    motors.stopBothMotors();
}


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

    if(section == TOOLS) {
        // 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();

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

        motors.stopBothMotors();

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

        motors.stopBothMotors();

        motors.setMotor0Speed(0.7*MAX_SPEED); //right
        motors.setMotor1Speed(-0.7*MAX_SPEED); //left
    } else {
        rightTurn();
        motors.setMotor0Speed(-0.7*MAX_SPEED); //right
        motors.setMotor1Speed(0.7*MAX_SPEED); //left
    }

    usValue = 0;
    while(1) {
        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();
}

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

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

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


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

    motors.begin();

    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(-0.2*127); //right
    motors.setMotor1Speed(-0.2*127); //left
    while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50);
    motors.stopBothMotors();

    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.2*127); //right
    motors.setMotor1Speed(0.2*127); //left
    while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0) {
        preLeft=leftEncoder.getPulses();
        preRight=rightEncoder.getPulses();
        wait_ms(100);
        //pc.printf(" first while left %d right %d \r\n", preLeft, preRight);
        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
    }

    motors.stopBothMotors();
    motors.begin();
    wait(2);
    /*
        motors.stopBothMotors();
        motors.setMotor0Speed(0.15*127); //right
        motors.setMotor1Speed(0.15*127); //left
        preLeft=preRight=5000 ;
        leftEncoder.reset();
        rightEncoder.reset();
     */
//   while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
    /*        preLeft=leftEncoder.getPulses();
            preRight=rightEncoder.getPulses();
            pc.printf("second while left %d right %d \r\n", preLeft, preRight);
            wait_ms(200);
            if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
        }*/

    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
        }
        if(abs(leftEncoder.getPulses()) <1000 || abs(leftEncoder.getPulses())<1000) out=1;
    }

    motors.stopBothMotors();
    wait(2);
    motors.begin();

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

    if(section == TOOLS || section == MID) {
        while(IR.getDistance() > 20 ) {
            //pc.printf("IR %f\r\n", IR.getDistance());
            //pc.printf("third while left %d right %d \r\n", preLeft, preRight);
        }
    } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));

    motors.setMotor0Speed(-.25*127); //right
    motors.setMotor1Speed(-.25*127); //left
    wait_ms(10);
    motors.stopBothMotors();
    wait(2);
    motors.begin();

}

void to_tools_section(float* location, float &current)
{
    wall_follow(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;
    pc.printf("current %f \r\n",current);

    motors.stopBothMotors();

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

}

void from_tools_section(float* location, float &current)
{
    alignWithWall(TOOLS);

    wait_ms(100);

    wall_follow2(LEFT,FORWARD,MID, current);
    current= 78;

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

    if(range < 20) {
        wall_follow2(LEFT,BACKWARD,TOOLS, current);
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(-MAX_SPEED); //right
        motors.setMotor1Speed(-MAX_SPEED); //left
        while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);

        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(MAX_SPEED); //right
        motors.setMotor1Speed(MAX_SPEED); //left
        while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40);
        motors.stopBothMotors();

        wait_ms(500);
        location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        current= location[0];
        leftTurn();
        slightleft();
        overBump(TOOLS);
    } else {
        location[0]= 77;
        leftTurn();
        wait_ms(20);
        overBump(FIRST_WAVE);
    }

    pc.printf("First Wavegap = %f\r\n",location[0]);
}
void mid_section(float* location, float &current, int* direction)
{

    motors.begin();

    if(IR.getDistance() > 20) return;

    alignWithWall(MID);

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

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

    pc.printf("wavegap2 = %f\r\n",location[1]);
    leftTurn();
    overBump(TOOLS);
    // go forward
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.2*127); //right
    motors.setMotor1Speed(0.2*127); //left
    while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
    motors.stopBothMotors();

}

void mid_section2(float* location, float &current, int* direction)
{

    motors.begin();

    if(IR.getDistance() > 20) return;

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

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

    leftTurn();
    overBump(RIGS);
}

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


}

void ledtoggle(void){
    pwm.setPWM(9, 0, 4094);
    wait(2);
    pwm.setPWM(9, 0, 0);
    }