Diff: Control/Actuator.cpp

Revision:

9:7a8fb72f9a93

Child:

10:22c44650c7c1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Control/Actuator.cpp	Wed Mar 25 14:34:23 2015 +0000
@@ -0,0 +1,74 @@
+#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));
+}
+