void NaoHeadControl::ReturnToBall()
{
float pitch, yaw;
pitch = gNormalizeDeg(WM.getVisionSense(BALL).phi);
yaw = gNormalizeDeg(WM.getVisionSense(BALL).theta);
aLOG << "yaw : " << yaw << " pitch : "<< pitch << endl;
setJoints(yaw, pitch);
}
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;
}
