 SoftwareSerial mySerial(10, 11);    // RX, TX
 
 void setup() {
-  
+
   Serial.flush();                                       // Clear the serial buffer of garbage data before running the code.
   Serial1.begin(115200);
   mySerial.begin(SERVO_SET_Baudrate);                   // We now need to set Ardiuno to the new Baudrate speed 115200

  Dynamixel.setProfileAcceleration(0x05, 300);

  //Set profile velocity
  Dynamixel.setProfileVelocity(0x01, 100);              //Set profile vel for each servo 
  Dynamixel.setProfileVelocity(0x02, 100);
  Dynamixel.setProfileVelocity(0x03, 100);
  Dynamixel.setProfileVelocity(0x04, 200);
  Dynamixel.setProfileVelocity(0x05, 200);  
 }

int x, y, z, emg1, emg2, g, h;
int emgcounter = 0;
int i = 2148;
int j = 2048;

    //delay(500);
    Dynamixel.performMovement(1128, 3250, 1846, CLOSE);     //Detour
    //delay(500);
    Dynamixel.performMovement(1128, 3250, 1846, OPEN);      //Move to sponge.
    //delay(1000);
    Dynamixel.performMovement(1142, 3250, 775, OPEN);     //Ready position
    //delay(1000);
