 
 #define SERVO_ControlPin 0x02       // Control pin of buffer chip, NOTE: this does not matter becasue we are not using a half to full control buffer.
 #define SERVO_SET_Baudrate 57600    // Baud rate speed which the Dynamixel will be set too (57600)
+#define LED13 0x0D
+#define CW_LIMIT_ANGLE 0x001        // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
+#define CCW_LIMIT_ANGLE 0xFFF       // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode
 #define OPEN 'o'
 #define CLOSE 'c'
 
 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
   Serial.begin(SERVO_SET_Baudrate);                                  // Start serial communication on baudrate 57600
   Dynamixel.begin(mySerial);                            // Calling mySerial function which sets 10 pin as the 2nd RX serial pin, and sets pin 11 as the 2nd TX serial pin
   Dynamixel.setDirectionPin(SERVO_ControlPin);          // Optional. Set direction control pin which controls if the program writes or reads
-  Serial.flush();                                       // Clear the serial buffer of garbage data before running the code.
 
   //Turn on hold for the servos, so they stay in place
   Dynamixel.setHoldingTorque(0x01, true);               //Set on hold turque for each servo

   }
   break;
}
  if(z >= 400 && z <= 470){                         //Sets it back to the original position and the counter to 0
    jointexcess_prevention=0;
    }
while(joint == 1 && y > 650 && z > 300 && z < 700){                        //Tilting forward moves joint 1 down
  i = i + 10;
  if(i > 3200){
    i = 3200;
  }
  Dynamixel.setGoalPosition(1, i);
  break;
}
while(joint == 1 && y < 400 && z > 300 && z < 700){                        //Tilting backward moves joint 1 up
  i = i - 10;
  if(i < 800){
  i = 800;
  }  
  Dynamixel.setGoalPosition(1, i);
  break;
}
while(joint == 2 && y > 650 && z > 300 && z < 700){                        //Tilting forward moves joint 2 down
  j = j + 10;
  if(j > 3200){
  j = 3200;
  }
  Dynamixel.setGoalPosition(2, j);
  break;
}
while(joint == 2 && y < 400 && z > 300 && z < 700){                        //Tilting backward moves joint 2 up
  j = j - 10;
  if(j < 800){
  j = 800;
  }
  Dynamixel.setGoalPosition(2, j);
  break;
}
while(joint == 3 && y > 650 && z > 300 && z < 700){                        //Tilting forward moves joint 3 down
  k = k + 10;
  if(k > 3200){
  k = 3200;
  }
  Dynamixel.setGoalPosition(3, k);
  break;
}
while(joint == 3 && y < 400 && z > 300 && z < 700){                        //Tilting backward moves joint 3 up
  k = k - 10;
  if(k < 800){
  k = 800;
  }
  Dynamixel.setGoalPosition(3, k);
  break;
}
while(joint == 4 && y > 650 && z > 300 && z < 700){                        //Tilting forward opens the gripper
   Dynamixel.gripper(OPEN);
   break;
}
while(joint == 4 && y < 400 && z > 300 && z < 700){                        //Tilting backward closes the gripper
   Dynamixel.gripper(CLOSE);
   break;
}
  
}

void loop() {

  Dynamixel.setHoldingTorque(0x05, true);

  //Set profile acceleration
  Dynamixel.setProfileAcceleration(0x01, 0);           //Set profile acc for each servo
  Dynamixel.setProfileAcceleration(0x02, 0);
  Dynamixel.setProfileAcceleration(0x03, 0);
  Dynamixel.setProfileAcceleration(0x04, 100);
  Dynamixel.setProfileAcceleration(0x05, 100);

