File content as of revision 9:7a8fb72f9a93:

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