////////////////////////////////////////////////////////////////////////// //DeviceExternalCommunication sample: Simple network dialer //Usage: //Run associated simulation then use Telnet on port 20000 for dialing with the robot //Uses D, E, S, F keys in order to make the robot to move. ////////////////////////////////////////////////////////////////////////// #include "Modacpp.h" #include "conio.h" #define DEFAULTMODASERVER "localhost" #define DEFAULTROBOTNAME "/robot0" //************************************************************************************** void DoDialer(ModaCPP::DeviceExternalCommunicationPort *pDevice,ModaCPP::DeviceMotor *pMLeft,ModaCPP::DeviceMotor *pMRight) { //receives chars and moves the robot according to sent keys. while(!_kbhit()) { char temp[255]; //receive data then display it int receivedbytes=pDevice->SyncRead(temp,sizeof(temp)-1); if(receivedbytes>=1) { float left=0; float right=0; float speed=100.0f; temp[receivedbytes]=0; _cprintf("received '%s'\r\n",temp); //move the robot switch(temp[0]) { case 'E': case 'e': left=right=speed; break; case 'D': case 'd': left=right=-speed; break; case 'F': case 'f': left=speed; right=-speed; break; case 'S': case 's': left=-speed; right=speed; break; } pMLeft->SetVelocityDPS(left); pMRight->SetVelocityDPS(right); } //send echo to telnet pDevice->Write(xkode::lib::String("OK!!"),false); } } //************************************************************************************** int main(int argc, char* argv[]) { ModaCPP::Connection *connection=new ModaCPP::Connection(true); xkode::lib::String Server=DEFAULTMODASERVER; xkode::lib::String RobotName=DEFAULTROBOTNAME; //process command line arguments ModaCPP::CommandLine::ProcessCommandLine(argc,argv); if(ModaCPP::CommandLine::ExistsValue("/robot")) RobotName=ModaCPP::CommandLine::GetArgumentValue("/robot"); if(ModaCPP::CommandLine::ExistsValue("/server"))Server =ModaCPP::CommandLine::GetArgumentValue("/server"); //Try connect to MODA server if(connection->Connect(Server)) { _cprintf("Connection ok to moda server\r\n"); //Find the robot ModaCPP::RobotPHX *robot=connection->QueryRobotPHX(RobotName); if(robot) { //Find the devices _cprintf("Robot found\r\n"); ModaCPP::DeviceExternalCommunicationPort *pDevice=robot->QueryDeviceExternalCommunicationPort("port"); ModaCPP::DeviceMotor *pMLeft=robot->QueryDeviceMotor("joint_left/axis_red/motor"); ModaCPP::DeviceMotor *pMRight=robot->QueryDeviceMotor("joint_right/axis_red/motor"); if(pDevice && pMLeft && pMRight) { //OK ! _cprintf("Device 'port' and motors OK\r\n"); _cprintf("Use Telnet on Port 20000 and press keys E, D, S, F in order to make the robot to move\r\n"); DoDialer(pDevice,pMLeft,pMRight); delete pDevice; delete pMLeft; delete pMRight; } else { _cprintf("Device 'port' or motors devices not found\r\n"); } delete robot; } else { _cprintf("Robot '%s' not found\r\n",RobotName.GetData()); } } else { _cprintf("Unable to connect to moda server '%s'\r\n",Server.GetData()); } //Disconnect & delete connection->Disconnect(); delete connection; _getch(); return 0; }