int main(int argc,char* argv[]) { int portNb=0; int leftMotorHandle; int rightMotorHandle; int sensorHandle; if (argc>=5) { portNb=atoi(argv[1]); leftMotorHandle=atoi(argv[2]); rightMotorHandle=atoi(argv[3]); sensorHandle=atoi(argv[4]); } else { printf("Indicate following arguments: 'portNumber leftMotorHandle rightMotorHandle sensorHandle'!\n"); extApi_sleepMs(5000); return 0; } int clientID=simxStart((simxChar*)"127.0.0.1",portNb,true,true,2000,5); if (clientID!=-1) { int driveBackStartTime=-99000; float motorSpeeds[2]; while (simxGetConnectionId(clientID)!=-1) { simxUChar sensorTrigger=0; if (simxReadProximitySensor(clientID,sensorHandle,&sensorTrigger,NULL,NULL,NULL,simx_opmode_streaming)==simx_return_ok) { // We succeeded at reading the proximity sensor int simulationTime=simxGetLastCmdTime(clientID); if (simulationTime-driveBackStartTime<3000) { // driving backwards while slightly turning: motorSpeeds[0]=-3.1415f*0.5f; motorSpeeds[1]=-3.1415f*0.25f; } else { // going forward: motorSpeeds[0]=3.1415f; motorSpeeds[1]=3.1415f; if (sensorTrigger) driveBackStartTime=simulationTime; // We detected something, and start the backward mode } simxSetJointTargetVelocity(clientID,leftMotorHandle,motorSpeeds[0],simx_opmode_oneshot); simxSetJointTargetVelocity(clientID,rightMotorHandle,motorSpeeds[1],simx_opmode_oneshot); } extApi_sleepMs(5); } simxFinish(clientID); } return(0); }
int RobotVREP::getLastCmdTime() { return simxGetLastCmdTime(clientID); }