//Sample from Tutorial 5 main.cpp file

#include "stdafx.h"
#include "Modacpp.h"
#include "conio.h"

#define ROBOT        "/robot"
#define CONVEYOR    "/enclos/conveyor"

#define MODASERVER    "localhost"

ModaCPP::DeviceServoMotor *a1;
ModaCPP::DeviceServoMotor *a2;
ModaCPP::DeviceServoMotor *a3;
ModaCPP::DeviceServoMotor *a4;
ModaCPP::DeviceServoMotor *a5;
ModaCPP::DeviceServoMotor *a6;
ModaCPP::DevicesGroupServo *pServosGroup;

void ServoTestLoop(ModaCPP::DeviceServoMotor *pServo)
{
pServo->GoPositionDeg(0);
pServo->GetRobot()->Sleep(500);
pServo->GoPositionDeg(-90);
pServo->GetRobot()->Sleep(500);
pServo->GoPositionDeg(0);
pServo->GetRobot()->Sleep(500);
pServo->GoPositionDeg(90);
pServo->GetRobot()->Sleep(500);
pServo->GoPositionDeg(0);
}

void ServoTests( void)
{
//Test 1 : Single servo access
ServoTestLoop(a1);
ServoTestLoop(a2);
ServoTestLoop(a3);
ServoTestLoop(a4);
ServoTestLoop(a5);
ServoTestLoop(a6);

a2->GoPositionDeg(45);
a3->GoPositionDeg(-45);
a3->GetRobot()->Sleep(500);
a2->GoPositionDeg(0);
a3->GoPositionDeg(0);
a3->GetRobot()->Sleep(500);


//test 2 : go multiple degree positions
float positions1[6]={10,20,30,40,50,60};
float positions2[6]={-10,-20,-30,-40,-50,-60};
pServosGroup->GoPositionsDeg(positions1,NULL,6);
pServosGroup->GetConnection()->Sleep(1000);
pServosGroup->GoPositionsDeg(positions2,NULL,6);
pServosGroup->GetConnection()->Sleep(1000);

//test 2 : go multiple indexed positions
M32 indices1[6]={10000,20000,30000,40000,50000,60000};
M32 indices2[6]={0,0,0,0,0,0};
pServosGroup->GoPositionsIndex(indices1,NULL,6);
pServosGroup->GetConnection()->Sleep(1000);
pServosGroup->GoPositionsIndex(indices2,NULL,6);

//Home
pServosGroup->GoPositionDeg(0); //applyed to all servos
pServosGroup->GetConnection()->Sleep(1000);
}

int main(int argc, char* argv[])
{
ModaCPP::Connection *connection=new ModaCPP::Connection(true);
if(connection->Connect(MODASERVER))
    {
    ModaCPP::RobotPHX *robot=connection->QueryRobotPHX(ROBOT);
    if(robot)
        {
        _cprintf("robot found\r\n");
        a1=robot->QueryDeviceServoMotor("joint1/a1/m");
        a2=robot->QueryDeviceServoMotor("joint2/a2/m");
        a3=robot->QueryDeviceServoMotor("joint3/a3/m");
        a4=robot->QueryDeviceServoMotor("joint4/a4/m");
        a5=robot->QueryDeviceServoMotor("joint5/a5/m");
        a6=robot->QueryDeviceServoMotor("joint6/a6/m");

        if(a1 && a2 && a3 && a4 && a5 && a6)
            {
            //create a servo group to allow single call
            pServosGroup=new ModaCPP::DevicesGroupServo(connection);
            pServosGroup->AddDevice(a1);
            pServosGroup->AddDevice(a2);
            pServosGroup->AddDevice(a3);
            pServosGroup->AddDevice(a4);
            pServosGroup->AddDevice(a5);
            pServosGroup->AddDevice(a6);

            xkode::types::MU32 time=pConveyor->GetTime();
            while(1)
                {
                ServoTests();
                time=pConveyor->WaitTime(time+100);
                }
            }
        else
            {
            _cprintf("missing servo: %p %p %p %p %p %p\r\n",a1 , a2 , a3 ,a4 , a5 , a6);
            }
        }
    else
        {
        _cprintf("robot not found\r\n");
        }
    }
else
    {
    _cprintf("Unable to connect\r\n");
    }

connection->Disconnect();
_getch();
return 0;
}