1
Dependencies: CircularBuffer Radio Servo Terminal mbed
Fork of WalkingRobot by
RobotLeg.cpp
- Committer:
- pclary
- Date:
- 2013-04-09
- Revision:
- 10:dc1ba352667e
- Parent:
- 9:a6d1502f0f20
File content as of revision 10:dc1ba352667e:
#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; }