File content as of revision 1:fe4a0b47ff25:

#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 servoBegin(void);
void initServoDriver(void);
void setServoPulse(uint8_t n, float pulse);
void setServoPulseNo_delay(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();
servoBegin();
//servoPosition(STORE_POSITION);
//ServoOutputDisable = 0;
while(1){
        if(pc.readable()){
            //pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
             pc.scanf("%d %d", &servoNum, &pulseWidth);
             setServoPulse(servoNum, pulseWidth);
            
        }
}


/**************************************************
*     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
    int i = currentPosition[n];
    pc.printf("\ncurrent position = %d\n", currentPosition[n]);
    int pulse2; 
    if(currentPosition[n] < pulse){
          pc.printf("\ncurrent position < pulse\n");
          for(i; i < pulse; i++){  
          pulse2 = 4094 * i / pulselength;
          pwm.setPWM(n, 0, pulse2);
          wait_ms(3); 
          }
    } else if (currentPosition[n] > pulse) {
           pc.printf("\ncurrent position > pulse\n");
           for(i; i > pulse; i--){  
           pulse2 = 4094 * i / pulselength;
           pwm.setPWM(n, 0, pulse2);
           wait_ms(3);
           }
    }
    currentPosition[n] = i;
    pc.printf("\nending position = %d\n\n", i);  
}

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){
    setServoPulseNo_delay(0, 900);
    setServoPulseNo_delay(1, 500);
    setServoPulseNo_delay(2, 600);
    setServoPulseNo_delay(3, 2450);
    setServoPulseNo_delay(4, 2450); 
    setServoPulseNo_delay(5, 0);
    setServoPulseNo_delay(6, 0); 
    }
    
void setServoPulseNo_delay(uint8_t n, float pulse) {
    float pulselength = 20000;   // 20,000 us per second
    currentPosition[n] = pulse;
    pulse = 4094 * pulse / pulselength;
    pwm.setPWM(n, 0, pulse);
}
    
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); 
}