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