File content as of revision 11:425dff6a4af9:

#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.07  ;
   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.07  ;

   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));
}