File content as of revision 0:1b64a0cedc5d:

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

Serial pc(USBTX,USBRX);
Adafruit_PWMServoDriver pwm(p9,p10);
DigitalOut ServoOutputDisable(p8);
extern Serial lrf;



//Servo Positions
#define STORE_POSITION  0
#define OIL_RIG1        1
#define OIL_RIG2        2

void restingState(void);
void initServoDriver(void);
void setServoPulse(uint8_t n, float pulse);
void servoPosition(int set);

/************
Variables for Servos
*************/
int servoNum, pulseWidth, outputDisabled, posNum;
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, 2300, 500, 600, 2450, 2450, 0, 0}, // storing position
    {OIL_RIG2, 1200, 1400, 1900, 900, 900, 0, 0} // point laser at oilrig2

}; 

int main() {
    
/*****************
 INITIALIZATIONS
*******************/
//lrf_baudCalibration();
initServoDriver();
restingState();
servoPosition(STORE_POSITION);
ServoOutputDisable = 0;
while(1){
        if(pc.readable()){
            pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
            //pc.scanf("%d", &posNum);
            //servoPosition(posNum);
             setServoPulse(servoNum, pulseWidth);
             ServoOutputDisable = outputDisabled;
        }
}


/**************************************************
*     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, float pulse) {
   /* float pulselength = 20000;   // 20,000 us per second
    if (currentPosition[n] < pulse){
        for (int i=currentPosition[n]; i<(pulse+1); i++)
        pulse = 4094 * currentPosition[n]+1 / pulselength;
        pwm.setPWM(n, 0, pulse);
        } else {
            }*/
       float pulselength = 20000;   // 20,000 us per second     
       pulse = 4094 * currentPosition[n]+1 / pulselength;
       pwm.setPWM(n, 0, pulse);        
}

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 restingState(void){
    /*
    currentPosition[0] = 2300;
    currentPosition[1] = 500;
    currentPosition[2] = 600;
    currentPosition[3] = 2450;
    currentPosition[4] = 2450;
    currentPosition[5] = 0;
    currentPosition[6] = 0;
    */
    
    setServoPulse(0, 2300);
    setServoPulse(1, 500);
    setServoPulse(2, 600);
    setServoPulse(3, 2450);
    setServoPulse(4, 2450); 
    }
    
void servoPosition(int set){   
            //moves to current position
            setServoPulse(1, Arm_Table[set].base_arm,);
            setServoPulse(0, Arm_Table[set].base_rotate);
            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); 
}