Diff: RobotLeg.cpp

Revision:

3:6fa07ceb897f

Child:

5:475f67175510

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotLeg.cpp	Sat Nov 03 17:20:07 2012 +0000
@@ -0,0 +1,146 @@
+#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;
+    }
+}
\ No newline at end of file