void NxRobot::goToThisPose( NxReal x, NxReal y, NxReal angle )
{
//Controle proporcional
//Velocidades Maximas do nosso robô: 17,5 x pi x 0,057m = 3,13 m/s (o da Skuba, atual campeã mundial: 3,5m/s)
//Rotacao maxima: 6,180582777638119118568314880347
NxReal maxSpeedAng = 6.2;
NxReal maxLinearSpeed = 3130;
//Controle de angulo
NxReal distanceAngle = angle - this->getAngle2DFromVehicle(); //TODO: remover redundância de getAngle2DFromRobot tanto em goToThisPose quanto em calcWheelSpeedFromRobotSpeed
NxReal speedAng;
if(NxMath::abs(distanceAngle) > NxPi / 72.) speedAng = distanceAngle / NxPi * maxSpeedAng;
else speedAng = 0;
//Controle de posicao
NxVec3 position = this->getPos();
NxReal distanceX = x - position.x;
NxReal distanceY = y - position.y;
NxReal distance = NxMath1::calcDistanceVec2D( position.x, position.y, x, y);
NxReal distanceThreshold = 2000.;
NxReal speedX;
NxReal speedY;
NxReal speed;
if(NxMath::abs(distance) > 10.)
{
if( distance > distanceThreshold )
if( distance > 0 ) speed = maxLinearSpeed;
else speed = - maxLinearSpeed;
else
speed = distance / distanceThreshold * maxLinearSpeed;
speedX = speed * NxMath::cos( NxMath::atan2( distanceY, distanceX ) );
speedY = speed * NxMath::sin( NxMath::atan2( distanceY, distanceX ) );
}
else
{
speedX = 0;
speedY = 0;
}
controlRobot(speedX, speedY, speedAng, 0, 0);
}
void Doctor::doExecuteLoop()
{
if (RCmode == "doctor")
{
controlRobot();
return;
}
if (homeVisit)
{
attendPatient();
}
if (goHome)
{
goToNode("nodeHouseDoor");
}
}
