 {
   byte servo = cmd[1];
   byte side = cmd[2];
-  byte speed = cmd[3];
-
-  Serial.print("Dynamixel Set Endless");
+  byte speed0 = cmd[3];
+  byte speed1 = cmd[4];
+
+  int speed = dynamixel.makeWord(speed0, speed1);
+
+  Serial.print("Dynamixel Set turn speed");
   Serial.print(", servo: "); Serial.print(servo);
   Serial.print(", side: "); Serial.print(side);
   Serial.print(", speed: "); Serial.println(speed);


void receiveData(int byteCount)
{
    while(Wire.available())
    {
      if(Wire.available()==5)
      {
        flag=0; 
        idx=0;

void loop()
{
  long dur,RangeCm;
  if(idx==6)
  {
    flag=1;
    //IR reciever pin set command
