bool rys::isTheObstacleAhead(float distance)
{
int state;
simxGetIntegerSignal(clientID,positionSignal.c_str(),&state,simx_opmode_streaming);
float linVel;
simxGetFloatSignal(clientID,linVelSignal.c_str(),&linVel,simx_opmode_streaming);
float orientation[3];
simxGetObjectOrientation(clientID, cuboidHandle, -1, orientation, simx_opmode_streaming);
if (state==1)
{
if (sensorUpVal>0 and sensorUpVal<distance)
return true;
}
else if (state==0)
{
if (orientation[1]<3*3.14/180 and orientation[1]>-3*3.14/180)
{
if (linVel<0)
{
if (sensorBackVal>0 and sensorBackVal<distance+0.05)
return true;
}
else
if (sensorFrontVal>0 and sensorFrontVal<distance+0.05)
return true;
}
}
return false;
}
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);
}
vector < double > RobotVREP::getObjectOrientation(Object * object, int relativeTo, simxInt operationMode)
{
int uniqueId = object->getUniqueObjectId();
int vrepHandlerVectorPosition = objectIdToVrepHandler_map.at(uniqueId);
int * handler = VrepHandlerVector.at(vrepHandlerVectorPosition);
float * ori = new float[3];
int error = simxGetObjectOrientation(clientID,*handler, relativeTo, ori, operationMode);
if(error != 0) vrep_error << "simxGetObjectOrientation - " << *handler << " : "<< error << endl;
vector < double > orientation;
orientation.push_back((double)ori[0]);
orientation.push_back((double)ori[1]);
orientation.push_back((double)ori[2]);
delete[] ori;
return orientation;
}
