Patrick Clary / Mbed 2 deprecated WalkingRobot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   CircularBuffer Servo Terminal mbed Radio

RobotLeg.cpp@17:4ec59e8b52a6, 2013-05-27 (annotated)

Committer:
pclary
Date:
Mon May 27 03:31:58 2013 +0000
Revision:
17:4ec59e8b52a6
Parent:
13:1c5d255835ce
Child:
18:8806d24809c2
Added a function for computing the distance to the step circle edge

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pclary 3:6fa07ceb897f 1 #include "RobotLeg.h"
pclary 17:4ec59e8b52a6 2 #include "utility.h"
pclary 3:6fa07ceb897f 3
pclary 3:6fa07ceb897f 4
pclary 3:6fa07ceb897f 5
pclary 8:db453051f3f4 6 RobotLeg::RobotLeg(PinName thetaPin, PinName phiPin, PinName psiPin, bool start) : theta(thetaPin, start), phi(phiPin, start), psi(psiPin, start)
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 6:0163f2737cc6 12 stepTime = 0.4f;
pclary 6:0163f2737cc6 13 stepDelta = 3.141593f / stepTime;
pclary 3:6fa07ceb897f 14 stepHeight = 0.05f;
pclary 3:6fa07ceb897f 15 }
pclary 3:6fa07ceb897f 16
pclary 3:6fa07ceb897f 17
pclary 3:6fa07ceb897f 18
pclary 3:6fa07ceb897f 19 void RobotLeg::setDimensions(float a, float b, float c, float d)
pclary 3:6fa07ceb897f 20 {
pclary 3:6fa07ceb897f 21 this->a = a;
pclary 3:6fa07ceb897f 22 this->b = b;
pclary 3:6fa07ceb897f 23 this->c = c;
pclary 3:6fa07ceb897f 24 this->d = d;
pclary 3:6fa07ceb897f 25 }
pclary 3:6fa07ceb897f 26
pclary 3:6fa07ceb897f 27
pclary 3:6fa07ceb897f 28
pclary 3:6fa07ceb897f 29 void RobotLeg::setAngleOffsets(float oth, float oph, float ops)
pclary 3:6fa07ceb897f 30 {
pclary 3:6fa07ceb897f 31 this->oth = oth;
pclary 3:6fa07ceb897f 32 this->oph = oph;
pclary 3:6fa07ceb897f 33 this->ops = ops;
pclary 3:6fa07ceb897f 34 }
pclary 3:6fa07ceb897f 35
pclary 3:6fa07ceb897f 36
pclary 3:6fa07ceb897f 37
pclary 5:475f67175510 38 void RobotLeg::setStepCircle(float xc, float yc, float zc, float rc)
pclary 5:475f67175510 39 {
pclary 5:475f67175510 40 circleCenter = vector3(xc, yc, zc);
pclary 5:475f67175510 41 circleRadius = rc;
pclary 5:475f67175510 42 }
pclary 5:475f67175510 43
pclary 5:475f67175510 44
pclary 5:475f67175510 45
pclary 3:6fa07ceb897f 46 vector3 RobotLeg::getPosition()
pclary 3:6fa07ceb897f 47 {
pclary 17:4ec59e8b52a6 48 return position;
pclary 17:4ec59e8b52a6 49
pclary 17:4ec59e8b52a6 50 /*const float deg2rad = 0.01745329f;
pclary 3:6fa07ceb897f 51 vector3 p;
pclary 3:6fa07ceb897f 52 float L, thetaR, phiR, psiR;
pclary 3:6fa07ceb897f 53
pclary 3:6fa07ceb897f 54 // Convert degrees to radians
pclary 3:6fa07ceb897f 55 thetaR = theta.read() * deg2rad;
pclary 3:6fa07ceb897f 56 phiR = phi.read() * deg2rad;
pclary 3:6fa07ceb897f 57 psiR = psi.read() * deg2rad;
pclary 3:6fa07ceb897f 58
pclary 3:6fa07ceb897f 59 // Calculate forward kinematics
pclary 3:6fa07ceb897f 60 L = a*cos(phiR + oph) + b*sin(phiR + oph + psiR + ops);
pclary 3:6fa07ceb897f 61 p.x = -c*sin(thetaR + oth) + (L + d)*cos(thetaR + oth);
pclary 3:6fa07ceb897f 62 p.y = c*cos(thetaR + oth) + (L + d)*sin(thetaR + oth);
pclary 3:6fa07ceb897f 63 p.z = a*sin(phiR + oph) - b*cos(phiR + oph + psiR + ops);
pclary 3:6fa07ceb897f 64
pclary 17:4ec59e8b52a6 65 return p;*/
pclary 17:4ec59e8b52a6 66 }
pclary 17:4ec59e8b52a6 67
pclary 17:4ec59e8b52a6 68
pclary 17:4ec59e8b52a6 69
pclary 17:4ec59e8b52a6 70 float RobotLeg::getStepDistance()
pclary 17:4ec59e8b52a6 71 {
pclary 17:4ec59e8b52a6 72 // Returns distance to step circle edge in the current direction of movement.
pclary 17:4ec59e8b52a6 73 float vx, vy, m, cosval;
pclary 17:4ec59e8b52a6 74
pclary 17:4ec59e8b52a6 75 vx = position.x - circleCenter.x;
pclary 17:4ec59e8b52a6 76 vy = position.y - circleCenter.y;
pclary 17:4ec59e8b52a6 77 m = sqrt(vx*vx + vy*vy);
pclary 17:4ec59e8b52a6 78 cosval = (nDeltaPosition.x*vx + nDeltaPosition.y*vy) /
pclary 17:4ec59e8b52a6 79 (m * sqrt(nDeltaPosition.x*nDeltaPosition.x + nDeltaPosition.y*nDeltaPosition.y));
pclary 17:4ec59e8b52a6 80
pclary 17:4ec59e8b52a6 81 return m*cosval + sqrt(pos(circleRadius*circleRadius - m*m*(1.0f - cosval*cosval)));
pclary 3:6fa07ceb897f 82 }
pclary 3:6fa07ceb897f 83
pclary 3:6fa07ceb897f 84
pclary 3:6fa07ceb897f 85
pclary 3:6fa07ceb897f 86 bool RobotLeg::move(vector3 dest)
pclary 3:6fa07ceb897f 87 {
pclary 3:6fa07ceb897f 88 const float pi2 = 1.5707963;
pclary 3:6fa07ceb897f 89 const float rad2deg = 57.2958;
pclary 3:6fa07ceb897f 90 float L;
pclary 3:6fa07ceb897f 91
pclary 17:4ec59e8b52a6 92 position = dest;
pclary 17:4ec59e8b52a6 93
pclary 3:6fa07ceb897f 94 // Calculate new angles
pclary 3:6fa07ceb897f 95 L = sqrt(dest.x*dest.x + dest.y*dest.y - c*c) - d;
pclary 3:6fa07ceb897f 96 thetaAngle = atan2( ((L + d)*dest.y - c*dest.x), ((L + d)*dest.x + c*dest.y) ) - oth;
pclary 12:a952bd74d363 97 phiAngle = atan2(dest.z, L) + acos((a*a + L*L + dest.z*dest.z - b*b)/(2.0f*a*sqrt(L*L + dest.z*dest.z))) - oph;
pclary 12:a952bd74d363 98 psiAngle = acos((a*a + b*b - L*L - dest.z*dest.z)/(2.0f*a*b)) - ops - pi2;
pclary 3:6fa07ceb897f 99
pclary 3:6fa07ceb897f 100 // Convert radians to degrees
pclary 3:6fa07ceb897f 101 thetaAngle *= rad2deg;
pclary 3:6fa07ceb897f 102 phiAngle *= rad2deg;
pclary 3:6fa07ceb897f 103 psiAngle *= rad2deg;
pclary 3:6fa07ceb897f 104
pclary 3:6fa07ceb897f 105 // Return true if angle is reachable
pclary 3:6fa07ceb897f 106 if (thetaAngle <= theta.upperLimit && thetaAngle >= theta.lowerLimit &&
pclary 3:6fa07ceb897f 107 phiAngle <= phi.upperLimit && phiAngle >= phi.lowerLimit &&
pclary 3:6fa07ceb897f 108 psiAngle <= psi.upperLimit && psiAngle >= psi.lowerLimit)
pclary 3:6fa07ceb897f 109 {
pclary 3:6fa07ceb897f 110 // Set new angles
pclary 3:6fa07ceb897f 111 theta = thetaAngle;
pclary 3:6fa07ceb897f 112 phi = phiAngle;
pclary 3:6fa07ceb897f 113 psi = psiAngle;
pclary 3:6fa07ceb897f 114
pclary 3:6fa07ceb897f 115 return true;
pclary 3:6fa07ceb897f 116 }
pclary 3:6fa07ceb897f 117 else
pclary 3:6fa07ceb897f 118 {
pclary 3:6fa07ceb897f 119 return false;
pclary 3:6fa07ceb897f 120 }
pclary 3:6fa07ceb897f 121 }
pclary 3:6fa07ceb897f 122
pclary 3:6fa07ceb897f 123
pclary 3:6fa07ceb897f 124
pclary 3:6fa07ceb897f 125 void RobotLeg::step(vector3 dest)
pclary 3:6fa07ceb897f 126 {
pclary 13:1c5d255835ce 127 stepA = position;
pclary 3:6fa07ceb897f 128 stepB = dest;
pclary 3:6fa07ceb897f 129
pclary 3:6fa07ceb897f 130 stepTimer.reset();
pclary 3:6fa07ceb897f 131 stepTimer.start();
pclary 3:6fa07ceb897f 132 state = stepping;
pclary 3:6fa07ceb897f 133 }
pclary 3:6fa07ceb897f 134
pclary 3:6fa07ceb897f 135
pclary 3:6fa07ceb897f 136
pclary 9:a6d1502f0f20 137 vector3 RobotLeg::reset(float f)
pclary 5:475f67175510 138 {
pclary 5:475f67175510 139 vector3 newPosition;
pclary 5:475f67175510 140 newPosition = circleCenter + nDeltaPosition.unit() * circleRadius * f;
pclary 5:475f67175510 141 step(newPosition);
pclary 9:a6d1502f0f20 142 return nDeltaPosition;
pclary 5:475f67175510 143 }
pclary 5:475f67175510 144
pclary 5:475f67175510 145
pclary 5:475f67175510 146
pclary 5:475f67175510 147 bool RobotLeg::update(const matrix4& deltaTransform)
pclary 3:6fa07ceb897f 148 {
pclary 10:dc1ba352667e 149 float t, d;
pclary 11:9ee0214bd410 150 vector3 newNDeltaPosition, v;
pclary 11:9ee0214bd410 151 const float eps = 0.00001f;
pclary 3:6fa07ceb897f 152
pclary 3:6fa07ceb897f 153 switch (state)
pclary 3:6fa07ceb897f 154 {
pclary 3:6fa07ceb897f 155 case neutral:
pclary 10:dc1ba352667e 156 // Calculate new position and position delta
pclary 5:475f67175510 157 newPosition = deltaTransform*position;
pclary 9:a6d1502f0f20 158 newNDeltaPosition = position - newPosition;
pclary 12:a952bd74d363 159 newNDeltaPosition.z = 0.0f;
pclary 12:a952bd74d363 160 if (fabs(newNDeltaPosition.x) > eps || fabs(newNDeltaPosition.y) > eps)
pclary 9:a6d1502f0f20 161 nDeltaPosition = newNDeltaPosition;
pclary 10:dc1ba352667e 162
pclary 10:dc1ba352667e 163 // Check if new position is outside the step circle
pclary 11:9ee0214bd410 164 v = newPosition - circleCenter;
pclary 10:dc1ba352667e 165 d = sqrt(v.x*v.x + v.y*v.y);
pclary 10:dc1ba352667e 166
pclary 10:dc1ba352667e 167 // Attempt to move to the new position
pclary 11:9ee0214bd410 168 return d < circleRadius;
pclary 3:6fa07ceb897f 169
pclary 3:6fa07ceb897f 170 case stepping:
pclary 11:9ee0214bd410 171 // Compute new position along step trajectory
pclary 3:6fa07ceb897f 172 t = stepTimer.read();
pclary 3:6fa07ceb897f 173 newPosition.x = stepA.x + (stepB.x - stepA.x)*0.5f*(1 - cos(stepDelta*t));
pclary 3:6fa07ceb897f 174 newPosition.y = stepA.y + (stepB.y - stepA.y)*0.5f*(1 - cos(stepDelta*t));
pclary 3:6fa07ceb897f 175 newPosition.z = stepA.z + (stepB.z - stepA.z)*stepDelta*t + stepHeight*sin(stepDelta*t);
pclary 3:6fa07ceb897f 176
pclary 6:0163f2737cc6 177 if (t < stepTime)
pclary 3:6fa07ceb897f 178 {
pclary 3:6fa07ceb897f 179 move(newPosition);
pclary 3:6fa07ceb897f 180 }
pclary 3:6fa07ceb897f 181 else
pclary 3:6fa07ceb897f 182 {
pclary 3:6fa07ceb897f 183 move(stepB);
pclary 3:6fa07ceb897f 184 state = neutral;
pclary 3:6fa07ceb897f 185 stepTimer.stop();
pclary 13:1c5d255835ce 186 newPosition = position;
pclary 3:6fa07ceb897f 187 }
pclary 11:9ee0214bd410 188
pclary 11:9ee0214bd410 189 return true;
pclary 3:6fa07ceb897f 190 }
pclary 5:475f67175510 191
pclary 12:a952bd74d363 192 // Should never reach here
pclary 11:9ee0214bd410 193 return false;
pclary 11:9ee0214bd410 194 }
pclary 11:9ee0214bd410 195
pclary 11:9ee0214bd410 196
pclary 11:9ee0214bd410 197
pclary 11:9ee0214bd410 198 void RobotLeg::apply()
pclary 11:9ee0214bd410 199 {
pclary 11:9ee0214bd410 200 if (neutral == state)
pclary 11:9ee0214bd410 201 {
pclary 17:4ec59e8b52a6 202 move(newPosition);
pclary 11:9ee0214bd410 203 }
pclary 11:9ee0214bd410 204 }
pclary 11:9ee0214bd410 205
pclary 11:9ee0214bd410 206
pclary 11:9ee0214bd410 207
pclary 11:9ee0214bd410 208 bool RobotLeg::getStepping()
pclary 11:9ee0214bd410 209 {
pclary 11:9ee0214bd410 210 return stepping == state;
pclary 11:9ee0214bd410 211 }