예제 #1
0
float NaoHeadControl::headPositionDistanceToActualPosition(Vector3f comp,bool leftSide)
{
    float pos;
    pos = gRadToDeg(NAO->GetLink()[Nao::JID_HEAD_1].q) ;

    return -gAbs(pos-comp.y());

}
예제 #2
0
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;

}