//Sample Samples/Devices/Motor/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::DeviceMotor *pMotorLeft=robot->QueryDeviceMotor("JointLeft/axis/motor");
        ModaCPP::DeviceMotor *pMotorRight=robot->QueryDeviceMotor("Jointright/axis/motor");
        if(pMotorLeft && pMotorRight)
            {
            while(!_kbhit())
                {
                pMotorLeft->SetVelocityDPS(90);
                pMotorRight->SetVelocityDPS(180);
                connection->Sleep(4000);

                pMotorLeft->SetVelocityDPS(180);
                pMotorRight->SetVelocityDPS(90);
                connection->Sleep(4000);

                pMotorLeft->SetVelocityDPS(90);
                pMotorRight->SetVelocityDPS(-90);
                connection->Sleep(4000);
                }
            delete pMotorLeft;
            delete pMotorRight;
            }
        else
            {
            _cprintf("motor(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;
}