Diff: Control/Actuator.cpp

Revision:

10:22c44650c7c1

Parent:

9:7a8fb72f9a93

Child:

11:425dff6a4af9

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