#include "stdafx.h" #include "Modacpp.h" #include "conio.h" #define MODASERVER "localhost" #define MYROBOTNAME "/robot1" int main(int argc, char* argv[]) { ModaCPP::Connection *connection=new ModaCPP::Connection(true); if(connection->Connect(MODASERVER)) { _cprintf("Connection ok to moda server\r\n"); ModaCPP::RobotPHX *robot=connection->QueryRobotPHX(MYROBOTNAME); if(robot) { _cprintf("robot found\r\n"); //find the robot motors ModaCPP::DeviceMotor *pFrontLeftMotor; ModaCPP::DeviceMotor *pBackLeftMotor; ModaCPP::DeviceMotor *pFrontRightMotor; ModaCPP::DeviceMotor *pBackRightMotor; pFrontLeftMotor =robot->QueryDeviceMotor("joint_front_left/axis/motor"); pFrontRightMotor=robot->QueryDeviceMotor("joint_front_right/axis/motor"); pBackLeftMotor =robot->QueryDeviceMotor("joint_back_left/axis/motor"); pBackRightMotor =robot->QueryDeviceMotor("joint_back_right/axis/motor"); //check all is OK if(pFrontLeftMotor && pFrontRightMotor && pBackLeftMotor && pBackRightMotor) { //create devices group ModaCPP::DevicesGroupMotor *pMotorsGroup=new ModaCPP::DevicesGroupMotor(robot->GetConnection()); pMotorsGroup->AddDevice(pFrontLeftMotor); pMotorsGroup->AddDevice(pBackLeftMotor); pMotorsGroup->AddDevice(pFrontRightMotor); pMotorsGroup->AddDevice(pBackRightMotor); float Speeds[4]={0,0,0,0}; while(!_kbhit()) { //devices group unique call : fast (1 network call) pMotorsGroup->SetVelocitiesDPS(Speeds,4); /*Is equivalent to set the 4 motors speeds separately: slow (4 network calls) pFrontLeftMotor->SetVelocityDPS(Speeds[0]); pBackLeftMotor->SetVelocityDPS(Speeds[1]); pFrontRightMotor->SetVelocityDPS(Speeds[2]); pBackRightMotor->SetVelocityDPS(Speeds[3]); */ Speeds[0]+=1.0f; Speeds[1]+=1.0f; Speeds[2]+=0.5f; Speeds[3]+=0.5f; robot->Wait(100); } } else { _cprintf("one or more motor sensor(s) missing\r\n"); } } else { _cprintf("robot not found\r\n"); } } else { _cprintf("Unable to connect to moda server\r\n"); } connection->Disconnect(); delete connection; return 0; }