dynamixelのリード
short dynamixel_read(short id,short addr,short data_len){ unsigned char buffer[50]; short len; short inst; int ret; int i; short len_all; int timeout; short len_tmp = 0; len = 0x04; inst = READ; //read 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; buffer[6] = (unsigned char)data_len; buffer[7] = ~(buffer[2]+buffer[3]+buffer[4]+buffer[5]+buffer[6]); //write serial ret = write(fd,buffer,8); //read serial len = 0; len_all = data_len + 6; for(timeout=0;timeout<TIMEOUT;timeout++){ if(len < len_all){ len_tmp = read(fd,buffer+len,BUFF_SIZE); if(len_tmp > 0) { len += len_tmp; } }else { PRINTF("READ serial end len=%d\n",len); break; } } for(i=0;i<len;i++){ PRINTF("%d\n",buffer[i]); }
できた。
READ serial end len=15 255 255 1 4 2 43 1 204 255 255 1 3 0 32 219 0
dynamixelのテーブル構造体への格納はアライメントがずれるから面倒だな。いっそ全部charにしちまうか。
すごくだめな手法な気がするけど。