#include <Herkulex.h>
int n=2; //motor ID - verify your ID !!!!
void setup()
{
delay(2000); //a delay to have time for serial monitor opening
Serial.begin(115200); // Open serial communications
Serial.println("Begin");
Herkulex.begin(115200,10,11); //open serial with rx=10 and tx=11
Herkulex.reboot(n); //reboot first motor
delay(500);
Herkulex.initialize(); //initialize motors
delay(200);
}
void loop(){
Serial.println("Move Angle: -100 degrees");
Herkulex.moveOneAngle(n, -100, 1000, LED_BLUE); //move motor with 300 speed
delay(1200);
Serial.print("Get servo Angle:");
Serial.println(Herkulex.getAngle(n));
Serial.println("Move Angle: 100 degrees");
Herkulex.moveOneAngle(n, 100, 1000, LED_BLUE); //move motor with 300 speed
delay(1200);
Serial.print("Get servo Angle:");
Serial.println(Herkulex.getAngle(n));
}