#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 6, MG996R servo (motor min -60, motor max +60), starting angle +5
  servocontroller->AddMotorconfig(6, ServoMotors::SERVO_MG996R, 5);

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

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