Alex Chepilev / Mbed 2 deprecated WalkingRobot

1

Dependencies:   CircularBuffer Radio Servo Terminal mbed

Fork of WalkingRobot by Patrick Clary

RobotLeg.cpp@5:475f67175510, 2013-01-13 (annotated)

Committer:
pclary
Date:
Sun Jan 13 02:59:48 2013 +0000
Revision:
5:475f67175510
Parent:
3:6fa07ceb897f
Child:
6:0163f2737cc6
Changed things, terminal isn't working properly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pclary 3:6fa07ceb897f 1 #include "RobotLeg.h"
pclary 3:6fa07ceb897f 2
pclary 3:6fa07ceb897f 3
pclary 3:6fa07ceb897f 4
pclary 3:6fa07ceb897f 5
pclary 3:6fa07ceb897f 6 RobotLeg::RobotLeg(PinName thetaPin, PinName phiPin, PinName psiPin) : theta(thetaPin), phi(phiPin), psi(psiPin)
pclary 3:6fa07ceb897f 7 {
pclary 3:6fa07ceb897f 8 setDimensions(0.1f, 0.1f, 0.0f, 0.0f);
pclary 3:6fa07ceb897f 9 setAngleOffsets(0.0f, 0.0f, 0.0f);
pclary 3:6fa07ceb897f 10
pclary 3:6fa07ceb897f 11 state = neutral;
pclary 3:6fa07ceb897f 12 stepPeriod = 0.5f;
pclary 3:6fa07ceb897f 13 stepDelta = 3.141593f / stepPeriod;
pclary 3:6fa07ceb897f 14 stepHeight = 0.05f;
pclary 3:6fa07ceb897f 15
pclary 3:6fa07ceb897f 16 position = getPosition();
pclary 5:475f67175510 17 nDeltaPosition = vector3(0.0f, 0.01f, 0.0f);
pclary 3:6fa07ceb897f 18 }
pclary 3:6fa07ceb897f 19
pclary 3:6fa07ceb897f 20
pclary 3:6fa07ceb897f 21
pclary 3:6fa07ceb897f 22 void RobotLeg::setDimensions(float a, float b, float c, float d)
pclary 3:6fa07ceb897f 23 {
pclary 3:6fa07ceb897f 24 this->a = a;
pclary 3:6fa07ceb897f 25 this->b = b;
pclary 3:6fa07ceb897f 26 this->c = c;
pclary 3:6fa07ceb897f 27 this->d = d;
pclary 3:6fa07ceb897f 28 }
pclary 3:6fa07ceb897f 29
pclary 3:6fa07ceb897f 30
pclary 3:6fa07ceb897f 31
pclary 3:6fa07ceb897f 32 void RobotLeg::setAngleOffsets(float oth, float oph, float ops)
pclary 3:6fa07ceb897f 33 {
pclary 3:6fa07ceb897f 34 this->oth = oth;
pclary 3:6fa07ceb897f 35 this->oph = oph;
pclary 3:6fa07ceb897f 36 this->ops = ops;
pclary 3:6fa07ceb897f 37 }
pclary 3:6fa07ceb897f 38
pclary 3:6fa07ceb897f 39
pclary 3:6fa07ceb897f 40
pclary 5:475f67175510 41 void RobotLeg::setStepCircle(float xc, float yc, float zc, float rc)
pclary 5:475f67175510 42 {
pclary 5:475f67175510 43 circleCenter = vector3(xc, yc, zc);
pclary 5:475f67175510 44 circleRadius = rc;
pclary 5:475f67175510 45 }
pclary 5:475f67175510 46
pclary 5:475f67175510 47
pclary 5:475f67175510 48
pclary 3:6fa07ceb897f 49 vector3 RobotLeg::getPosition()
pclary 3:6fa07ceb897f 50 {
pclary 3:6fa07ceb897f 51 const float deg2rad = 0.01745329f;
pclary 3:6fa07ceb897f 52 vector3 p;
pclary 3:6fa07ceb897f 53 float L, thetaR, phiR, psiR;
pclary 3:6fa07ceb897f 54
pclary 3:6fa07ceb897f 55 // Convert degrees to radians
pclary 3:6fa07ceb897f 56 thetaR = theta.read() * deg2rad;
pclary 3:6fa07ceb897f 57 phiR = phi.read() * deg2rad;
pclary 3:6fa07ceb897f 58 psiR = psi.read() * deg2rad;
pclary 3:6fa07ceb897f 59
pclary 3:6fa07ceb897f 60 // Calculate forward kinematics
pclary 3:6fa07ceb897f 61 L = a*cos(phiR + oph) + b*sin(phiR + oph + psiR + ops);
pclary 3:6fa07ceb897f 62 p.x = -c*sin(thetaR + oth) + (L + d)*cos(thetaR + oth);
pclary 3:6fa07ceb897f 63 p.y = c*cos(thetaR + oth) + (L + d)*sin(thetaR + oth);
pclary 3:6fa07ceb897f 64 p.z = a*sin(phiR + oph) - b*cos(phiR + oph + psiR + ops);
pclary 3:6fa07ceb897f 65
pclary 3:6fa07ceb897f 66 return p;
pclary 3:6fa07ceb897f 67 }
pclary 3:6fa07ceb897f 68
pclary 3:6fa07ceb897f 69
pclary 3:6fa07ceb897f 70
pclary 3:6fa07ceb897f 71 bool RobotLeg::move(vector3 dest)
pclary 3:6fa07ceb897f 72 {
pclary 3:6fa07ceb897f 73 const float pi2 = 1.5707963;
pclary 3:6fa07ceb897f 74 const float rad2deg = 57.2958;
pclary 3:6fa07ceb897f 75 float L;
pclary 3:6fa07ceb897f 76
pclary 3:6fa07ceb897f 77 // Calculate new angles
pclary 3:6fa07ceb897f 78 L = sqrt(dest.x*dest.x + dest.y*dest.y - c*c) - d;
pclary 3:6fa07ceb897f 79 thetaAngle = atan2( ((L + d)*dest.y - c*dest.x), ((L + d)*dest.x + c*dest.y) ) - oth;
pclary 3:6fa07ceb897f 80 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;
pclary 3:6fa07ceb897f 81 psiAngle = acos((a*a + b*b - L*L - dest.z*dest.z)/(2*a*b)) - ops - pi2;
pclary 3:6fa07ceb897f 82
pclary 3:6fa07ceb897f 83 // Convert radians to degrees
pclary 3:6fa07ceb897f 84 thetaAngle *= rad2deg;
pclary 3:6fa07ceb897f 85 phiAngle *= rad2deg;
pclary 3:6fa07ceb897f 86 psiAngle *= rad2deg;
pclary 3:6fa07ceb897f 87
pclary 3:6fa07ceb897f 88 // Return true if angle is reachable
pclary 3:6fa07ceb897f 89 if (thetaAngle <= theta.upperLimit && thetaAngle >= theta.lowerLimit &&
pclary 3:6fa07ceb897f 90 phiAngle <= phi.upperLimit && phiAngle >= phi.lowerLimit &&
pclary 3:6fa07ceb897f 91 psiAngle <= psi.upperLimit && psiAngle >= psi.lowerLimit)
pclary 3:6fa07ceb897f 92 {
pclary 3:6fa07ceb897f 93 // Set new angles
pclary 3:6fa07ceb897f 94 theta = thetaAngle;
pclary 3:6fa07ceb897f 95 phi = phiAngle;
pclary 3:6fa07ceb897f 96 psi = psiAngle;
pclary 3:6fa07ceb897f 97
pclary 3:6fa07ceb897f 98 return true;
pclary 3:6fa07ceb897f 99 }
pclary 3:6fa07ceb897f 100 else
pclary 3:6fa07ceb897f 101 {
pclary 3:6fa07ceb897f 102 return false;
pclary 3:6fa07ceb897f 103 }
pclary 3:6fa07ceb897f 104 }
pclary 3:6fa07ceb897f 105
pclary 3:6fa07ceb897f 106
pclary 3:6fa07ceb897f 107
pclary 3:6fa07ceb897f 108 void RobotLeg::step(vector3 dest)
pclary 3:6fa07ceb897f 109 {
pclary 3:6fa07ceb897f 110 stepA = getPosition();
pclary 3:6fa07ceb897f 111 stepB = dest;
pclary 3:6fa07ceb897f 112
pclary 3:6fa07ceb897f 113 stepTimer.reset();
pclary 3:6fa07ceb897f 114 stepTimer.start();
pclary 3:6fa07ceb897f 115 state = stepping;
pclary 3:6fa07ceb897f 116 }
pclary 3:6fa07ceb897f 117
pclary 3:6fa07ceb897f 118
pclary 3:6fa07ceb897f 119
pclary 5:475f67175510 120 void RobotLeg::reset(float f)
pclary 5:475f67175510 121 {
pclary 5:475f67175510 122 vector3 newPosition;
pclary 5:475f67175510 123 newPosition = circleCenter + nDeltaPosition.unit() * circleRadius * f;
pclary 5:475f67175510 124 step(newPosition);
pclary 5:475f67175510 125 }
pclary 5:475f67175510 126
pclary 5:475f67175510 127
pclary 5:475f67175510 128
pclary 5:475f67175510 129 bool RobotLeg::update(const matrix4& deltaTransform)
pclary 3:6fa07ceb897f 130 {
pclary 3:6fa07ceb897f 131 float t;
pclary 3:6fa07ceb897f 132 vector3 newPosition;
pclary 3:6fa07ceb897f 133
pclary 3:6fa07ceb897f 134 switch (state)
pclary 3:6fa07ceb897f 135 {
pclary 3:6fa07ceb897f 136 case neutral:
pclary 5:475f67175510 137 newPosition = deltaTransform*position;
pclary 5:475f67175510 138 nDeltaPosition = position - newPosition;
pclary 5:475f67175510 139 position = newPosition;
pclary 3:6fa07ceb897f 140 if (!move(position))
pclary 3:6fa07ceb897f 141 {
pclary 5:475f67175510 142 // Project path forward from circle center
pclary 5:475f67175510 143 reset(1.0f);
pclary 5:475f67175510 144 return false;
pclary 3:6fa07ceb897f 145 }
pclary 3:6fa07ceb897f 146 break;
pclary 3:6fa07ceb897f 147
pclary 3:6fa07ceb897f 148 case stepping:
pclary 3:6fa07ceb897f 149 t = stepTimer.read();
pclary 3:6fa07ceb897f 150 newPosition.x = stepA.x + (stepB.x - stepA.x)*0.5f*(1 - cos(stepDelta*t));
pclary 3:6fa07ceb897f 151 newPosition.y = stepA.y + (stepB.y - stepA.y)*0.5f*(1 - cos(stepDelta*t));
pclary 3:6fa07ceb897f 152 newPosition.z = stepA.z + (stepB.z - stepA.z)*stepDelta*t + stepHeight*sin(stepDelta*t);
pclary 3:6fa07ceb897f 153
pclary 3:6fa07ceb897f 154 if (t < stepPeriod)
pclary 3:6fa07ceb897f 155 {
pclary 3:6fa07ceb897f 156 move(newPosition);
pclary 3:6fa07ceb897f 157 }
pclary 3:6fa07ceb897f 158 else
pclary 3:6fa07ceb897f 159 {
pclary 3:6fa07ceb897f 160 move(stepB);
pclary 3:6fa07ceb897f 161 state = neutral;
pclary 3:6fa07ceb897f 162 stepTimer.stop();
pclary 3:6fa07ceb897f 163 position = getPosition();
pclary 3:6fa07ceb897f 164 }
pclary 3:6fa07ceb897f 165 break;
pclary 3:6fa07ceb897f 166 }
pclary 5:475f67175510 167
pclary 5:475f67175510 168 return true;
pclary 3:6fa07ceb897f 169 }