#include <OzIDManager.h>
#include <OzServoController.h>

OzIDManager* manager;
OzServoController* servocontroller;

void setup()
{
  Serial.begin(115200);

  manager = new OzIDManager;
  manager->_sendACK = true;
  manager->_checksum = true;

  OzCommunication::setIDManager(manager);

  servocontroller = new OzServoController;
  //pin 5, MG996R servo (motor min -60, motor max +60)
  servocontroller->AddMotorconfig(5, ServoMotors::SERVO_MG996R);
  //pin 6, MG996R servo (motor min -60, motor max +60), starting angle +20
  servocontroller->AddMotorconfig(6, ServoMotors::SERVO_MG996R, 20);
  //pin 7, MG996R servo (motor min -60, motor max +60), starting angle -30
  servocontroller->AddMotorconfig(7, ServoMotors::SERVO_MG996R, -30);
  //pin 8, motor min -60, motor max +60, starting angle +10
  servocontroller->AddMotorconfig(8, -60, 60, 10);

  manager->sendLinkSetup();

  int x=1; 
  manager->sendLinkSetup();
  manager->PrintWelcomeLine(servocontroller, x++, "MyServoMotors");
  servocontroller->showMotorDetails();
}

void loop()
{
  OzCommunication::communicate();
}