int MovetoPoint(float *GoalPosition, float minDistance, int clientID, int leftMotorHandle, int rightMotorHandle, int cuboidHandle)
{
float radius=0.25;
float axis=0.5;
float P=0.7;
float LinVel=0.3;
float AngVel;
float ObjectPosition[3];
float ObjectOrientation[3]; //rotation about Z --> ObjectOrientation[2]
float OrientationError;
simxGetObjectPosition(clientID,cuboidHandle,-1,ObjectPosition,simx_opmode_oneshot_wait);
float distance=sqrt(pow(ObjectPosition[0]-GoalPosition[0],2)+pow(ObjectPosition[1]-GoalPosition[1],2)+pow(ObjectPosition[2]-GoalPosition[2],2));
auto snow = std::chrono::system_clock::now();
auto sduration = snow.time_since_epoch();
auto smillis = std::chrono::duration_cast<std::chrono::milliseconds>(sduration).count();
while (distance>minDistance)
{
simxGetObjectOrientation(clientID, cuboidHandle, -1, ObjectOrientation, simx_opmode_streaming);
simxGetObjectPosition(clientID,cuboidHandle,-1,ObjectPosition,simx_opmode_oneshot_wait);
OrientationError=orientationError(ObjectPosition[0], ObjectPosition[1], ObjectOrientation[2], GoalPosition[0], GoalPosition[1]);
AngVel=P*OrientationError;
MoveandRotate(LinVel, AngVel, radius, axis, clientID, leftMotorHandle, rightMotorHandle);
printf("Distance: %f Robot: %f Error: %f\n", distance, ObjectOrientation[2], OrientationError);
distance=sqrt(pow(ObjectPosition[0]-GoalPosition[0],2)+pow(ObjectPosition[1]-GoalPosition[1],2));
//printing data to file
auto now = std::chrono::system_clock::now();
auto duration = now.time_since_epoch();
auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
//std::cout << millis-smillis << "\t" << ObjectPosition[0] << "\t" << ObjectPosition[1] << std::endl;
}
MoveandRotate(0, 0, radius, axis, clientID, leftMotorHandle, rightMotorHandle);
}
std::tuple<float, float, float> Object::globalPosition() const {
simxFloat *positionVec = new simxFloat[3];
try {
ReturnCodesExceptions::handleReturnCode(
simxGetObjectPosition(m_ClientId, m_Handle, -1, positionVec, simx_opmode_oneshot_wait));
return std::make_tuple(static_cast<float>(positionVec[0]), positionVec[1], positionVec[2]);
}
catch (ReturnCodesExceptions *ex) {
throw ex;
}
}
vector < double > RobotVREP::getObjectPosition(Object * object, int relativeTo, simxInt operationMode)
{
int uniqueId = object->getUniqueObjectId();
int vrepHandlerVectorPosition = objectIdToVrepHandler_map.at(uniqueId);
int * handler = VrepHandlerVector.at(vrepHandlerVectorPosition);
float * pos = new float[3];
int error = simxGetObjectPosition(clientID, *handler, relativeTo, pos, operationMode);
if(error != 0) vrep_error << "simxGetObjectPosition - " << *handler << " : "<< error << endl;
vector < double > position;
position.push_back((double)pos[0]);
position.push_back((double)pos[1]);
position.push_back((double)pos[2]);
delete[] pos;
return position;
}
void rys::setTarget()
{
simxGetObjectPosition(clientID,goalHandle,-1,goalPosition,simx_opmode_oneshot_wait);
}
void Individuo::initStreamingSensors() {
//cout << this->robotName << ": Conexao com motores, sensores e corpo do robo #" + to_string(this->id) + "\n";
sensorName = "Pioneer_p3dx_robot_" + to_string(this->id);
if (simxGetObjectHandle(this->clientId, sensorName.c_str(),
&body, simx_opmode_oneshot_wait) == simx_return_ok) {
//cout << this->robotName << ": Conectado corpo do motor." << std::endl;
simxGetObjectPosition(clientId, body, -1, position, simx_opmode_streaming);
}
else {
cout << this->robotName << ": Nao conectado ao corpo do motor." << std::endl;
}
sensorName = "Pioneer_p3dx_leftMotor_robot_" + to_string(this->id);
if (simxGetObjectHandle(this->clientId, sensorName.c_str(),
&motorLeft, simx_opmode_oneshot_wait) == simx_return_ok) {
//cout << this->robotName << ": Conectado no motor esquerdo." << std::endl;
}
else
cout << this->robotName << ": Nao conectado no motor esquerdo." << std::endl;
sensorName = "Pioneer_p3dx_rightMotor_robot_" + to_string(this->id);
if (simxGetObjectHandle(this->clientId, sensorName.c_str(),
&motorRight, simx_opmode_oneshot_wait) == simx_return_ok){
//cout << this->robotName << ": Conectado no motor direito." << std::endl;
}
else
cout << this->robotName << ": Nao conectado no motor direito." << std::endl;
sensorName = "Pioneer_p3dx_caster_freeJoint_robot_" + to_string(this->id);
if (simxGetObjectHandle(this->clientId, sensorName.c_str(),
&casterWheel, simx_opmode_oneshot_wait) == simx_return_ok) {
// cout << this->robotName << ": Conectado a rodinha livre." << std::endl;
}
else
cout << this->robotName << ": Nao conectado a rodinha livre." << std::endl;
for (int i = 0; i < 2; i++) {
sensorName = "LaserPointer_sensor_front_" + to_string(i + 1)+"_robot_"+to_string(this->id);
if (simxGetObjectHandle(this->clientId, sensorName.c_str(), &laserPointerFront[i], simx_opmode_oneshot_wait) == simx_return_ok) {
//cout << this->robotName << ": Conectado ao laser pointer front #" << i + 1 << endl;
simxReadProximitySensor(this->clientId, laserPointerFront[i], NULL, NULL,
NULL, NULL, simx_opmode_streaming);
}else {
cout << this->robotName << ": Nao conectado ao laser pointer front #" << i + 1 << endl;
}
sensorName = "LaserPointer_sensor_back_" + to_string(i + 1) + "_robot_" + to_string(this->id);
if (simxGetObjectHandle(this->clientId, sensorName.c_str(), &laserPointerBack[i], simx_opmode_oneshot_wait) == simx_return_ok) {
//cout << this->robotName << ": Conectado ao laser pointer back #" << i + 1 << endl;
simxReadProximitySensor(this->clientId, laserPointerBack[i], NULL, NULL,
NULL, NULL, simx_opmode_streaming);
}else {
cout << this->robotName << ": Nao conectado ao laser pointer back" << i + 1 << endl;
}
sensorName = "LaserPointer_sensor_right_" + to_string(i + 1) + "_robot_" + to_string(this->id);
if (simxGetObjectHandle(this->clientId, sensorName.c_str(), &laserPointerRight[i], simx_opmode_oneshot_wait) == simx_return_ok) {
//cout << this->robotName << ": Conectado ao laser pointer right #" << i + 1 << endl;
simxReadProximitySensor(this->clientId, laserPointerRight[i], NULL, NULL,
NULL, NULL, simx_opmode_streaming);
}else {
cout << this->robotName << ": Nao conectado ao laser pointer right" << i + 1 << endl;
}
sensorName = "LaserPointer_sensor_left_" + to_string(i + 1) + "_robot_" + to_string(this->id);
if (simxGetObjectHandle(this->clientId, sensorName.c_str(), &laserPointerLeft[i], simx_opmode_oneshot_wait) == simx_return_ok) {
//cout << this->robotName << ": Conectado ao laser pointer back #" << i + 1 << endl;
simxReadProximitySensor(this->clientId, laserPointerLeft[i], NULL, NULL,
NULL, NULL, simx_opmode_streaming);
}else {
cout << this->robotName << ": Nao conectado ao laser pointer back" << i + 1 << endl;
}
}
/*simxGetObjectPosition(clientId, body, -1, initPosBody, simx_opmode_oneshot_wait);
simxGetObjectPosition(clientId, motorLeft, -1, initPosLeftWheel, simx_opmode_oneshot_wait);
simxGetObjectPosition(clientId, motorRight, -1, initPosRightWheel, simx_opmode_oneshot_wait);
simxGetObjectPosition(clientId, casterWheel, -1, initPosCasterWheel, simx_opmode_oneshot_wait);*/
/*cout << "X: " << initPosBody[0] << " Y: " << initPosBody[1] << " Z: " << initPosBody[2] << endl;
cout << "X: " << initPosLeftWheel[0] << " Y: " << initPosLeftWheel[1] << " Z: " << initPosLeftWheel[2] << endl;
cout << "X: " << initPosRightWheel[0] << " Y: " << initPosRightWheel[1] << " Z: " << initPosRightWheel[2] << endl;
cout << "X: " << initPosCasterWheel[0] << " Y: " << initPosCasterWheel[1] << " Z: " << initPosCasterWheel[2] << endl;*/
}
bool Individuo::freeFall() {
float pos[3];
simxGetObjectPosition(clientId, body, -1, position, simx_opmode_buffer);
return (position[2] < MIN_HEIGHT) ? true : false;
}
int main(int argc,char* argv[])
{
int portNb=0;
int leftMotorHandle;
int rightMotorHandle;
int cuboidHandle;
int goalHandle;
if (argc>=6)
{
portNb=atoi(argv[1]);
leftMotorHandle=atoi(argv[2]);
rightMotorHandle=atoi(argv[3]);
cuboidHandle=atoi(argv[4]);
goalHandle=atoi(argv[5]);
}
else
{
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];
float leftMotorAngle;
float ObjectPosition[3];
float GoalPosition[3];
float ObjectOrientation[3];
float minDistance=0.3;
if (simxGetConnectionId(clientID)!=-1)
{
simxUChar sensorTrigger=0;
simxGetObjectHandle(clientID, argv[2], &leftMotorHandle, simx_opmode_blocking);
simxGetObjectHandle(clientID, argv[3], &rightMotorHandle, simx_opmode_blocking);
simxGetObjectHandle(clientID, argv[4], &cuboidHandle, simx_opmode_blocking);
simxGetObjectHandle(clientID, argv[5], &goalHandle, simx_opmode_blocking);
simxGetObjectPosition(clientID,goalHandle,-1,GoalPosition,simx_opmode_oneshot_wait);
MovetoPoint(GoalPosition, minDistance, clientID, leftMotorHandle, rightMotorHandle, cuboidHandle);
simxSetJointTargetVelocity(clientID,leftMotorHandle,0,simx_opmode_blocking);
simxSetJointTargetVelocity(clientID,rightMotorHandle,0,simx_opmode_blocking);
extApi_sleepMs(5);
}
printf("Fin!\n");
simxFinish(clientID);
}
return(0);
}
