![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
IMU and knee angle. No servo yet
Fork of FYDP_Final2 by
Diff: Control/Actuator.cpp
- Revision:
- 10:22c44650c7c1
- Parent:
- 9:7a8fb72f9a93
- Child:
- 11:425dff6a4af9
diff -r 7a8fb72f9a93 -r 22c44650c7c1 Control/Actuator.cpp --- a/Control/Actuator.cpp Wed Mar 25 14:34:23 2015 +0000 +++ b/Control/Actuator.cpp Wed Mar 25 16:36:11 2015 +0000 @@ -1,74 +1,62 @@ -#include "Actuator.hpp" - -#include "spline.h" -#include <vector> - - -void Actuator::interpolate(tk::spline curve, double xs[10], double ys[10]) { - curve.set_points( - std::vector<double>(xs, xs+10), - std::vector<double>(ys, ys+10)); -} - -void Actuator::setPeriod(float T) { - if((T < TMIN || T > TMAX) && T != this->T) { - return; - } - this->T = T; - - double kneeDiffX[] = { - 0, 0.01*T, - 0.2*T, 0.201*T, - 0.6*T, 0.601*T, - 0.8*T, 0.801*T, - T, 1.01*T - }; - double kneeDiffY[] = { - 0, 0, - 5, 5, - 0, 0, - 55, 55, - 0, 0 - }; - interpolate(kneeDiffCurve, kneeDiffX, kneeDiffY); - - double hipX[10] = { - -0.1*T, -0.099*T, - 0.4*T, 0.401*T, - 0.6*T, 0.601*T, - 0.9*T, 0.901*T, - 1.1*T, 1.101*T - }; - double hipY[10] = { - 22, 22, - -8, -8.0, - -19, -19, - 22, 22, - -8, -8.0 - }; - interpolate(hipCurve, hipX, hipY); -} - -void Actuator::startHeelStrike(float time) { - tHeelStrike = time; -} -void Actuator::startMidStance(float time) { - tHeelStrike = time - T*MIDSTANCE; -} - -float Actuator::getKneeDiff(float time) { - float tFromHeelStrike = time - tHeelStrike; - if(tFromHeelStrike > T) { - return (float)kneeDiffCurve(0); - } - return (float)(kneeDiffCurve(tFromHeelStrike)); -} - -float Actuator::getHip(float time) { - float tFromHeelStrike = time - tHeelStrike; - if(tFromHeelStrike > T) { - return (float)hipCurve(0); - } - return (float)(hipCurve(tFromHeelStrike)); -} +#include "Actuator.hpp" + +#include "spline.h" +#include <vector> + +void Actuator::setPeriod(float T) { + if((T < TMIN || T > TMAX) && T != this->T) { + return; + } + this->T = T; + + std::vector<double> X(10), Y(10); + X[0]=0.0 ; Y[0]=0 ; + X[1]=T*0.01 ; Y[1]=0 ; + X[2]=T*0.2 ; Y[2]=5 ; + X[3]=T*0.201 ; Y[3]=5 ; + X[4]=T*0.6 ; Y[4]=0 ; + X[5]=T*0.601 ; Y[5]=0 ; + X[6]=T*0.8 ; Y[6]=55 ; + X[7]=T*0.801 ; Y[7]=55 ; + X[8]=T ; Y[8]=0 ; + X[9]=T*1.01 ; Y[9]=0 ; + + kneeDiffCurve.set_points(X, Y); + X[0]=T*(-0.1) ; Y[0]=22 ; + X[1]=T*(-0.099) ; Y[1]=22 ; + X[2]=T*0.4 ; Y[2]=-8 ; + X[3]=T*0.401 ; Y[3]=-8 ; + X[4]=T*0.6 ; Y[4]=-19 ; + X[5]=T*0.601 ; Y[5]=-19 ; + X[6]=T*0.9 ; Y[6]=22 ; + X[7]=T*0.901 ; Y[7]=22 ; + X[8]=T*1.1 ; Y[8]=-8 ; + X[9]=T*1.101 ; Y[9]=-8 ; + + hipCurve.set_points(X, Y); +} + +void Actuator::startHeelStrike(float time) { + tHeelStrike = time; +} +void Actuator::startMidStance(float time) { + tHeelStrike = time - T*MIDSTANCE; +} + +float Actuator::getKneeDiff(float time) { + float tFromHeelStrike = time - tHeelStrike; + if(tFromHeelStrike > T) { + return (float)kneeDiffCurve(0); + } + return (float)(kneeDiffCurve(tFromHeelStrike)); +} + +float Actuator::getHip(float time) { + float tFromHeelStrike = time - tHeelStrike; + if(tFromHeelStrike > T) { + return (float)hipCurve(0); + } + return (float)(hipCurve(tFromHeelStrike)); +} +