dynamixel_write関数

何となく作って見た。
今まで機能別に作ってたけどこれを使うことでどこでもアクセス可能。
リードもすでに作ってあるのでこれで、linux版dyanmixelコンフィグレータが作れないこともない。
後はサーチ機能か?

//---------------------------------------
// servo data write
//---------------------------------------
int dynamixel_write(short id,short addr,short data_len,char *p_addr){
    unsigned char buffer[100];
    short len;
    short inst;
    int ret;
    int i;
    short len_all;


    memset(buffer,0,100);

    len = data_len+3;
    inst = 0x03;            //write instruction set

    //send data set
    buffer[0] = 0xff;       //
    buffer[1] = 0xff;
    buffer[2] = (unsigned char)id;
    buffer[3] = (unsigned char)len;
    buffer[4] = (unsigned char)inst;
    buffer[5] = (unsigned char)addr;
    for(i=0;i<data_len;i++,p_addr++) buffer[6+i] =  *p_addr;
    for(i=2;i<data_len+6;i++) buffer[6+data_len] += buffer[i];
    buffer[6+data_len] = ~buffer[6+data_len];

    for(i=0;i<data_len+7;i++){
        PRINTF("%x ",buffer[i]);
    }
    PRINTF("\n");

    //write serial
    ret = write(fd,buffer,7+data_len);

    //read serial
    len = 0;
    len_all = 0;
    while(len_all < data_len+6+7){
        len=read(fd,buffer,BUFF_SIZE);
        if(len > 0) len_all +=len;
        i++;
        if(i>TIMEOUT) break;
    }

    return 0;
}