   int id1 = 2071; //Servo 1 goal position
   int id2 = 2048; //Servo 2 goal position
   int id3 = 2048; //Servo 3 goal position
-  //int id4 = 2548; //Servo 4 goal position
-  //int id5 = 1548; //Servo 5 goal position
+  int id4 = 2548; //Servo 4 goal position
+  int id5 = 1548; //Servo 5 goal position
 
  Dynamixel.setNGoalPositions(-1, id2, id3, -1, -1);  //Set goal position of all the servos
     


  // Set the Profile acceleration.
  Dynamixel.setProfileAcceleration(0x01, 10);  //Set the Profile Acceleration for each servo. (max. is 32767)
  Dynamixel.setProfileAcceleration(0x02, 10);  //Set the Profile Acceleration for each servo. (max. is 32767)
  Dynamixel.setProfileAcceleration(0x03, 10);  //Set the Profile Acceleration for each servo. (max. is 32767)
  /*
  Dynamixel.setProfileAcceleration(0x04, 300);  //Set the Profile Acceleration for each servo. (max. is 32767)
  Dynamixel.setProfileAcceleration(0x05, 300);  //Set the Profile Acceleration for each servo. (max. is 32767)
  */

  // Set the Profile velocity.
  Dynamixel.setProfileVelocity(0x01, 100);  //Set the Profile Velocity for each servo. (max. is 1023)
  Dynamixel.setProfileVelocity(0x02, 100);  //Set the Profile Velocity for each servo. (max. is 1023)
  Dynamixel.setProfileVelocity(0x03, 100);  //Set the Profile Velocity for each servo. (max. is 1023)
  /*
  Dynamixel.setProfileVelocity(0x04, 200);  //Set the Profile Velocity for each servo. (max. is 1023)
  Dynamixel.setProfileVelocity(0x05, 200);  //Set the Profile Velocity for each servo. (max. is 1023)
  */


  /*
  //Get position for servos in steps
  Dynamixel.getPosition(0x01); 
  Dynamixel.getPosition(0x02);

    Serial.println(data[i]);
  }


}


