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@9:7a8fb72f9a93, 2015-03-25 (annotated)

Committer:
tntmarket
Date:
Wed Mar 25 14:34:23 2015 +0000
Revision:
9:7a8fb72f9a93
Child:
10:22c44650c7c1
Working except for actuator

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tntmarket 9:7a8fb72f9a93 1 #include "Actuator.hpp"
tntmarket 9:7a8fb72f9a93 2
tntmarket 9:7a8fb72f9a93 3 #include "spline.h"
tntmarket 9:7a8fb72f9a93 4 #include <vector>
tntmarket 9:7a8fb72f9a93 5
tntmarket 9:7a8fb72f9a93 6
tntmarket 9:7a8fb72f9a93 7 void Actuator::interpolate(tk::spline curve, double xs[10], double ys[10]) {
tntmarket 9:7a8fb72f9a93 8 curve.set_points(
tntmarket 9:7a8fb72f9a93 9 std::vector<double>(xs, xs+10),
tntmarket 9:7a8fb72f9a93 10 std::vector<double>(ys, ys+10));
tntmarket 9:7a8fb72f9a93 11 }
tntmarket 9:7a8fb72f9a93 12
tntmarket 9:7a8fb72f9a93 13 void Actuator::setPeriod(float T) {
tntmarket 9:7a8fb72f9a93 14 if((T < TMIN || T > TMAX) && T != this->T) {
tntmarket 9:7a8fb72f9a93 15 return;
tntmarket 9:7a8fb72f9a93 16 }
tntmarket 9:7a8fb72f9a93 17 this->T = T;
tntmarket 9:7a8fb72f9a93 18
tntmarket 9:7a8fb72f9a93 19 double kneeDiffX[] = {
tntmarket 9:7a8fb72f9a93 20 0, 0.01*T,
tntmarket 9:7a8fb72f9a93 21 0.2*T, 0.201*T,
tntmarket 9:7a8fb72f9a93 22 0.6*T, 0.601*T,
tntmarket 9:7a8fb72f9a93 23 0.8*T, 0.801*T,
tntmarket 9:7a8fb72f9a93 24 T, 1.01*T
tntmarket 9:7a8fb72f9a93 25 };
tntmarket 9:7a8fb72f9a93 26 double kneeDiffY[] = {
tntmarket 9:7a8fb72f9a93 27 0, 0,
tntmarket 9:7a8fb72f9a93 28 5, 5,
tntmarket 9:7a8fb72f9a93 29 0, 0,
tntmarket 9:7a8fb72f9a93 30 55, 55,
tntmarket 9:7a8fb72f9a93 31 0, 0
tntmarket 9:7a8fb72f9a93 32 };
tntmarket 9:7a8fb72f9a93 33 interpolate(kneeDiffCurve, kneeDiffX, kneeDiffY);
tntmarket 9:7a8fb72f9a93 34
tntmarket 9:7a8fb72f9a93 35 double hipX[10] = {
tntmarket 9:7a8fb72f9a93 36 -0.1*T, -0.099*T,
tntmarket 9:7a8fb72f9a93 37 0.4*T, 0.401*T,
tntmarket 9:7a8fb72f9a93 38 0.6*T, 0.601*T,
tntmarket 9:7a8fb72f9a93 39 0.9*T, 0.901*T,
tntmarket 9:7a8fb72f9a93 40 1.1*T, 1.101*T
tntmarket 9:7a8fb72f9a93 41 };
tntmarket 9:7a8fb72f9a93 42 double hipY[10] = {
tntmarket 9:7a8fb72f9a93 43 22, 22,
tntmarket 9:7a8fb72f9a93 44 -8, -8.0,
tntmarket 9:7a8fb72f9a93 45 -19, -19,
tntmarket 9:7a8fb72f9a93 46 22, 22,
tntmarket 9:7a8fb72f9a93 47 -8, -8.0
tntmarket 9:7a8fb72f9a93 48 };
tntmarket 9:7a8fb72f9a93 49 interpolate(hipCurve, hipX, hipY);
tntmarket 9:7a8fb72f9a93 50 }
tntmarket 9:7a8fb72f9a93 51
tntmarket 9:7a8fb72f9a93 52 void Actuator::startHeelStrike(float time) {
tntmarket 9:7a8fb72f9a93 53 tHeelStrike = time;
tntmarket 9:7a8fb72f9a93 54 }
tntmarket 9:7a8fb72f9a93 55 void Actuator::startMidStance(float time) {
tntmarket 9:7a8fb72f9a93 56 tHeelStrike = time - T*MIDSTANCE;
tntmarket 9:7a8fb72f9a93 57 }
tntmarket 9:7a8fb72f9a93 58
tntmarket 9:7a8fb72f9a93 59 float Actuator::getKneeDiff(float time) {
tntmarket 9:7a8fb72f9a93 60 float tFromHeelStrike = time - tHeelStrike;
tntmarket 9:7a8fb72f9a93 61 if(tFromHeelStrike > T) {
tntmarket 9:7a8fb72f9a93 62 return (float)kneeDiffCurve(0);
tntmarket 9:7a8fb72f9a93 63 }
tntmarket 9:7a8fb72f9a93 64 return (float)(kneeDiffCurve(tFromHeelStrike));
tntmarket 9:7a8fb72f9a93 65 }
tntmarket 9:7a8fb72f9a93 66
tntmarket 9:7a8fb72f9a93 67 float Actuator::getHip(float time) {
tntmarket 9:7a8fb72f9a93 68 float tFromHeelStrike = time - tHeelStrike;
tntmarket 9:7a8fb72f9a93 69 if(tFromHeelStrike > T) {
tntmarket 9:7a8fb72f9a93 70 return (float)hipCurve(0);
tntmarket 9:7a8fb72f9a93 71 }
tntmarket 9:7a8fb72f9a93 72 return (float)(hipCurve(tFromHeelStrike));
tntmarket 9:7a8fb72f9a93 73 }
tntmarket 9:7a8fb72f9a93 74