Dave Lu
/
FYDP
IMU and knee angle. No servo yet
Fork of FYDP_Final2 by
Control/Actuator.cpp@9:7a8fb72f9a93, 2015-03-25 (annotated)
- Committer:
- tntmarket
- Date:
- Wed Mar 25 14:34:23 2015 +0000
- Revision:
- 9:7a8fb72f9a93
- Child:
- 10:22c44650c7c1
Working except for actuator
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tntmarket | 9:7a8fb72f9a93 | 1 | #include "Actuator.hpp" |
tntmarket | 9:7a8fb72f9a93 | 2 | |
tntmarket | 9:7a8fb72f9a93 | 3 | #include "spline.h" |
tntmarket | 9:7a8fb72f9a93 | 4 | #include <vector> |
tntmarket | 9:7a8fb72f9a93 | 5 | |
tntmarket | 9:7a8fb72f9a93 | 6 | |
tntmarket | 9:7a8fb72f9a93 | 7 | void Actuator::interpolate(tk::spline curve, double xs[10], double ys[10]) { |
tntmarket | 9:7a8fb72f9a93 | 8 | curve.set_points( |
tntmarket | 9:7a8fb72f9a93 | 9 | std::vector<double>(xs, xs+10), |
tntmarket | 9:7a8fb72f9a93 | 10 | std::vector<double>(ys, ys+10)); |
tntmarket | 9:7a8fb72f9a93 | 11 | } |
tntmarket | 9:7a8fb72f9a93 | 12 | |
tntmarket | 9:7a8fb72f9a93 | 13 | void Actuator::setPeriod(float T) { |
tntmarket | 9:7a8fb72f9a93 | 14 | if((T < TMIN || T > TMAX) && T != this->T) { |
tntmarket | 9:7a8fb72f9a93 | 15 | return; |
tntmarket | 9:7a8fb72f9a93 | 16 | } |
tntmarket | 9:7a8fb72f9a93 | 17 | this->T = T; |
tntmarket | 9:7a8fb72f9a93 | 18 | |
tntmarket | 9:7a8fb72f9a93 | 19 | double kneeDiffX[] = { |
tntmarket | 9:7a8fb72f9a93 | 20 | 0, 0.01*T, |
tntmarket | 9:7a8fb72f9a93 | 21 | 0.2*T, 0.201*T, |
tntmarket | 9:7a8fb72f9a93 | 22 | 0.6*T, 0.601*T, |
tntmarket | 9:7a8fb72f9a93 | 23 | 0.8*T, 0.801*T, |
tntmarket | 9:7a8fb72f9a93 | 24 | T, 1.01*T |
tntmarket | 9:7a8fb72f9a93 | 25 | }; |
tntmarket | 9:7a8fb72f9a93 | 26 | double kneeDiffY[] = { |
tntmarket | 9:7a8fb72f9a93 | 27 | 0, 0, |
tntmarket | 9:7a8fb72f9a93 | 28 | 5, 5, |
tntmarket | 9:7a8fb72f9a93 | 29 | 0, 0, |
tntmarket | 9:7a8fb72f9a93 | 30 | 55, 55, |
tntmarket | 9:7a8fb72f9a93 | 31 | 0, 0 |
tntmarket | 9:7a8fb72f9a93 | 32 | }; |
tntmarket | 9:7a8fb72f9a93 | 33 | interpolate(kneeDiffCurve, kneeDiffX, kneeDiffY); |
tntmarket | 9:7a8fb72f9a93 | 34 | |
tntmarket | 9:7a8fb72f9a93 | 35 | double hipX[10] = { |
tntmarket | 9:7a8fb72f9a93 | 36 | -0.1*T, -0.099*T, |
tntmarket | 9:7a8fb72f9a93 | 37 | 0.4*T, 0.401*T, |
tntmarket | 9:7a8fb72f9a93 | 38 | 0.6*T, 0.601*T, |
tntmarket | 9:7a8fb72f9a93 | 39 | 0.9*T, 0.901*T, |
tntmarket | 9:7a8fb72f9a93 | 40 | 1.1*T, 1.101*T |
tntmarket | 9:7a8fb72f9a93 | 41 | }; |
tntmarket | 9:7a8fb72f9a93 | 42 | double hipY[10] = { |
tntmarket | 9:7a8fb72f9a93 | 43 | 22, 22, |
tntmarket | 9:7a8fb72f9a93 | 44 | -8, -8.0, |
tntmarket | 9:7a8fb72f9a93 | 45 | -19, -19, |
tntmarket | 9:7a8fb72f9a93 | 46 | 22, 22, |
tntmarket | 9:7a8fb72f9a93 | 47 | -8, -8.0 |
tntmarket | 9:7a8fb72f9a93 | 48 | }; |
tntmarket | 9:7a8fb72f9a93 | 49 | interpolate(hipCurve, hipX, hipY); |
tntmarket | 9:7a8fb72f9a93 | 50 | } |
tntmarket | 9:7a8fb72f9a93 | 51 | |
tntmarket | 9:7a8fb72f9a93 | 52 | void Actuator::startHeelStrike(float time) { |
tntmarket | 9:7a8fb72f9a93 | 53 | tHeelStrike = time; |
tntmarket | 9:7a8fb72f9a93 | 54 | } |
tntmarket | 9:7a8fb72f9a93 | 55 | void Actuator::startMidStance(float time) { |
tntmarket | 9:7a8fb72f9a93 | 56 | tHeelStrike = time - T*MIDSTANCE; |
tntmarket | 9:7a8fb72f9a93 | 57 | } |
tntmarket | 9:7a8fb72f9a93 | 58 | |
tntmarket | 9:7a8fb72f9a93 | 59 | float Actuator::getKneeDiff(float time) { |
tntmarket | 9:7a8fb72f9a93 | 60 | float tFromHeelStrike = time - tHeelStrike; |
tntmarket | 9:7a8fb72f9a93 | 61 | if(tFromHeelStrike > T) { |
tntmarket | 9:7a8fb72f9a93 | 62 | return (float)kneeDiffCurve(0); |
tntmarket | 9:7a8fb72f9a93 | 63 | } |
tntmarket | 9:7a8fb72f9a93 | 64 | return (float)(kneeDiffCurve(tFromHeelStrike)); |
tntmarket | 9:7a8fb72f9a93 | 65 | } |
tntmarket | 9:7a8fb72f9a93 | 66 | |
tntmarket | 9:7a8fb72f9a93 | 67 | float Actuator::getHip(float time) { |
tntmarket | 9:7a8fb72f9a93 | 68 | float tFromHeelStrike = time - tHeelStrike; |
tntmarket | 9:7a8fb72f9a93 | 69 | if(tFromHeelStrike > T) { |
tntmarket | 9:7a8fb72f9a93 | 70 | return (float)hipCurve(0); |
tntmarket | 9:7a8fb72f9a93 | 71 | } |
tntmarket | 9:7a8fb72f9a93 | 72 | return (float)(hipCurve(tFromHeelStrike)); |
tntmarket | 9:7a8fb72f9a93 | 73 | } |
tntmarket | 9:7a8fb72f9a93 | 74 |