File content as of revision 2:a94e3ade9632:

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

Adafruit_PWMServoDriver pwm(p28, p27);
Serial pc(USBTX, USBRX);
int num, input;


typedef struct { int Arm1; int Arm2; int Arm3; int Arm4; int Arm5; } Coord;

Coord Arm_Table[] = 
{
   //Arm1   Arm2   Arm3   Arm4   Arm5
   //Claw                        Base 
      
    {0, 1700, 1650, 1650, 0},// straight up 
    {0, 2700, 600, 600, 0}//storing position

}; 



void setServoPulse(uint8_t n, float pulse) {
    static int prev[6];
    int inc, loop, i, value, set;
    float pulselength = 10000;   // 10,000 us per second
    
    if(pulse>=500 && pulse<=2700){
        inc=abs(pulse-prev[n]);
        
        if(inc <500){
            inc=inc/16;
            loop=16;
        }
        else if(inc< 1000){
            inc=inc/32;
            loop=32;
        }
        else{
            inc= inc/64;
            loop=64;
        }

        value= prev[n] + inc;
        
        for(i=0; i<loop; i++){
            set = 4094 * value / pulselength;
            pwm.setPWM(n, 0, set);
            if( prev[n] < pulse)value+= inc;
            else value-= inc;
            wait_ms(15);
        }

        
        prev[n]= pulse; 
    
        pulse = 4094 * pulse / pulselength;
        pwm.setPWM(n, 0, pulse);
    }
}

void setPosition( int set ){

    setServoPulse(1, Arm_Table[set].Arm1);  
    setServoPulse(2, Arm_Table[set].Arm2);
    setServoPulse(3, Arm_Table[set].Arm3);
    setServoPulse(4, Arm_Table[set].Arm4);
    setServoPulse(5, Arm_Table[set].Arm5);  

}

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

int main() {
    //pwm.i2c_probe();
    initServoDriver();
    setPosition(1);
    setPosition(0);
    setPosition(1); 
    
    while(1){
        if(pc.readable()){
             pc.scanf("%d %d", &num, &input);
             setServoPulse(num, input);
        }
    }
    

}