File content as of revision 3:6fa07ceb897f:

#include "RobotLeg.h"




RobotLeg::RobotLeg(PinName thetaPin, PinName phiPin, PinName psiPin) : theta(thetaPin), phi(phiPin), psi(psiPin)
{
    setDimensions(0.1f, 0.1f, 0.0f, 0.0f);
    setAngleOffsets(0.0f, 0.0f, 0.0f);
    
    state = neutral;
    stepPeriod = 0.5f;
    stepDelta = 3.141593f / stepPeriod;
    stepHeight = 0.05f;

    position = getPosition();
}



void RobotLeg::setDimensions(float a, float b, float c, float d)
{
    this->a = a;
    this->b = b;
    this->c = c;
    this->d = d;
}



void RobotLeg::setAngleOffsets(float oth, float oph, float ops)
{
    this->oth = oth;
    this->oph = oph;
    this->ops = ops;
}



vector3 RobotLeg::getPosition()
{
    const float deg2rad = 0.01745329f;
    vector3 p;
    float L, thetaR, phiR, psiR;
    
    // Convert degrees to radians
    thetaR = theta.read() * deg2rad;
    phiR = phi.read() * deg2rad;
    psiR = psi.read() * deg2rad;
    
    // Calculate forward kinematics
    L = a*cos(phiR + oph) + b*sin(phiR + oph + psiR + ops);
    p.x = -c*sin(thetaR + oth) + (L + d)*cos(thetaR + oth);
    p.y = c*cos(thetaR + oth) + (L + d)*sin(thetaR + oth);
    p.z = a*sin(phiR + oph) - b*cos(phiR + oph + psiR + ops);
    
    return p;
}



bool RobotLeg::move(vector3 dest)
{
    const float pi2 = 1.5707963;
    const float rad2deg =  57.2958;
    float L;
    
    // Calculate new angles
    L = sqrt(dest.x*dest.x + dest.y*dest.y - c*c) - d;
    thetaAngle = atan2( ((L + d)*dest.y - c*dest.x), ((L + d)*dest.x + c*dest.y) ) - oth;
    phiAngle = atan2(dest.z, L) + acos((a*a + L*L + dest.z*dest.z - b*b)/(2.f*a*sqrt(L*L + dest.z*dest.z))) - oph;
    psiAngle = acos((a*a + b*b - L*L - dest.z*dest.z)/(2*a*b)) - ops - pi2;
    
    // Convert radians to degrees
    thetaAngle *= rad2deg;
    phiAngle *= rad2deg;
    psiAngle *= rad2deg;
    
    // Return true if angle is reachable
    if (thetaAngle <= theta.upperLimit && thetaAngle >= theta.lowerLimit &&
        phiAngle <= phi.upperLimit && phiAngle >= phi.lowerLimit &&
        psiAngle <= psi.upperLimit && psiAngle >= psi.lowerLimit)
    {
        // Set new angles
        theta = thetaAngle;
        phi = phiAngle;
        psi = psiAngle;
    
        return true;
    }
    else
    {
        return false;
    }
}



void RobotLeg::step(vector3 dest)
{
    stepA = getPosition();
    stepB = dest;

    stepTimer.reset();
    stepTimer.start();
    state = stepping;
}



void RobotLeg::update(const matrix4& deltaTransform)
{
    float t;
    vector3 newPosition;

    switch (state)
    {
    case neutral:
        position = deltaTransform*position;
        if (!move(position))
        {
            // Eventually, use the inverse matrix to calculate the reverse path
            step(vector3(0.1f, 0.15f, -0.05f));
        }
        break;
        
    case stepping:
        t = stepTimer.read();
        newPosition.x = stepA.x + (stepB.x - stepA.x)*0.5f*(1 - cos(stepDelta*t));
        newPosition.y = stepA.y + (stepB.y - stepA.y)*0.5f*(1 - cos(stepDelta*t));
        newPosition.z = stepA.z + (stepB.z - stepA.z)*stepDelta*t + stepHeight*sin(stepDelta*t);
        
        if (t < stepPeriod)
        {
            move(newPosition);
        }
        else
        {
            move(stepB);
            state = neutral;
            stepTimer.stop();
            position = getPosition();
        }
        break;
    }
}