#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, starting angle +10
  servocontroller->AddMotorconfig(6, ServoMotors::SERVO_MG996R, 10); 
  //pin 7, MG996R servo, starting angle +20
  servocontroller->AddMotorconfig(7, ServoMotors::SERVO_MG996R, 20); 
  //pin 8, MG996R servo, starting angle -40
  servocontroller->AddMotorconfig(8, ServoMotors::SERVO_MG996R, -40); 
  //pin 9, MG996R servo, starting angle +30
  servocontroller->AddMotorconfig(9, ServoMotors::SERVO_MG996R, 30); 
  //pin 10, motor min -60, motor max +60, starting angle +20
  servocontroller->AddMotorconfig(10, -60, 60, 20);

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

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