1
Dependencies: CircularBuffer Radio Servo Terminal mbed
Fork of WalkingRobot by
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