File content as of revision 13:529323807361:

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

//States
#define START                       0
#define OILRIG1_POS                 1
#define OILRIG2_POS                 2
#define GOTO_TOOLS1                 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
#define GOTO_TOOLS2                 17



//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
#define PU_TOOL_1               10
#define PU_TOOL_2               11
#define PU_TOOL_3               12
#define PU_TOOL_1_STAB          13
#define PU_TOOL_2_STAB          14
#define PU_TOOL_3_STAB          15

//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     5000
#define OILRIG2_MIN     1000
#define OILRIG3_MAX     5000
#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);
int fire_checker(int rig);


//Navigation Functions
void wall_follow(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 to_tools_section1(float* location, float &current);
void to_tools_section2(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 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);


/************
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, 0, 165, 175, 0},              // storing position
    {OIL_RIG1, 160, 20, 60, 45, 175, 0},                 // point laser at oilrig1
    {OIL_RIG2, 163, 20, 60, 45, 175, 0},                // point laser at oilrig2
    {OIL_RIG3, 130, 90, 90, 52, 175, 0},                // point laser at oilrig2
    {DRIVE_POSITION_NOTOOL, 85, 5, 0, 165, 175, 0},    // Drive through course
    {TOOL_1, 94, 50, 80, 109, 175, 0},                  // Look over first tool
    {TOOL_2, 77, 50, 80, 110, 175, 0},                  // Look over second tool
    {TOOL_3, 57, 50, 80, 109, 175, 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
    {PU_TOOL_1, 98, 50, 82, 118, 90, 0},                // STATIC Pickup tool POSITION
    {PU_TOOL_2, 78, 50, 82, 108, 90, 0},                // STATIC Pickup tool POSITION
    {PU_TOOL_3, 53, 50, 82, 118, 90, 0},                // STATIC Pickup tool POSITION
    {PU_TOOL_1_STAB, 98, 50, 90, 118, 90, 0},           // STAB TOOL 1
    {PU_TOOL_2_STAB, 78, 50, 90, 108, 90, 0},           // STAB TOOL 2
    {PU_TOOL_3_STAB, 53, 50, 90, 118, 90, 0},           // STAB TOOL 3
};


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


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


int main()
{

    /*****************
     INITIALIZATIONS
    *******************/
    float location[3], current=4;
    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);
            distLaser = getDistance();
            pc.printf("Distance %d", distLaser);
    */

    /*
            int shape_alignX_done = 0;
            int shape_alignY_done = 0;


            //pc.scanf("%d", &posNum);

            while(pc.getc() != 'g');
            servoPosition(TOOL_1);
            while(pc.getc() != 'g');
            //shape_detected = shapeDetection();

            ImageToArray(GREYSCALE);
            //clearBounds();
            printImageToFile(GREYSCALE);
            while(1);

            while(shape_alignY_done == 0)  {
                shape_detected = shapeDetection();
                pc.printf("\nY movement\n");
                if(get_com_y() >= 80) {
                    setServoPulse(1, Arm_Table[TOOL_1].base_arm-=2);
                    setServoPulse(2, Arm_Table[TOOL_1].big_arm-=1);
                    //setServoPulse(3, Arm_Table[TOOL_1].claw_arm+=2);
                } else if(get_com_y() > 70 && get_com_y() < 90) {
                    setServoPulse(1, Arm_Table[TOOL_1].base_arm-=1);
                    setServoPulse(2, Arm_Table[TOOL_1].big_arm-=1);
                    //setServoPulse(3, Arm_Table[TOOL_1].claw_arm+=1);
                } else if(get_com_y() <= 35 ) {
                    setServoPulse(1, Arm_Table[TOOL_1].base_arm+=2);
                    setServoPulse(2, Arm_Table[TOOL_1].big_arm+=1);
                    //setServoPulse(3, Arm_Table[TOOL_1].claw_arm-=2);
                } else if(get_com_y() < 50 && get_com_y() > 20) {
                    setServoPulse(1, Arm_Table[TOOL_1].base_arm+=1);
                    setServoPulse(2, Arm_Table[TOOL_1].big_arm+=1);
                    //setServoPulse(3, Arm_Table[TOOL_1].claw_arm-=1);
                } else {
                    shape_alignY_done = 1;
                }
                }

                while( shape_alignX_done == 0){
                shape_detected = shapeDetection();
                pc.printf("\nX movement\n");
                if(get_com_x() > 95) {
                    Arm_Table[TOOL_1].base_rotate+=1;
                    setServoPulse(0, Arm_Table[TOOL_1].base_rotate);
                    setServoPulse(0, Arm_Table[TOOL_1].base_rotate-1);
                    setServoPulse(0, Arm_Table[TOOL_1].base_rotate+1);


                } else if(get_com_x() < 65) {
                    Arm_Table[TOOL_1].base_rotate-=1;
                    setServoPulse(0, Arm_Table[TOOL_1].base_rotate);
                    setServoPulse(0, Arm_Table[TOOL_1].base_rotate-1);
                    setServoPulse(0, Arm_Table[TOOL_1].base_rotate+1);

                } else {
                    shape_alignX_done = 1;
                }


            }
            printImageToFile(BINARY);*/

    // }





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

        switch (state) {

                /**************************************************
                *           STAGE 0
                *
                *          - START OF THE COMETITION
                *
                **************************************************/
            case START :
                myled1 = 1;
                while(pc.getc() != 'g'); //waits for the letter g before starting program
                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);
                wait(3); //wait for servo to settle before laser distance

                fire = fire_checker(OIL_RIG1);  //determines if oil rig is on fire

                //determines what tool is needed
                if (fire == 1) {
                    pc.printf("FIRE FOUND!!!!!!!!\n\r");
                    tool_needed = SQUARE;
                    state = GOTO_TOOLS1;
                } else {
                    pc.printf("XXXXXX   FIRE NOT FOUND    XXXXXX");
                    state = OILRIG2_POS;
                }
                break;

            case OILRIG2_POS:

                servoPosition(DRIVE_POSITION_NOTOOL);
                wait(3);                                //wait for servos to settle

                to_tools_section2(location, current);   // moves to second rig

                servoPosition(OIL_RIG2);    //position arm to point at first oilrig
                wait(3);

                fire = fire_checker(OIL_RIG2);
                if (fire == 1) {
                    pc.printf("FIRE FOUND!!!!!!!!");
                    tool_needed = TRIANGLE;
                    state = GOTO_TOOLS2;
                } else {
                    pc.printf("XXXXXX   FIRE NOT FOUND    XXXXXX");
                    tool_needed = CIRCLE;
                    state = GOTO_TOOLS2;
                }
                break;

                /**************************************************
                *           STAGE 2
                *
                *          - TRAVEL TO TOOLS
                *
                **************************************************/
            case GOTO_TOOLS1:

                servoPosition(DRIVE_POSITION_NOTOOL);
                wait(3);                                //wait for servos to settle
                to_tools_section1(location, current);
                state = IDENTIFY_TOOLS;
                break;

            case GOTO_TOOLS2:

                servoPosition(DRIVE_POSITION_NOTOOL);
                wait(3);                                //wait for servos to settle

                slightMove(FORWARD,3100);
                current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

                state = IDENTIFY_TOOLS;
                break;



                /**************************************************
                *           STAGE 3
                *
                *           - Determine order of tools
                *           - Aquire appropriate tool
                *
                **************************************************/
            case IDENTIFY_TOOLS:

                servoPosition(TOOL_2);              //arm/camera looks over tool
                wait(5);                            //wait for servos to settle

                //shape_detected = shapeDetection();  //determines the shape
                //clearBounds();
                //printImageToFile(BINARY);

                while( shape_alignX_done == 0) {
                    shape_detected = shapeDetection();
                    pc.printf("X - Adjust to center tool\n\r");
                    if(get_com_x() > 95) {
                        Arm_Table[TOOL_1].base_rotate+=1;

                    } else if(get_com_x() < 65) {
                        Arm_Table[TOOL_1].base_rotate-=1;

                    } else {
                        shape_alignX_done = 1;
                    }


                    //either goes to aquire the tool or look at the next shape
                    if(shape_detected == tool_needed) {
                        state = AQUIRE_TOOL2;
                    } else {
                        servoPosition(TOOL_1);
                        wait(3);                        //wait for servos to settle

                        shape_alignX_done = 0;
                        while( shape_alignX_done == 0) {

                            shape_detected = shapeDetection();
                            pc.printf("X - Adjust to center tool\n\r");
                            if(get_com_x() > 95) {
                                Arm_Table[TOOL_1].base_rotate+=1;

                            } else if(get_com_x() < 65) {
                                Arm_Table[TOOL_1].base_rotate-=1;

                            } else {
                                shape_alignX_done = 1;
                            }

                            if (shape_detected == tool_needed) {
                                state = AQUIRE_TOOL;
                            } else {
                                servoPosition(TOOL_3);
                                wait(3);                            //wait for servos to settle
                                shape_detected = shapeDetection();
                                pc.printf("X - Adjust to center tool\n\r");
                                if(get_com_x() > 95) {
                                    Arm_Table[TOOL_1].base_rotate+=1;

                                } else if(get_com_x() < 65) {
                                    Arm_Table[TOOL_1].base_rotate-=1;

                                } else {
                                    shape_alignX_done = 1;
                                }
                                
                                state = AQUIRE_TOOL3;
                            }
                        }
                        
                        while(1);
                        break;
                        
                    case AQUIRE_TOOL1:

                        servoPosition(PU_TOOL_1_STAB);
                        wait(2);

                        servoPosition(DRIVE_POSITION_TOOL);     //arm position for driving with the tool
                        state  = NAVIGATE_WAVES_ROW1;
                        break;
                        
                    case AQUIRE_TOOL2:
                        servoPosition(PU_TOOL_2_STAB);
                        wait(2);

                        servoPosition(DRIVE_POSITION_TOOL);     //arm position for driving with the tool
                        state  = NAVIGATE_WAVES_ROW1;
                        break;

                    case AQUIRE_TOOL3:
                        servoPosition(PU_TOOL_3_STAB);
                        wait(2);


                        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("END: pulse: %d,  angle: %d\n\r", 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 Servo Position\n\r");
            servoPosition(STORE_POSITION);
        }

        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 servoPosition(int set) {
            //moves to current position
            setServoPulse(3, Arm_Table[set].claw_arm);
            setServoPulse(2, Arm_Table[set].big_arm);
            setServoPulse(1, Arm_Table[set].base_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<10; i++) {
                        distLaser = getDistance();
                        pc.printf("L DISTANCE: %d \n\r", distLaser);
                        if ((distLaser < OILRIG1_MAX)
                                && (distLaser > OILRIG1_MIN)) {
                            fire_detected++;
                        } else {
                            fire_not_detected++;
                        }
                    }
                    break;
                case  2:
                    for (int i = 0; i<10; i++) {
                        distLaser = getDistance();
                        pc.printf("L DISTANCE: %d \n\r", distLaser);
                        if ((distLaser < OILRIG2_MAX)
                                && (distLaser > OILRIG2_MIN)) {
                            fire_detected++;
                        } else {
                            fire_not_detected++;
                        }
                    }
                    break;

            }

            if (fire_detected > 0) {
                return 1;
            } else {
                return 0;
            }
        }

        void errFunction(void) {
            //Nothing
        }
        void wall_follow(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< 66.5) {
                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.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);
        }

        /* MODIFIED WALL_FOLLOW FOR NAVIGATION */

        void wall_follow2(int side, int direction, int section, float location, int rig) {
            int dir=1, limit=86, 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.25*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.getIRDistance() >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.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {

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

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

                    if(IR.getIRDistance() > 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 to_tools_section1(float* location, float &current) {
            slightMove(FORWARD,6500);
            current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

        }

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

        }

        void from_tools_section(float* location, float &current) {


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