File content as of revision 11:975d24305af7:

#include "RobotLeg.h"




RobotLeg::RobotLeg(PinName thetaPin, PinName phiPin, PinName psiPin, bool start) : theta(thetaPin, start), phi(phiPin, start), psi(psiPin, start)
{
    setDimensions(0.1f, 0.1f, 0.0f, 0.0f);
    setAngleOffsets(0.0f, 0.0f, 0.0f);
    
    state = neutral;
    stepTime = 0.4f;
    stepDelta = 3.141593f / stepTime;
    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;
}



void RobotLeg::setStepCircle(float xc, float yc, float zc, float rc)
{
    circleCenter = vector3(xc, yc, zc);
    circleRadius = rc;
}



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



vector3 RobotLeg::reset(float f)
{
    vector3 newPosition;
    newPosition = circleCenter + nDeltaPosition.unit() * circleRadius * f;
    step(newPosition);
    return nDeltaPosition;
}



bool RobotLeg::update(const matrix4& deltaTransform)
{
    float t, d;
    vector3 newPosition, newNDeltaPosition, v;
    const float eps = 0.000001f;

    switch (state)
    {
    case neutral:
        // Calculate new position and position delta
        newPosition = deltaTransform*position;
        newNDeltaPosition = position - newPosition;
        if (fabs(newNDeltaPosition.x) > eps || fabs(newNDeltaPosition.y) > eps || fabs(newNDeltaPosition.z) > eps)
            nDeltaPosition = newNDeltaPosition;
        position = newPosition;
        
        // Check if new position is outside the step circle
        v = position - circleCenter;
        d = sqrt(v.x*v.x + v.y*v.y);
        
        // Attempt to move to the new position
        if (!move(position) || d > circleRadius) return false;
        
        break;
        
    case stepping:
    // Compute new position along step trajectory
        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 < stepTime)
        {
            move(newPosition);
        }
        else
        {
            move(stepB);
            state = neutral;
            stepTimer.stop();
            position = getPosition();
        }
        break;
    }
    
    return true;
}