File content as of revision 4:116829a5ae3c:

#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


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);
int ServoTest(void);

/************
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, 46, 0, 0, 100, 96, 0, 0}, // storing position
    {OIL_RIG1, 83, 75, 100, 33, 33, 0, 0}, // point laser at oilrig2
    {STORE_POSITION, 100, 17, 17, 0, 100, 0, 0}, // Shape Detect
    {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
    {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
    {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
    {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position

}; 

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){
    int instr;
        
    pc.printf("Set Servo Position = 0, Shape Detect = 1, Oil Rig Detect = 2");
    while(!pc.readable());
    pc.scanf("%d", &instr);
    
    if(instr == 0){// Servo Control
        while(!ServoTest());
    }
    else if( instr == 1){// shape detect 
        //servoPosition(3);
        
        
    }
    else if( instr == 2){// oil rig detect
            servoPosition(2);
            
    }

}// End Main while        
}

/**************************************************
*     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], pstart;
    int pulse2, min=500, max=2700; 
    
    // Set when to start the pulse for the different servos
    // and normalize values 
 /*   if(n < 2){
        pstart = 0;// need to change this to match the different servos
        pulse1= pulse ;
    }    
    else{
        pstart = 0;
        pulse1=pulse;     
    } */   
    
    pulse= ((pulse*(max-min))/100)+min;
    
    if(currentPosition[n] < pulse){
          pc.printf("\ncurrent position < pulse\n\r");
          for(i=currentPosition[n]; i < pulse; i++){  
          pulse2 = 4094 * i / pulselength;
          pwm.setPWM(n, pstart, pulse2);
          wait_ms(1); 
          }
    } else if (currentPosition[n] > pulse) {
           pc.printf("\ncurrent position > pulse\n\r");
           for(i=currentPosition[n]; i > pulse; i--){  
           pulse2 = 4094 * i / pulselength;
           pwm.setPWM(n, pstart, pulse2);
           wait_ms(1);
           }
    }
    currentPosition[n] = i;
    pc.printf("\nending position = %d\n\n\r", 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, 46);
    setServoPulseNo_delay(1, 0);
    setServoPulseNo_delay(2, 0);
    setServoPulseNo_delay(3, 100);
    setServoPulseNo_delay(4, 96); 
    setServoPulseNo_delay(5, 0);
    setServoPulseNo_delay(6, 0); 
    }
    
void setServoPulseNo_delay(uint8_t n, float pulse) {
    float pulselength = 20000;   // 20,000 us per second
    int min=500, max=2700; 
    
    pulse= ((pulse*(max-min))/100)+min;
    currentPosition[n] = pulse;
    pulse = 4094 * pulse / pulselength;
    pwm.setPWM(n, 0, pulse);
}
    
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); 
}

int ServoTest(void){
    int num, delta=1;  // Change Delta to  change increment size
    static int move[7]={46, 0, 0, 100, 96, 0, 0};
    
    if(pc.readable()){
        num=pc.getc();
             
        if(num == 'a' || num == 'z'){
                if(move[0]>=(100-delta))move[0]=(100-delta);
                else if(move[0] <= (0+delta)) move[0]=delta;
                 
                if(num=='a')setServoPulse(0, (move[0]+=delta));
                else setServoPulse(0, (move[0]-=delta));
        }
        else if(num == 's' || num == 'x'){
                if(move[1]>=(100-delta))move[1]=(100-delta);
                else if(move[1] <= (0+delta)) move[1]=delta;
                 
                if(num=='s')setServoPulse(1, (move[1]+=delta));
                else setServoPulse(1, (move[1]-=delta));
        }
        else if(num == 'd' || num == 'c'){
                if(move[2]>=(100-delta))move[2]=(100-delta);
                else if(move[2] <= (0+delta)) move[2]=delta;
                 
                if(num=='d')setServoPulse(2, (move[2]+=delta));
                else setServoPulse(2, (move[2]-=delta));
        }
        else if(num == 'f' || num == 'v'){
                if(move[3]>=(100-delta))move[3]=(100-delta);
                else if(move[3] <= (0+delta)) move[3]=delta;
                 
                if(num=='f')setServoPulse(3, (move[3]+=delta));
                else setServoPulse(3, (move[3]-=delta));
        }
        else if(num == 'g' || num == 'b'){
                if(move[4]>=(100-delta))move[4]=(100-delta);
                else if(move[4] <= (0+delta)) move[4]=delta;
                 
                if(num=='g')setServoPulse(4, (move[4]+=delta));
                else setServoPulse(4, (move[4]-=delta));
        }
        else if(num == 'h' || num == 'n'){
                if(move[5]>=(100-delta))move[5]=(100-delta);
                else if(move[5] <= (0+delta)) move[5]=delta;
                 
                if(num=='h')setServoPulse(5, (move[5]+=delta));
                else setServoPulse(5, (move[5]-=delta));
        }
        else if(num == 'j' || num == 'n'){
                if(move[6]>=(100-delta))move[6]=(100-delta);
                else if(move[6] <= (0+delta)) move[6]=delta;
                 
                if(num=='j')setServoPulse(6, (move[6]+=delta));
                else setServoPulse(6, (move[6]-=delta));
        }  
        else if(num== 'e') return 1;
                
        pc.printf("0\t 1\t 2\t 3 \t4 \t 5 \t 6\t type e to end\n\r");
        pc.printf("%d\t %d\t %d\t %d \t%d \t %d \t %d\n\r",move[0],move[1], move[2], move[3],move[4],move[5],move[6]);
    }
    return 0;
}