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"); } }