///////////////////////////////////////////////////////
//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)
            {
            while(!_kbhit())
                {
                M32 DeviceResponse;

                //go to a specific position
                pServo1->GoPositionDeg(-89);
                //wait the servo reach this position, wait an infinite time if needed
                //acceptable error is not specified...
                pServo1->WaitPositionDegComplete(-89,0,INFINITE,&DeviceResponse);
                if(DeviceResponse==MODA_EWAITCOMPLETETIMEOUT)    _cprintf("1: TIMEOUT\r\n");

                //go to another specific position
                pServo1->GoPositionDeg(89);
                //wait the servo reach this position, wait 250 ms max
                //acceptable error is not specified ...
                pServo1->WaitPositionDegComplete(89,0,250,&DeviceResponse);
                if(DeviceResponse==MODA_EWAITCOMPLETETIMEOUT)    _cprintf("2: TIMEOUT\r\n");
                }
            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;
}