#include "DynamixelSerial.h"

int Temperature,Voltage,Position; 


void setup(){
Dynamixel.begin(1000000,5);  // Inicialize the servo at 1Mbps and Pin Control 2
delay(1000);
}

void loop(){


  Dynamixel.move(1,random(200,800));  // Move the Servo radomly from 200 to 800
  delay(1000);
  Dynamixel.moveSpeed(1,random(200,800),random(200,800));
  delay(2000);
  Dynamixel.setEndless(1,ON);
  Dynamixel.turn(1,RIGTH,1000);
  delay(3000);
  Dynamixel.turn(1,LEFT,1000);
  delay(3000);
  Dynamixel.setEndless(1,OFF);
  Dynamixel.ledStatus(1,ON);
  Dynamixel.moveRW(1,512);
  delay(1000);
  Dynamixel.action();
  Dynamixel.ledStatus(1,OFF);

  delay(100);

/*
  Temperature = Dynamixel.readTemperature(1); // Request and Print the Temperature
  Voltage = Dynamixel.readVoltage(1);         // Request and Print the Voltage
  Position = Dynamixel.readPosition(1);       // Request and Print the Position 
  Serial.print(" *** Temperature: ");   // Print the variables in the Serial Monitor
  Serial.print(Temperature);
  Serial.print(" Celcius  Voltage: ");
  Serial.print(Voltage);
  Serial.print("  Volts   Position: ");
  Serial.print(Position);
  Serial.println(" of 1023 resolution");
*/  
 
delay(1000);

}