//Sample Samples/Devices/ServoMotor/C++

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

//Settings:
#define MODASERVER "localhost"

int main(int argc, char* 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)
            {
            float deg=0;
            while(!_kbhit())
                {
                pServo1->GoPositionDeg(-90);
                connection->Sleep(2000);
                pServo1->GoPositionDeg(90);
                connection->Sleep(2000);
                pServo1->GoPositionDeg(0);
                connection->Sleep(2000);

                deg+=90;
                pServo2->GoPositionDeg(deg);
                connection->Sleep(2000);
                }
            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;
}