File content as of revision 0:09bf8e3091e8:

#include "mbed.h"
#include "Servo.h"
#include "command.h"
#include "iSerial.h"

#define MAX_DATA 1800
#define TIME_INTERRAPUT_US 16000

#define targetPosition_COMMAND 0x64
#define SET_targetPosition_COMMAND 0x67
#define SERVO_MOVE_COMMAND 0x58
#define POWER_ONOFF_COMMAND 0x59
#define SENDDATA_COMMAND 0x05
#define INIT_COMMAND 0x66

#define targetPosition_BYTE 7
#define SERVO_MOVE_BYTE 3
#define POWER_ONOFF_BYTE 3
#define SENDDATA_BYTE 3
#define INIT_BYTE 6

#define ACK 0x06
#define NACK 0x15

Ticker flipper;
Serial device(USBTX, USBRX);  // tx, rx
DigitalOut myLed1(LED1);
DigitalOut myLed2(LED2);
DigitalOut myLed3(LED3);

Command servo1(p21);
Command servo2(p22);
Command servo3(p23);
Command servo4(p24);
Command servo5(p25);
Command servo6(p26);

AnalogIn ain1(p15);
AnalogIn ain2(p16);
AnalogIn ain3(p17);
AnalogIn ain4(p18);
AnalogIn ain5(p19);
AnalogIn ain6(p20);

typedef struct{
    short int target;
    short int preTarget;
    long perTime;
    bool moveFlag;
    int resolution;
    double positionNow;
}servo_t;
servo_t servo[7];

void flip() {
    for(int it=1;it<7;it++){
        if(servo[it].moveFlag==true){
            servo[it].positionNow+=servo[it].resolution;
            if((servo[it].target-servo[it].preTarget > 0 && servo[it].positionNow-servo[it].target >= 0) || (servo[it].target-servo[it].preTarget < 0 && servo[it].positionNow-servo[it].target <= 0)){
                 servo[it].moveFlag=false;
                 servo[it].preTarget = servo[it].target;
                 servo[it].positionNow = servo[it].target;
                 if(it == 1){
                     myLed1 = 0;
                 }else if(it == 2){
                     myLed2 = 0;
                 }else if(it == 3){
                     myLed3 = 0;
                 }
            }
            switch(it){
               case 1:
                 servo1=servo[it].positionNow/MAX_DATA;
                 break;
               case 2:
                 servo2=servo[it].positionNow/MAX_DATA;
                 break;
               case 3:
                 servo3=servo[it].positionNow/MAX_DATA;
                 break;
               case 4:
                 servo4=servo[it].positionNow/MAX_DATA;
                 break;
               case 5:  
                 servo5=servo[it].positionNow/MAX_DATA;
                 break;
               case 6:
                 servo6=servo[it].positionNow/MAX_DATA;
                 break;
            }
        }
    }
}


int main() {
    device.baud(115200);
    flipper.attach_us(&flip, TIME_INTERRAPUT_US);

//    flag=false;
    for(int it=0;it<7;it++){
        servo[it].target=900;
        servo[it].perTime=0;
        servo[it].moveFlag=false;
        servo[it].preTarget=900;
        servo[it].resolution=0;
        servo[it].positionNow=900;
    }

    myLed1=0;
    myLed2=0;
    myLed3=0;

    
    int valueNumber=0;
    int id=0;
    int command=0;
    int getValue=0;
    int checkSum=0;

    while(1){
        if(device.readable()){
            valueNumber++;
            getValue = device.getc();            
            
            switch(valueNumber){
                case 1: //get ID
                  id = getValue;
                  checkSum = getValue;
                  break;
                case 2: //get command upbyte
                  command = getValue;
                  checkSum += getValue;
                  break;
                case 3:
                case 4:
                case 5:
                case 6:
                case 7:

                //servo_move_command
                if(command == SERVO_MOVE_COMMAND){
                     if((valueNumber == SERVO_MOVE_BYTE && (checkSum & 0x0FF) == getValue)  || valueNumber < SERVO_MOVE_BYTE){
                           if(0 < id && id < 7){
                               servo[id].moveFlag = true;
                           }else if(id == 0x0FE){
                               for(int it=1; it<7; it++){
                                   servo[it].moveFlag = true;                               
                               }
                           }
                           if(valueNumber == SERVO_MOVE_BYTE){
                               device.putc(id);   
                               device.putc(ACK);  //ACK                           
                               valueNumber = 0;
                           }
                      }else{
                           device.putc(id);   
                           device.putc(NACK);  //NACK                       
                           valueNumber = 0;
                      }
                }


                //senddata command check
                if(command == SENDDATA_COMMAND){
                     if((valueNumber == SENDDATA_BYTE && (checkSum & 0xFF) == getValue)  || valueNumber < SENDDATA_BYTE){
                           int send_data = 0;
                           switch(id){
                           case 1:  //x
                               send_data = ain1.read_u16();
                               device.putc(send_data >> 8);
                               device.putc(send_data & 0xFF);
                               break;
                           case 2:  //y
                               send_data = ain2.read_u16();
                               device.putc(send_data >> 8);
                               device.putc(send_data & 0xFF);
                               break;
                           case 3:  //z
                               send_data = ain3.read_u16();
                               device.putc(send_data >> 8);
                               device.putc(send_data & 0xFF);
                               break;
                           case 4:  //other
                               send_data = ain4.read_u16();
                               device.putc(send_data >> 8);
                               device.putc(send_data & 0xFF);
                               break;
                           case 5:  //other
                               send_data = ain5.read_u16();
                               device.putc(send_data >> 8);
                               device.putc(send_data & 0xFF);
                               break;
                           case 6:  //other
                               send_data = ain6.read_u16();
                               device.putc(send_data >> 8);
                               device.putc(send_data & 0xFF);
                               break;                                                              
                           default:
                               device.putc(id);   
                               device.putc(NACK);  //NACK
                               valueNumber = 0;
                               break;
                           }
                           if(valueNumber == SENDDATA_BYTE){
                               device.putc(id);   
                               device.putc(ACK);  //ACK                           
                               valueNumber = 0;
                           }
                      }else{
                           device.putc(id);   
                           device.putc(NACK);  //NACK                       
                           valueNumber = 0;
                      }
                }
                  
                //power_onoff command check
                if(command == POWER_ONOFF_COMMAND){
                     if((valueNumber == POWER_ONOFF_BYTE && (checkSum & 0xFF) == getValue)  || valueNumber < POWER_ONOFF_BYTE){
                           switch(id){
                           case 1:
                               servo1.onOff();
                               break;
                           case 2:
                               servo2.onOff();
                               break;
                           case 3:
                               servo3.onOff();
                               break;
                           case 4:
                               servo4.onOff();
                               break;
                           case 5:
                               servo5.onOff();
                               break;
                           case 6:
                               servo6.onOff();
                               break;
                           default :
                               device.putc(id);   
                               device.putc(NACK);  //NACK
                               valueNumber = 0;
                               break;
                           }
                           if(valueNumber == POWER_ONOFF_BYTE){
                               device.putc(id);   
                               device.putc(ACK);  //ACK                           
                               valueNumber = 0;
                           }
                      }else{
                           device.putc(id);   
                           device.putc(NACK);  //NACK                       
                           valueNumber = 0;
                      }
                 }
                 
                 //init command check
                 if(command == INIT_COMMAND){
                      if((valueNumber == INIT_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < INIT_BYTE){
                            switch(id){
                            case 1:
                                servo1.homePosition(valueNumber,getValue);
                                break;
                            case 2:
                                servo2.homePosition(valueNumber,getValue);
                                break;
                            case 3:
                                servo3.homePosition(valueNumber,getValue);
                                break;
                            case 4:
                                servo4.homePosition(valueNumber,getValue);
                                break;
                            case 5:
                                servo5.homePosition(valueNumber,getValue);
                                break;
                            case 6:
                                servo6.homePosition(valueNumber,getValue);
                                break;
                            default :
                               device.putc(id);   
                               device.putc(NACK);  //NACK
                               valueNumber = 0;
                               break;
                            }
                            if(valueNumber == INIT_BYTE){
                               device.putc(id);   
                               device.putc(ACK);  //ACK
                               valueNumber = 0;
                            }
                      }else{
                            device.putc(id);   
                            device.putc(NACK);  //NACK
                            valueNumber = 0;                    
                      }
                 }

                //move command check
                 if(command == targetPosition_COMMAND || command == SET_targetPosition_COMMAND){
                       if((valueNumber == targetPosition_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < targetPosition_BYTE){
                            switch(id){
                            case 1:
                                servo1.targetPosition(valueNumber,getValue);
                                servo[id].target = servo1.getPosition();
                                servo[id].perTime = servo1.getTime();
                                break;
                            case 2:
                                servo2.targetPosition(valueNumber,getValue);
                                servo[id].target = servo2.getPosition();
                                servo[id].perTime = servo2.getTime();
                                break;
                            case 3:
                                servo3.targetPosition(valueNumber,getValue);
                                servo[id].target = servo3.getPosition();
                                servo[id].perTime = servo3.getTime();
                                break;
                            case 4:
                                servo4.targetPosition(valueNumber,getValue);
                                servo[id].target = servo4.getPosition();
                                servo[id].perTime = servo4.getTime();
                                break;
                            case 5:
                                servo5.targetPosition(valueNumber,getValue);
                                servo[id].target = servo5.getPosition();
                                servo[id].perTime = servo5.getTime();
                                break;
                            case 6:
                                servo6.targetPosition(valueNumber,getValue);
                                servo[id].target = servo6.getPosition();
                                servo[id].perTime = servo6.getTime();
                                break;
                            }
                            if(valueNumber == targetPosition_BYTE){
                                device.putc(id);   
                                device.putc(ACK);  //ACK
                                servo[id].resolution = (servo[id].target-servo[id].preTarget)/(servo[id].perTime/(TIME_INTERRAPUT_US/1000.00));
                                if(command == targetPosition_COMMAND){
                                    servo[id].moveFlag = true;
                                     if(id == 1){
                                         myLed1 = 1;
                                     }else if(id == 2){
                                         myLed2 = 2;
                                     }else if(id == 3){
                                         myLed3 = 3;
                                     }
                                }else if(command == SET_targetPosition_COMMAND){
                                }
                                valueNumber = 0;
                            }
                       }else{
                            device.putc(id);   
                            device.putc(NACK);  //NACK
                            valueNumber = 0;                    
                       }
                 }
                 //plus checkSum
                 checkSum += getValue;
                 break;

            }
        }
    }
}