File content as of revision 3:b7b4780a7f6e:

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

Serial pc(USBTX,USBRX);
Adafruit_PWMServoDriver pwm(p28,p27);
DigitalOut ServoOutputDisable(p8);
extern Serial lrf;

//Servo Positions
#define STORE_POSITION  0
#define OIL_RIG1        1
#define OIL_RIG2        2
#define OIL_RIG3        3
#define IMG_SHAPE_1     4
#define IMG_SHAPE_2     5
#define IMG_SHAPE_3     6
#define GRASP_SHAPE_1   7
#define GRASP_SHAPE_2   8
#define GRASP_SHAPE_3   9
#define INSERT_TOOL_1   10
#define INSERT_TOOL_2   11
#define INSERT_TOOL_3   11

#define MIN_SERVO_PULSE     900
#define MAX_SERVO_PULSE     2100
#define SERVO_MAX_ANGLE     180


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

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

typedef struct {
    int arm_action;
    int base_rotate;
    int base_arm;
    int lil_arm;
    int big_arm;
    int claw_arm;
    int claw_rotate;
    int claw_open;
} Coord;

Coord Arm_Table[] = {
    // POSITION ODER:
    // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open

    {STORE_POSITION, 900, 900, 900, 900, 900, 900, 900}, // storing position
    {OIL_RIG1, 1500, 1400, 1900, 900, 900, 0, 0}, // point laser at oilrig2

};


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






int main()
{

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

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

    while(1) {
        pc.printf("PICK A TEST TO PERFORM\n");
        pc.printf("1) Distance Reading\n");
        pc.printf("2) Shape Detection\n");
        pc.printf("3) Servo Control\n");
        pc.printf("4) Motor Control\n");

        pc.scanf("%d", &testVal);

        switch (testVal) {
            case 1:
                pc.printf("Distance = %d\n", getDistance());
                break;
            case 2:
                shape =  shapeDetection_mass();
                printImageToFile(BINARY);
                break;
            case 3:
                servoBegin();
                pc.scanf("%d %d", &servoNum, &servoAngle);
                setServoPulse(servoNum, servoAngle);
                break;
            default:
                pc.printf("ERROR: NOT A VALID TEST PROCEDURE");
                break;
        }


        pc.printf("Distance = %d", getDistance());

        /*if(pc.readable()) {

            pc.scanf("%d %d", &servoNum, &pulseWidth);
            setServoPulse(servoNum, pulseWidth);


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


        }*/
    }

    /**************************************************
    *     FIRST STAGE
    *
    *          - DETERMINE OIL RIG ON FIRE
    *          - DETERMINE PATH
    *
    **************************************************/

    //TODO: EXTEND ARM AND FACE OILRIGS

    //OILRIG 1 DISTANCE READING

    //TODO: ROTATE ARM TO NEXT OIL RIG

    //OILRIG 2 DISTANCE READING

    //ROTATE ARM TO NEXT OIL RIG

    //OILRIG 3 DISTANCE READING

}



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

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, 90);
    setServoPulseNo_delay(2, 90);
    setServoPulseNo_delay(3, 90);
    setServoPulseNo_delay(4, 90);
    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].lil_arm);
    setServoPulse(3, Arm_Table[set].big_arm);
    setServoPulse(4, Arm_Table[set].claw_arm);
    setServoPulse(5, Arm_Table[set].claw_rotate);
    setServoPulse(6, Arm_Table[set].claw_open);
}