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

}

とりあえずきれいに動くよ。スピードも指定可能。
やろうとおもえばサーボごとにスピードも変えられる。
とりあえずスピードは一個だけだけど。