1
Dependencies: CircularBuffer Radio Servo Terminal mbed
Fork of WalkingRobot by
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?
User | Revision | Line number | New 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 | } |