float NaoHeadControl::headPositionDistanceToActualPosition(Vector3f comp,bool leftSide) { float pos; pos = gRadToDeg(NAO->GetLink()[Nao::JID_HEAD_1].q) ; return -gAbs(pos-comp.y()); }
void NaoHeadControl::LookAtBall() { if(WM.SeeBall()) { float pitch, yaw; pitch = gNormalizeDeg(WM.getVisionSense(BALL).phi); yaw = gNormalizeDeg(WM.getVisionSense(BALL).theta); aLOG << "yaw : " << yaw << " pitch : "<< pitch << endl; pitch = pitch + gRadToDeg(NAO->GetLink()[Nao::JID_HEAD_1].q); yaw = yaw + gRadToDeg(NAO->GetLink()[Nao::JID_HEAD_2].q); aLOG << "yaw : " << yaw << " pitch : "<< pitch << endl; setJointsDirect(yaw,pitch); } else { ScanAllDirection(); } }
void NaoHeadControl::LookAtOppGoal() { oLOG << "Time: " << WM.getFieldInfo().match_time; const VisionSense & oppLeftGoal = WM.getVisionSense(GOAL_1_R); const VisionSense & oppRightGoal = WM.getVisionSense(GOAL_2_R); const VisionSense & ball = WM.getVisionSense(BALL); static bool InScanMode = false; if( InScanMode == true || (WM.getVisionState() && (oppLeftGoal.distance < 0.0 || oppRightGoal.distance < 0.0 || ball.distance < 0.0)) ) { ScanAllDirection(); if( WM.getVisionState() && ( oppLeftGoal.distance > 0.0 && oppRightGoal.distance > 0.0 && ball.distance > 0.0 ) ) InScanMode = false; else InScanMode = true; } else { oLOG << "Can See Both of the Goals" << endl; float yaw, pitch; yaw = (oppLeftGoal.theta + oppRightGoal.theta) / 2.0; yaw = (yaw + gNormalizeDeg(ball.theta)) / 2.0; pitch = (oppLeftGoal.phi + oppRightGoal.phi) / 2.0; pitch = (pitch + gNormalizeDeg(ball.theta)) / 2.0; yaw = gNormalizeDeg(yaw) + gRadToDeg(NAO->GetLink()[Nao::JID_HEAD_2].q); pitch = gNormalizeDeg(pitch) + gRadToDeg(NAO->GetLink()[Nao::JID_HEAD_1].q); setJointsDirect(yaw,pitch); oLOG << "yaw: " << yaw << " pitch: " << pitch << endl; oLOG << "oppLeftGoal.phi: " << oppLeftGoal.phi << " oppLeftGoal.theta: " << oppLeftGoal.theta << endl; oLOG << "oppRightGoal.phi: " << oppRightGoal.phi << " oppRightGoal.theta: " << oppRightGoal.theta << endl; boost::shared_array<Robot::Link> uLink = NAO->GetLink(); oLOG << gRadToDeg(uLink[Nao::JID_HEAD_1].q) << ' ' << gRadToDeg(uLink[Nao::JID_HEAD_2].q) << endl; } }
void Effectors::calculateVel(EJointID id, float angle, float maxVel) { if (maxVel < 0) { std::cout << "calculateVel Error"; mJointVel[id] = 0.0f; return ; } float curAngle = gRadToDeg(mJointAngle[id]); float minus = gNormalizeDeg(angle - curAngle); float vel = 0.0; vel = gAbs(minus) > maxVel ? maxVel * gSign(minus) : minus; vel = std::min(gDegToRad(vel) * 20.0f, 200.0f); mJointVel[id] = vel; }