sync_write
こんな感じ。
short sync_write(short speed,short *pos){ char buffer[143]; //dynamixelの限界パケット長 short len; short inst; int ret; int i,j; unsigned char sum=0; // inst = 0x83; len = 109; //send buffer set buffer[0] = 0xff; buffer[1] = 0xff; buffer[2] = 0xfe; buffer[3] = (unsigned char)len; buffer[4] = (unsigned char)inst; buffer[5] = 0x1e; buffer[6] = 0x04; for(i=7,j=1;i<112;*pos++,j++){ buffer[i] = (unsigned char)(j); i++; buffer[i] = (unsigned char)(0x00ff & *pos); i++; buffer[i] = (unsigned char)((*pos >> 8) & 0x00ff); i++; buffer[i] = (unsigned char)(speed & 0x00ff); i++; buffer[i] = (unsigned char)((speed >> 8) & 0x00ff); i++; } for(i=2;i<112;i++){ sum += buffer[i]; } buffer[112] = ~sum; for(i=0;i<113;i++){ PRINTF("buffer[%d]=%d\n",i,buffer[i]); } //write ret = write(fd,buffer,113); }
とりあえずきれいに動くよ。スピードも指定可能。
やろうとおもえばサーボごとにスピードも変えられる。
とりあえずスピードは一個だけだけど。