1
Dependencies: CircularBuffer Radio Servo Terminal mbed
Fork of WalkingRobot by
RobotLeg.cpp
- Committer:
- pclary
- Date:
- 2013-01-13
- Revision:
- 5:475f67175510
- Parent:
- 3:6fa07ceb897f
- Child:
- 6:0163f2737cc6
File content as of revision 5:475f67175510:
#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(); nDeltaPosition = vector3(0.0f, 0.01f, 0.0f); } 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; } void RobotLeg::reset(float f) { vector3 newPosition; newPosition = circleCenter + nDeltaPosition.unit() * circleRadius * f; step(newPosition); } bool RobotLeg::update(const matrix4& deltaTransform) { float t; vector3 newPosition; switch (state) { case neutral: newPosition = deltaTransform*position; nDeltaPosition = position - newPosition; position = newPosition; if (!move(position)) { // Project path forward from circle center reset(1.0f); return false; } 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; } return true; }