File content as of revision 0:c2319e0e250e:

#include"mbed.h"

Serial device(p28,p27);//tx:left  rx:right
Serial pc(USBTX, USBRX);//pcとの通信設定
DigitalOut REDE(LED1);


int data[2]={7,1};

//通信設定
void init(void){
    device.baud(115200);
    pc.baud(115200);
    REDE = 0;
}

//ID書き換え
void IDchange(unsigned char ID, unsigned char data){
    
    unsigned char Txdata[9]; //bytedata[9byte]
    unsigned char CheckSum = 0;
    
    Txdata[0] = 0xFA; //Header
    Txdata[1] = 0xAF; //Header
    Txdata[2] = ID;   //ID
    Txdata[3] = 0x00; //Flags
    Txdata[4] = 0x04; //Adress
    Txdata[5] = 0x01; //Length 
    Txdata[6] = 0x01; //Count
    Txdata[7] = data; //data
    for(int i=2; i<=7; i++){
         CheckSum = CheckSum ^ Txdata[i];
    }
    Txdata[8] = CheckSum;
    
    //Send Packet
    REDE = 1;
    for (int i=0; i<=8; i++){
        device.putc(Txdata[i]);
        pc.putc(Txdata[i]);
    }
    wait_us(250);
    REDE = 0;
}

//データの書き込み
void WriteROM(unsigned char ID){
    
    unsigned char Txdata[8]; //bytedata[8byte]
    unsigned char CheckSum = 0;
    
    Txdata[0] = 0xFA; //Header
    Txdata[1] = 0xAF; //Header
    Txdata[2] = ID;   //ID
    Txdata[3] = 0x40; //Flags
    Txdata[4] = 0xFF; //Adress
    Txdata[5] = 0x00; //Length 
    Txdata[6] = 0x00; //Count
    for(int i=2; i<=6; i++){
         CheckSum = CheckSum ^ Txdata[i];
    }
    Txdata[7] = CheckSum;
    
    //Send Packet
    REDE = 1;
    for (int i=0; i<=7; i++){
        device.putc(Txdata[i]);
        pc.putc(Txdata[i]);
    }
    wait_us(250);
    REDE = 0;
}
//ＰＣから割り込み時のデータ受け取り
void getdata(){
    for(int i=0;i<=1;i++){
         data[i] = pc.getc();
        }
        
     IDchange(data[0],data[1]);
     WriteROM(data[1]);
    }

//トルク出力
void torque(unsigned char ID, unsigned char data){
    
    unsigned char TxData[9]; //bytedata[9byte]
    unsigned char CheckSum = 0; //CheckSum cakcukation
    
    TxData[0] = 0xFA; //Header
    TxData[1] = 0xAF; //Header
    TxData[2] = ID;   //ID
    TxData[3] = 0x00; //Flags
    TxData[4] = 0x24; //Adress
    TxData[5] = 0x01; //Length 
    TxData[6] = 0x01; //Count
    TxData[7] = data; //data
    
    //CheckSum calculation
    for(int i=2; i<=7; i++){
         CheckSum = CheckSum ^ TxData[i];
    }
    TxData[8] = CheckSum;
    
    //Send Packet
    REDE = 1;
    for (int i=0; i<=8; i++){
        device.putc(TxData[i]);
     //   pc.putc(TxData[i]);
    }
    wait_us(250);
    REDE = 0;
}

//ゴールポジション
void GoalPosition (unsigned char ID, int data){
    
    unsigned char TxData[10];//10bytedata
    unsigned char CheckSum = 0;
    
    TxData[0] = 0xFA; //Header
    TxData[1] = 0xAF; //Header
    TxData[2] = ID;   //ID
    TxData[3] = 0x00; //Flags
    TxData[4] = 0x1E; //Adress
    TxData[5] = 0x02; //Length 
    TxData[6] = 0x01; //Count
    
    TxData[7] = (unsigned char)0x00FF & data;
    TxData[8] = (unsigned char)0x00FF & (data >> 8); 
    
    //CheckSum calculation
    for(int i=2; i<=8; i++){
         CheckSum = CheckSum ^ TxData[i];
    }
    TxData[9] = CheckSum;
    
    //Send Packet
    REDE = 1;
    for (int i=0; i<=9; i++){
        device.putc(TxData[i]);
        pc.putc(TxData[i]);
    }
    wait_us(250);
    REDE = 0;
}

int main(){
    init();
   // pc.attach(getdata, Serial::RxIrq);
    while(1){REDE = 1;
     IDchange(data[0],data[1]);
     WriteROM(data[1]);
        torque(data[1],0x01);
        wait(0.5);
        GoalPosition(data[1],1500);
        wait(2);
        GoalPosition(data[1],-15000);
        wait(2);
    }
}