///////////////////////////////////////////////////////
//exemple from Samples/Devices/Servomotor, modified
///////////////////////////////////////////////////////

#include "ModaCpp.h"
#include "conio.h"

//Settings:
#define MODASERVER "localhost"

int main(int argc, TCHAR* argv[])
{
ModaCPP::Connection *connection=new ModaCPP::Connection(true);

//Connect to MODA server
if(connection->Connect(MODASERVER))
    {
    _cprintf("Connexion to %s ok\r\n",MODASERVER);

    //Find the robot
    ModaCPP::RobotPHX *robot=connection->QueryRobotPHX("/");
    if(robot)
        {
        _cprintf("robot found in this world\r\n");
        ModaCPP::DeviceServoMotor *pServo1=robot->QueryDeviceServoMotor("hinge2/axisred/servo");
        ModaCPP::DeviceServoMotor *pServo2=robot->QueryDeviceServoMotor("hinge2/axisgreen/servo");

        if(pServo1 && pServo2)
            {
            //create group^and add the 2 servos
            ModaCPP::DevicesGroupServo group(connection);
            group.AddDevice(pServo1);
            group.AddDevice(pServo2);

            //positions to reah / test
            float positions1[2]={-89.0f,-45.0f};
            float positions2[2]={89.0f,45.0f};

            while(!_kbhit())
                {
                //ask to go to the specific position1
                group.GoPositionsDeg(positions1,NULL,2);
                //wait all servo reach this position with an acceptable error equal to servo accuracy
                //do this wait during an infinite time if needed
                group.WaitPositionsDegCompleteWithError(positions1,0,2,INFINITE);

                //ask to go to the specific position2
                group.GoPositionsDeg(positions2,NULL,2);
                //wait all servo reach this position with an acceptable error equal to servo accuracy
                //do this wait during 1000 ms maximum, if needed
                group.WaitPositionsDegCompleteWithError(positions2,0,2,1000);
                }
            delete pServo1;
            delete pServo2;
            }
        else
            {
            _cprintf("servo(s) not found ...\r\n");
            }
        delete robot;
        }
    else
        {
        _cprintf("robot not found in this world\r\n");
        }
    }
else
    {
    _cprintf("Unable to connect to moda server : be sure Exec is running and MODA TCP/UDP ports are open\r\n");
    }
connection->Disconnect();
delete connection;

return 0;
}