File content as of revision 30:db07aea6d119:

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


#define PI 3.14159



/* 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
#define RETURN_TO_START             18



//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 INSERT_TOOL_1           13


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


//Oil Rig distance thresholds
#define OILRIG1_MAX     1800
#define OILRIG1_MIN     1000
#define OILRIG2_MAX     1800
#define OILRIG2_MIN     1000
#define OILRIG3_MAX     1800
#define OILRIG3_MIN     1000

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

#define X_CENTER    80
#define Y_CENTER    60


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

void errFunction(void);
bool cRc;

Serial pc(USBTX,USBRX);                     //USB Comm
Adafruit_PWMServoDriver pwm(p28,p27);       //pwm(SDA,SCL) - Servo Control PWM
extern Serial lrf;                         //Laser Range Finder    lrf(p13,p14)
//Hardware Initialization
//Serial bt(p13,p14);
HCSR04 rangeFinderFront( PIN_TRIGGERL, PIN_ECHOL );
HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
PID pid1(25.0,0.0,0.0,0.0001);
PololuQik2 motors(p9, p10, p8, p11, &errFunction, cRc);
QEI rightEncoder(p22,p21,NC,PPR,QEI::X4_ENCODING);
QEI leftEncoder(p23,p24,NC,PPR,QEI::X4_ENCODING);
Sharp IRLeftFront(p18);
Sharp IRLeftBack(p17);
Sharp IRRightFront(p20);
Sharp IRRightBack(p15);
Sharp IRFrontL(p19);
Sharp IRFrontR(p16);
//InterruptIn encoder(p29);




/***************
local servo functions
****************/
void servoBegin(void);
void initServoDriver(void);
void setServoPulse(uint8_t n, int angle);
void setServoPulse2(uint8_t n, float angle); //float precision
void setServoPulseNo_delay(uint8_t n, int angle);
void servoPosition(int set);
int fire_checker(int rig);
int button_start = 0;


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

void testSensors(void);
float normd(int* pop, int count, int threshold);
int Xadjust(int tool);

extern "C" void mbed_reset();

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


/************
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, 170, 60, 0},              // storing position
    {OIL_RIG1, 164, 20, 60, 100, 175, 0},                 // point laser at oilrig1
    {OIL_RIG2, 164, 20, 60, 100, 175, 0},                // point laser at oilrig2
    {OIL_RIG3, 130, 90, 90, 100, 175, 0},                // NOT USED!!!!!    point laser at oilrig2
    {DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 60, 0},    // Drive through course
    {TOOL_1, 101, 50, 80, 133, 60, 0},                  // Look over first tool
    {TOOL_2, 82, 50, 80, 133, 60, 0},                  // Look over second tool
    {TOOL_3, 62, 50, 80, 132, 60, 0},                  // Look over third tool
    {DRIVE_POSITION_TOOL, 85, 10, 0, 165, 140, 120},     // Drive with tool loaded
    {ORIENT_TOOL, 135, 60, 75, 60, 90, 90},             // position tool to be inserted
    {PU_TOOL_1, 99, 46, 78, 110, 170, 0},               // STATIC Pickup tool POSITION
    {PU_TOOL_2, 76, 47, 80, 112, 170, 0},                // STATIC Pickup tool POSITION
    {PU_TOOL_3, 59, 44, 76, 110, 170, 0},                // STATIC Pickup tool POSITION
    {INSERT_TOOL_1, 170, 50, 127, 52, 140, 120},           // InsertToolIntoRig

};


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


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

void button_int(void)
{
    if(!button_start) {
        button_start = 1;
        wait(1.0);
    } else {
        button_start = 0;
        mbed_reset();
    }
    return;
}



int main()
{


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

    int pu, num, input;


    pc.baud(115200);

    //Laser Range Finder Initialization
    lrf_baudCalibration();

    motors.begin();

    startBtn.rise(&button_int);
 //////////////////////////// TEST SERVOS /////////////////////////
    //testSensors();
    
    motors.begin();
    //wall_follow2(LEFT, FORWARD, TOOLS, 10, 1);
    //while(1);

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

    /********************************
    MAIN WHILE LOOP FOR COMPETITION
    *********************************/


    while(1) {
        //if(1) {
        if(button_start == 1) {
            
            /*
            pc.printf("Servo Test");
            while(1) {
                pc.scanf("%d %d", &servoNum, &servoAngle);
                if(servoAngle > 175) {
                    servoAngle = 175;
                }
                if(servoNum > 5 ) {
                    servoNum = 0;
                    servoAngle = 90;
                }
                setServoPulse(servoNum, servoAngle);

            };*/


            switch (state) {

                    /**************************************************
                    *           STAGE 0
                    *
                    *          - START OF THE COMETITION
                    *
                    **************************************************/
                case START :
                    myled1 = 1;

                    current=75;
                    state  = NAVIGATE_WAVES_ROW1;
                   //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:

                    setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
                    setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
                    setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
                    setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
                    setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
                    setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
                    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:
                    setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
                    setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
                    setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
                    setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
                    setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
                    setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
                    wait(3);                                //wait for servos to settle

                    to_tools_section1(location, current);
                    state = IDENTIFY_TOOLS;
                    break;

                case GOTO_TOOLS2:

                    setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
                    setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
                    setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
                    setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
                    setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
                    setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
                    wait(3);                                //wait for servos to settle

                    slightMove(FORWARD,3250);
                    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:

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

                    //shape_detected = shapeDetection();  //determines the shape
                    //clearBounds();
                    //printImageToFile(BINARY);
                    shape_alignX_done = 0;
                    shape_alignY_done = 0;
                    /*
                    while( shape_alignY_done == 0) {
                        wait(1);
                        shape_detected = shapeDetection();

                        pc.printf("Y - Adjust to center tool\n\r");

                        if(get_com_y() < 50) {
                            wait(1);
                            slightMove(FORWARD,25);
                            current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

                        } else if(get_com_y() > 70) {
                            wait(1);
                            slightMove(BACKWARD,25);
                            current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

                        } else {
                            shape_alignY_done = 1;
                        }
                    }*/

                    // aveArea = sumArea/count;
                    //printImageToFile(BINARY);
                    //either goes to aquire the tool or look at the next shape

//****************//if(Xadjust(TOOL_2) == tool_needed) {
                    if(Xadjust(TOOL_2) == 162) {
                        //printImageToFile(BINARY);
                        state = AQUIRE_TOOL2;
                        break;
                    } else {
                        //printImageToFile(BINARY);
                        slightMove(FORWARD,70);
                        current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
                        servoPosition(TOOL_1);
                        wait(5);                        //wait for servos to settle
                        shape_alignX_done = 0;
                        shape_alignY_done = 0;
                        /*
                        while( shape_alignY_done == 0) {
                            wait(1);
                            shape_detected = shapeDetection();

                            pc.printf("Y - Adjust to center tool\n\r");

                            if(get_com_y() < 50) {
                                wait(1);
                                slightMove(FORWARD,25);
                                current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

                            } else if(get_com_y() > 70) {
                                wait(1);
                                slightMove(BACKWARD,25);
                                current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

                            } else {
                                shape_alignY_done = 1;
                            }
                        }*/
//****************//    if (Xadjust(TOOL_1) == tool_needed) {
                        if (Xadjust(TOOL_1) == 169) {
                            state = AQUIRE_TOOL1;
                            break;
                        } else {
                            servoPosition(TOOL_3);
                            wait(3);                            //wait for servos to settle
                            state = AQUIRE_TOOL3;
                        }
                    }

                    break;

                case AQUIRE_TOOL1:

                    servoPosition(PU_TOOL_1);
                    setServoPulse(4, 175);
                    wait(5);
                    setServoPulse(0, Arm_Table[PU_TOOL_1].base_rotate - 1);
                    wait(1);
                    setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 5);
                    wait(1);
                    setServoPulse(4, 175);
                    wait(2);
                    setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 6);
                    wait(.5);
                    setServoPulse(5, 100);
                    wait(.5);
                    setServoPulse(5, 00);
                    setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 10);
                    wait(1);
                    setServoPulse(5, 120);
                    wait(1);
                    setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm);
                    wait(2);

                    setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
                    setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
                    setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
                    setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
                    setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
                    setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);


                    state  = NAVIGATE_WAVES_ROW1;
                    break;

                case AQUIRE_TOOL2:
                    servoPosition(PU_TOOL_2);
                    setServoPulse(4, 175);
                    wait(5);
                    setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate - 1);
                    wait(1);
                    setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2);
                    wait(1);
                    setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 2);
                    wait(1);
                    setServoPulse(4, 175);
                    wait(2);
                    setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6);
                    wait(2);
                    setServoPulse(5, 100);
                    wait(2);
                    setServoPulse(5, 00);
                    setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 10);
                    wait(2);
                    setServoPulse(5, 120);
                    wait(2);
                    setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm);

                    wait(2);

                    setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
                    setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
                    setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
                    setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
                    setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
                    setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);


                    state  = NAVIGATE_WAVES_ROW1;
                    break;

                case AQUIRE_TOOL3:
                    /*
                        while( shape_alignY_done == 0) {
                            wait(1);

                            servoPosition(PU_TOOL_3);
                            shape_detected = shapeDetection();
                            wait(2);

                            pc.printf("Y - Adjust to center tool\n\r");

                            if(get_com_y() < 50) {
                                wait(1);
                                slightMove(FORWARD,25);
                                current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

                            } else if(get_com_y() > 70) {
                                wait(1);
                                slightMove(BACKWARD,25);
                                current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;

                            } else {
                                shape_alignY_done = 1;
                            }
                        }

                    */
                    Xadjust(TOOL_3);

                    setServoPulse(4, 175);
                    wait(5);
                    setServoPulse(0, Arm_Table[PU_TOOL_3].base_rotate - 1);
                    wait(1);
                    setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2);
                    wait(1);
                    setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 3);
                    wait(1);
                    setServoPulse(4, 175);
                    wait(2);
                    setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 9);
                    wait(.5);
                    setServoPulse(5, 100);
                    wait(.5);
                    setServoPulse(5, 00);
                    setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 10);
                    wait(1);
                    setServoPulse(5, 120);
                    wait(1);
                    setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm);

                    wait(2);

                    setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
                    setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
                    setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
                    setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
                    setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
                    setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);

                    state  = NAVIGATE_WAVES_ROW1;
                    break;


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

                case NAVIGATE_WAVES_ROW1:
                    from_tools_section(location,current);

                    mid_section(location, current, direction);

                    state = NAVIGATE_WAVES_ROW2;
                    break;

                case NAVIGATE_WAVES_ROW2:
                    mid_section2(location, current, direction);
                    state = NAVIGATE_WAVES_ROW3;
                    break;

                case NAVIGATE_WAVES_ROW3:
                    shape_detected = 1;
                    if(tool_needed == 1) {
                        rig_section(location, current, direction, 1);
                        //state = NAVIGATE_TO_SQUARE_RIG;
                        state = RETURN_TO_START;
                    } else if(tool_needed == 2) {
                        rig_section(location, current, direction, 2);
                        //state = NAVIGATE_TO_TRIANGLE_RIG;
                        state = RETURN_TO_START;
                    } else {
                        rig_section(location, current, direction, 3);
                        //state = NAVIGATE_TO_CIRCLE_RIG;
                        state = RETURN_TO_START;

                    }
                    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
                    *
                    *           - Return to start zone
                    *
                    **************************************************/
                case RETURN_TO_START:
                    wait(3);
                    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);
                    state = END;
                    break;

                    /**************************************************
                    *           STAGE 9
                    *
                    *           - 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;
            }
        }   // End while loop

    }   // End if for start button


}   // main loop



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

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");
    setServoPulseNo_delay(3, Arm_Table[STORE_POSITION].claw_arm);
    setServoPulseNo_delay(2, Arm_Table[STORE_POSITION].big_arm);
    wait(2);
    setServoPulseNo_delay(1, Arm_Table[STORE_POSITION].base_arm);
    setServoPulseNo_delay(0, Arm_Table[STORE_POSITION].base_rotate);
    setServoPulseNo_delay(4, Arm_Table[STORE_POSITION].claw_rotate);
    setServoPulseNo_delay(5, Arm_Table[STORE_POSITION].claw_open);
}

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 setServoPulse2(uint8_t n, float angle)
{
    float pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE -  MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
    float pulselength = 20000;   // 10,000 us per second
    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)
{
    pc.printf("\n\nERROR: %d", motors.getError() );

}


/* MODIFIED WALL_FOLLOW FOR NAVIGATION */

void wall_follow2(int side, int direction, int section, float location, int rig)
{
    int dir=1, limit=78, lowlim=4;
    float set=28, loc=0, Rigloc=0, location_cal=0;

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

    pid1.reset();

    if(section == TOOLS) {
        limit = 100;
        lowlim=10;
    } else if(section == RIGS) set = 18;
    else if(section == MID2) limit =80;

    if(direction == BACKWARD) {
        dir=-1;
        limit = 100;
    } else if(direction == FORWARD) lowlim=-20;

    if(location <4) limit=80;

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

    //pc.printf("dir*loc+location %f\r\n",dir*loc + location );
    //pc.printf("limit %d \r\n", limit);
    if((section == RIGS || section == RETURN) && side == LEFT)location_cal= -1*dir*loc + location;
    else location_cal= dir*loc + location;

    while((location_cal <= limit) && (location_cal >= lowlim)) {

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

        if((section == RIGS || section == RETURN) && side == LEFT)location_cal= -1*dir*loc + location;
        else location_cal= dir*loc + location;

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

        if(side== LEFT) {
            range = IRLeftFront.getIRDistance();
            range2= IRLeftBack.getIRDistance();
        } else {
            range = IRRightFront.getIRDistance();
            range2= IRRightBack.getIRDistance();
        }

        if(section == RIGS) {// check for the rigs on opposite side
            if(side == LEFT) {
                range = IRRightFront.getIRDistance();
                range2= IRRightBack.getIRDistance();

            } else {
                range = IRLeftFront.getIRDistance();
                range2= IRLeftBack.getIRDistance();
            }

            if(range2 < 40 || range < 40 ) {
                if( abs(location_cal - Rigloc) < 10) {
                    //STOP
                    motors.stopBothMotors(127);
                    break;
                }
            }
        }
        
        if(range > 40 && range2 > 40 && section != RIGS) {
                pc.printf("RANGE %f \tRANGE3 %f\r\n",range,range3);
                //STOP
                motors.stopBothMotors(127);
                break;

        } else {
            if(direction == FORWARD) pid1.setProcessValue(range);
            else pid1.setProcessValue(range2);

            if(abs(range - range2) < 10){ // does it see a wavegap? 
                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.stopBothMotors(127);
}


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(127);
        wait_ms(300);
        return;
        /*
               rangeFinderRight.startMeas();
               wait_ms(20);
               rangeFinderRight.getMeas(range);

               if(range>15){
                   // turning left
                   leftEncoder.reset();
                   rightEncoder.reset();
                   motors.setMotor0Speed(-MAX_SPEED); //right
                   motors.setMotor1Speed(MAX_SPEED); //left
                   while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50);
                   motors.stopBothMotors(127);
                   return;
               }
        */
        // turning left
        //motors.setMotor0Speed(0.9*MAX_SPEED); //right
        //motors.setMotor1Speed(-0.9*MAX_SPEED); //left

    } else if(section == RIGS) {

        // turn left 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())<500);
        motors.stopBothMotors(0);
        wait_ms(200);

        //go backwards away form 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(127);
        wait(2);

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

        motors.stopBothMotors(127);

    } else if(section == MID2) {

        // check distance to wall

        if(IRLeftFront.getIRDistance() > 4) {

            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()) < 150 || abs(rightEncoder.getPulses()) < 150);

            // turn left towards wall
            leftEncoder.reset();
            rightEncoder.reset();
            motors.setMotor0Speed(0.4*127); //right
            motors.setMotor1Speed(-0.4*127); //left
            while(abs(rightEncoder.getPulses()) < 65 || abs(leftEncoder.getPulses()) < 65);
            motors.stopBothMotors(127);

            slightMove(FORWARD,100);
        }
        return;

    } else { // MID
        //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 == 10) { // CURENTLY NOT USED (WAS RIGS)
                rangeFinderRight.startMeas();
                wait_ms(20);
                rangeFinderRight.getMeas(range);
            } else {
                range = IRLeftFront.getIRDistance();
            }
            //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())<800 || abs(rightEncoder.getPulses())<800);
    motors.stopBothMotors(127);
    wait_ms(300);

    if(IRLeftFront.getIRDistance() > 35 && IRLeftBack.getIRDistance() > 35) return;
    // align with wave
    while(IRLeftFront.getIRDistance() > 35) {
        pc.printf("front sensor sees gap\r\n");
        motors.setMotor0Speed(-0.1*127);//right
        motors.setMotor1Speed(-0.1*127);//left
    }
    while(IRLeftBack.getIRDistance() >35 ) {
        pc.printf("back sensor sees gap\r\n");
        motors.setMotor0Speed(0.1*127);//right
        motors.setMotor1Speed(0.1*127);//left
    }
    motors.stopBothMotors(127);
    wait_ms(300);

    while(IRLeftFront.getIRDistance() - IRLeftBack.getIRDistance() > 0.5) {
        pc.printf("turn left\r\n");
        motors.setMotor0Speed(0.3*127);// right
        motors.setMotor1Speed(-0.3*127);// left
    }
    while(IRLeftFront.getIRDistance() - IRLeftBack.getIRDistance() < -0.5) {
        pc.printf("turn right\r\n");
        motors.setMotor0Speed(-0.3*127);//right
        motors.setMotor1Speed(0.3*127);// left
    }

    motors.stopBothMotors(127);
    wait_ms(300);

}

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())<1075 || rightEncoder.getPulses()<1075);
    motors.stopBothMotors(127);
    wait_ms(300);

    if(IRLeftFront.getIRDistance() > 35 && IRLeftBack.getIRDistance() > 35) return;
    // align with wave
    while(IRLeftFront.getIRDistance() > 35) {
        pc.printf("front sensor sees gap\r\n");
        motors.setMotor0Speed(-0.1*127);//right
        motors.setMotor1Speed(-0.1*127);//left
    }
    while(IRLeftBack.getIRDistance() >35 ) {
        pc.printf("back sensor sees gap\r\n");
        motors.setMotor0Speed(0.1*127);//right
        motors.setMotor1Speed(0.1*127);//left
    }
    motors.stopBothMotors(127);
    wait_ms(300);

    while(IRLeftFront.getIRDistance() - IRLeftBack.getIRDistance() > 0.5) {
        pc.printf("turn left\r\n");
        motors.setMotor0Speed(0.3*127);// right
        motors.setMotor1Speed(-0.3*127);// left
    }
    while(IRLeftFront.getIRDistance() - IRLeftBack.getIRDistance() < -0.5) {
        pc.printf("turn right\r\n");
        motors.setMotor0Speed(-0.3*127);//right
        motors.setMotor1Speed(0.3*127);// left
    }

    motors.stopBothMotors(127);
    wait_ms(300);

}

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

void slightright(void)
{

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

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

void overBump(int section)
{
    float avg=0;
    int i;

    // first set
    motors.setMotor1Speed(0.1*127);//left
    motors.setMotor0Speed(0.1*127);//right
    wait_ms(100);
    while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5 );

    while(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5 ) {
        pc.printf("left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current());
        motors.setMotor1Speed(0.3*127);//left
        motors.setMotor0Speed(0.3*127);//right
        wait_ms(220);
        motors.stopBothMotors(127);
        wait_ms(10);
    }

    motors.setMotor1Speed(0.1*127);//left
    motors.setMotor0Speed(0.1*127);//right
    wait_ms(100);
    while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5 );

    motors.setMotor1Speed(0.3*127);//left
    motors.setMotor0Speed(0.3*127);//right
    // second set
    wait_ms(200);
    motors.stopBothMotors(127);
    wait_ms(500);
    
    if(section!= RIGS) {
        range = 0;

        do {
            pc.printf("front left %f \t front right %f\r\n", IRFrontL.getIRDistance(),IRFrontR.getIRDistance());
            if(IRFrontL.getIRDistance() < 20 && IRFrontR.getIRDistance() < 20) {
                motors.stopBothMotors(127);
                wait_ms(200);
                break;
            }

            motors.setMotor1Speed(0.3*127);//left
            motors.setMotor0Speed(0.3*127);//right
            wait_ms(220);
            motors.stopBothMotors(127);
            wait_ms(500);

        } while(IRFrontL.getIRDistance() > 20 && IRFrontR.getIRDistance() > 20);

        motors.setMotor1Speed(0.1*127);//left
        motors.setMotor0Speed(0.1*127);//right
        wait_ms(100);
        while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5);
        motors.stopBothMotors(127);
        wait_ms(200);

    } else {
        leftEncoder.reset();
        rightEncoder.reset();


        while(leftEncoder.getPulses()<500 || rightEncoder.getPulses()<500) {
            pc.printf("left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current());
            motors.setMotor1Speed(0.1*127);//left
            motors.setMotor0Speed(0.1*127);//right
            wait_ms(220);
            
            if(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5) {
                pc.printf("fast left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current());
                motors.setMotor1Speed(0.35*127);//left
                motors.setMotor0Speed(0.35*127);//right
                wait_ms(220);
                motors.stopBothMotors(127);
                wait_ms(500);
                leftEncoder.reset();
                rightEncoder.reset();
            }

        }
        motors.stopBothMotors(127);
        wait_ms(200);
    }




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

}


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

}

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

}

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

    pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());

    if(IRLeftFront.getIRDistance() < 40 || IRLeftBack.getIRDistance() < 40) {
        wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
        pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
        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
        //slightMove(BACKWARD,200);

        wait_ms(100);
        leftTurn();
        overBump(TOOLS);
    } else {
        pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
        //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)
{
    wait_ms(500);

    if(IRFrontL.getIRDistance() > 40 && IRFrontR.getIRDistance() > 40) {
        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);
        direction[0]= STRAIGHT;
        overBump(MID);
        return;
    }
    //pc.printf("before align with wall \r\n");
    //alignWithWall(MID);
    //current-=4;
    //wait_ms(200);

    //if(current > 20){
    //alignWithWall(MID2);
    //current-=4;
    //}
    rightTurn();

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

    if(IRLeftFront.getIRDistance() > 40 && IRLeftBack.getIRDistance() > 40) {
        wait(1);
        direction[0]= RIGHT;
        location[1]= current;
        wait_ms(300);
        slightMove(FORWARD,200);
        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, 75);
            current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        } else slightMove(BACKWARD,75);

    }

    wait_ms(200);
    //pc.printf("wavegap2 = %f\r\n",location[1]);
    //left turn
    motors.begin();
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.5*127);// right
    motors.setMotor1Speed(-0.5*127);// left
    while(abs(leftEncoder.getPulses())<110 || rightEncoder.getPulses()<1100);
    motors.stopBothMotors(127);

    wait_ms(100);

    overBump(MID);
}

void mid_section2(float* location, float &current, int* direction)
{
    //pc.printf("mid section 2\r\n");
    wait_ms(500);

    if(IRFrontL.getIRDistance() > 40 && IRFrontR.getIRDistance() > 40) {
        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);
        direction[1]= STRAIGHT;
        overBump(RIGS);
        return;
    }

    //alignWithWall(MID);
    wait_ms(100);

    rightTurn();
    wait_ms(100);


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

    if(IRLeftFront.getIRDistance() > 40 && IRLeftBack.getIRDistance() > 40) {
        direction[1]= RIGHT;
        location[2]= current;
        slightMove(FORWARD,150);
        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(BACKWARD,100);
    }

    //LEFT turn
    motors.begin();
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.5*127);// right
    motors.setMotor1Speed(-0.5*127);// left
    while(abs(leftEncoder.getPulses())<1115 || rightEncoder.getPulses()<1115);
    motors.stopBothMotors(127);

    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;

    // Slight forward for turn
    slightMove(FORWARD,150);
    wait_ms(100);
    leftTurn();
    //slightright();


    if(current > loc) {
        //pc.printf("RIG section %f\r\n",current);
        wall_follow2(LEFT, FORWARD, 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(LEFT, BACKWARD, RIGS, current, rig);
        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    }
    wait(1);
    alignWithWall(MID2);
    current+=4;
    wall_follow2(LEFT, FORWARD, RIGS, current, rig);
    current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    slightMove(FORWARD, 75);
    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, TOOLS, location[0], 0);
    }
    motors.stopBothMotors(127);

}

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

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

void rig_section_return(float* location, float &current, int* direction)
{
    alignWithWall(RIGS);

    if(location[2] > current) {
        wall_follow2(LEFT, BACKWARD, RETURN, current,0);
        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        wait_ms(500);
        //slightMove(FORWARD,50);
    } else {
        wall_follow2(LEFT, FORWARD, RETURN, current,0);
        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    }

    // LEFT TURN
    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()<110);

    motors.stopBothMotors(127);
    overBump(MID2);
}




int Xadjust(int tool)
{
    int areaArray[10];
    float C, T, S;
    for(int i = 0; i < 5; i++) {
        areaArray[i] = shapeDetection();
        wait(2);
        if(get_com_x() > X_CENTER ) {
            deltaX = get_com_x()-X_CENTER;
            setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) );
            Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
        }
        if(get_com_x() < X_CENTER) {
            deltaX = get_com_x()-X_CENTER;
            setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) );
            Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
        }


    }

    C = normd(areaArray, 10, CIRCLE_AREA);
    // S = normd(areaArray, 10, SQUARE_AREA);
    // T = normd(areaArray, 10, TRIANGLE_AREA);

    if((C < SQUARE_AREA) && (C > CIRCLE_AREA)) {
        pc.printf("\n\nCIRCLE DETECTED\n\r");
        return CIRCLE;
    } else if( ( C > SQUARE_AREA) ) {
        pc.printf("\n\nSQUARE DETECTED\n\r");
        return SQUARE;
    } else {
        pc.printf("\n\nTRIANGLE DETECTED\n\r");
        return TRIANGLE;
    }


    /*
        if((C < S) && (C < T)) {
            pc.printf("\n\nCIRCLE DETECTED\n\r");
            return CIRCLE;
        } else if( ( S<C ) && ( S<T ) ) {
            pc.printf("\n\nSQUARE DETECTED\n\r");
            return SQUARE;
        } else {
            pc.printf("\n\nTRIANGLE DETECTED\n\r");
            return TRIANGLE;
        }*/
}

float normd(int* pop, int count, int threshold)
{
    int i = 0;
    float mean=0, std=0;
    for(i=0; i<count; i++) {
        mean += pop[i];
    }
    mean /= (float)count;
    pc.printf("\n\nMean: %f\n\r", mean);

    for(i=0; i<count; i++) {
        std += pow(((float)pop[i]-mean),2);
    }
    std /= (float)count;
    std = sqrt(std);
    //pc.printf("\n\nStd: %f\n\r", std);

    //pc.printf("\n\nNorm: %f\n\r", (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2))));


    //return abs(mean - threshold);
    return mean;
    //return (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2)));

}

void testSensors(void)
{
    float range, range2;
    

    while(1) {
        
        //pc.printf("us front %f \t us right %f \t IR front %f \t IR back %f\r\n", range, range2, IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
        
        pc.printf("IR LEFT front %f \t",IRLeftFront.getIRDistance());
        pc.printf("IR left back %f \t",IRLeftBack.getIRDistance());
        
        pc.printf("IR right front %f \t",IRRightFront.getIRDistance());
        pc.printf("IR right back %f \t",IRRightBack.getIRDistance());

        pc.printf("IR front left %f \t",IRFrontL.getIRDistance());
        pc.printf("IR front right %f \r\n",IRFrontR.getIRDistance());
        wait_ms(20);
    }
}