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; }