Dave Lu / Mbed 2 deprecated FYDP

IMU and knee angle. No servo yet

Dependencies:   mbed

Fork of FYDP_Final2 by Dave Lu

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?

UserRevisionLine numberNew 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