File content as of revision 7:8fb4204f9600:

#include "mbed.h"
#include "Adafruit_PWMServoDriver.h"
#include "ShapeDetect.h"

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

Serial pc(USBTX,USBRX);                     //USB Comm
Adafruit_PWMServoDriver pwm(p28,p27);       //Servo Control PWM
DigitalOut ServoOutputDisable(p8);          //Servo Control Output Enable/Disable
extern Serial lrf;                         //Laser Range Finder    lrf(p13,p14)

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


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

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

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

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

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

/************
Main Variables
*************/
int state = START;
int fire = 0;
int tool_needed = 0;
int shape_detected = 0;

/************
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

    {STORE_POSITION, 90, 0, 0, 0, 90, 90},              // storing position
    {OIL_RIG1, 90, 60, 75, 60, 90, 90},                 // point laser at oilrig1
    {OIL_RIG2, 120, 60, 75, 60, 90, 90},                // point laser at oilrig2
    {OIL_RIG3, 135, 60, 75, 60, 90, 90},                // point laser at oilrig2
    {DRIVE_POSITION_NOTOOL, 135, 60, 75, 60, 90, 90},   // Drive through course
    {TOOL_1, 135, 60, 75, 60, 90, 90},                  // Look over first tool
    {TOOL_2, 135, 60, 75, 60, 90, 90},                  // Look over second tool
    {TOOL_3, 135, 60, 75, 60, 90, 90},                  // Look over third tool
    {DRIVE_POSITION_TOOL, 135, 60, 75, 60, 90, 90},     // Drive with tool loaded
    {ORIENT_TOOL, 135, 60, 75, 60, 90, 90},             // position tool to be inserted
};


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

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


int main()
{

    /*****************
     INITIALIZATIONS
    *******************/
    pc.baud(115200);
    //Laser Range Finder Initialization
    lrf_baudCalibration();

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

    /*******************
    WHILE LOOP FOR TESTING
    ********************/
    while(1) {
        //pc.scanf("%d %d", &servoNum, &servoAngle);
        //setServoPulse(servoNum, servoAngle);

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

    /********************************
    MAIN WHILE LOOP FOR COMPETITION
    *********************************/
    while(1) {
        switch (state) {
                
                /**************************************************
                *           STAGE 0
                *
                *          - START OF THE COMETITION
                *
                **************************************************/
            case START :
                myled1 = 1;
                wait(5);
                myled1 = 0;
                state = OILRIG1_POS;
                break;


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

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

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

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

                //*********************//
                //******* TODO ********//
                //*********************//
                //CODE TO NAVIGATE TO TOOLS

                state = IDENTIFY_TOOLS;
                break;

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

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


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

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

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

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

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

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

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

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

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

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

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

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

                break;
        }
    }


}



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

Servo Functions

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

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

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

}

void servoBegin(void)
{
    pc.printf("Setting Initial Position\n\r");
    setServoPulseNo_delay(0, 90);
    setServoPulseNo_delay(1, 0);
    setServoPulseNo_delay(2, 177);
    setServoPulseNo_delay(3, 0);
    setServoPulseNo_delay(4, 0);
    setServoPulseNo_delay(5, 90);
    setGripper(1);
}

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

}

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

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

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

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

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