File content as of revision 3:ff08522e6416:

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

#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // this is the 'maximum' pulse length count (out of 4096)

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


int Arm_Table[][7] = 
{
   //Arm0   Arm1   Arm2   Arm3   Arm4   Arm5    Arm 6
   //Base                                       Claw 
     
    {2500, 550, 600, 2700, 2700, 0, 0},// storing  
    {1300, 800, 1150, 2500, 2700, 0, 0},// straight up 
    {2350, 1050, 700, 500, 2350, 0, 0}// over tools

}; 



void setServoPulse(uint8_t n, float pulse) {
    static int prev[7]={2500, 550, 600, 2700, 2700, 0, 0};
    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 <55){
            inc=inc/10;
            loop=10;
        }
        else if(inc <500){
            inc=inc/32;
            loop=32;
        }
        else if(inc< 1000){
            inc=inc/70;
            loop=70;
        }
        else{
            inc= inc/100;
            loop=100;
        }

        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 ){
    int i;
    
    for(i=0; i<7; i++)setServoPulse(i, Arm_Table[set][i]);  
 

}

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() {

    int move[7]={2500, 550, 600, 2700, 2700, 0, 0};
    int i, delta=50;
    //pwm.i2c_probe();
    initServoDriver();
    setPosition(0);
    
    setPosition(1);// laser range find
    setPosition(0);
    
    setPosition(2); //tool find 
    setPosition(0);
     
    
    while(1){
        if(pc.readable()){
             num=pc.getc();
             
             if(num == 'a' || num == 'z'){
                 if(move[0]>2650)move[0]=2700;
                 else if(move[0] < 550) move[0]=500;
                 
                 if(num=='a'){
                     setServoPulse(0, (move[0]+=delta));
                 }
                 else setServoPulse(0, (move[0]-=delta));
             }
             else if(num == 's' || num == 'x'){
                 if(move[1]> 2650)move[1]=2700;
                 else if(move[1] < 550) move[1]=500;
                 
                 if(num=='s')setServoPulse(1, (move[1]+=delta));
                 else setServoPulse(1, (move[1]-=delta));
             }
             else if(num == 'd' || num == 'c'){
                 if(move[2]> 2650)move[2]=2700;
                 else if(move[2] < 550) move[2]=500;
                 
                 if(num=='d')setServoPulse(2, (move[2]+=delta));
                 else setServoPulse(2, (move[2]-=delta));
             }
             else if(num == 'f' || num == 'v'){
                 if(move[3]> 2650)move[3]=2700;
                 else if(move[3] < 550) move[3]=500;
                 
                 if(num=='f')setServoPulse(3, (move[3]+=delta));
                 else setServoPulse(3, (move[3]-=delta));
             }
             else if(num == 'g' || num == 'b'){
                 if(move[4]> 2650)move[4]=2700;
                 else if(move[4] < 550) move[4]=500;
                 
                 if(num=='g')setServoPulse(4, (move[4]+=delta));
                 else setServoPulse(4, (move[4]-=delta));
             }
             else if(num == 'h' || num == 'n'){
                 if(move[5]> 2650)move[5]=2700;
                 else if(move[5] < 550) move[5]=500;
                 
                 if(num=='h')setServoPulse(5, (move[5]+=delta));
                 else setServoPulse(5, (move[5]-=delta));
             }
             else if(num == 'j' || num == 'n'){
                 if(move[6]> 2650)move[6]=2700;
                 else if(move[6] < 550) move[6]=500;
                 
                 if(num=='j')setServoPulse(6, (move[6]+=delta));
                 else setServoPulse(6, (move[6]-=delta));
             }
             pc.printf("0\t 1\t 2\t 3 \t4 \t 5 \t 6\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]);
             //setServoPulse(num, input);
        }
    }
    

}