Dave Lu / Mbed 2 deprecated FYDP

IMU and knee angle. No servo yet

Dependencies:   mbed

Fork of FYDP_Final2 by Dave Lu

Control/control.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 "PaceDetector.hpp"
tntmarket 9:7a8fb72f9a93 2 #include "Actuator.hpp"
tntmarket 9:7a8fb72f9a93 3 #include "State.hpp"
tntmarket 9:7a8fb72f9a93 4 #include "Sensor.hpp"
tntmarket 9:7a8fb72f9a93 5 #include "io.h"
tntmarket 9:7a8fb72f9a93 6
tntmarket 9:7a8fb72f9a93 7 State state;
tntmarket 9:7a8fb72f9a93 8
tntmarket 9:7a8fb72f9a93 9 PaceDetector paceDetector;
tntmarket 9:7a8fb72f9a93 10 Actuator actuator;
tntmarket 9:7a8fb72f9a93 11
tntmarket 9:7a8fb72f9a93 12 Sensor _sensor;
tntmarket 9:7a8fb72f9a93 13
tntmarket 9:7a8fb72f9a93 14 void onStopToSwing() {
tntmarket 9:7a8fb72f9a93 15 paceDetector.lookForPeak();
tntmarket 9:7a8fb72f9a93 16 // actuator.startHeelStrike(_sensor.time());
tntmarket 9:7a8fb72f9a93 17 }
tntmarket 9:7a8fb72f9a93 18 void onStopToMaybe() {
tntmarket 9:7a8fb72f9a93 19 // actuator.startMidStance(_sensor.time());
tntmarket 9:7a8fb72f9a93 20 }
tntmarket 9:7a8fb72f9a93 21
tntmarket 9:7a8fb72f9a93 22 void setup(Sensor &s) {
tntmarket 9:7a8fb72f9a93 23 _sensor = s;
tntmarket 9:7a8fb72f9a93 24 state.state = State::UNCALIBRATED;
tntmarket 9:7a8fb72f9a93 25 state.onSwing(&onStopToSwing);
tntmarket 9:7a8fb72f9a93 26 state.onMaybe(&onStopToMaybe);
tntmarket 9:7a8fb72f9a93 27 // actuator.setPeriod(2);
tntmarket 9:7a8fb72f9a93 28 }
tntmarket 9:7a8fb72f9a93 29
tntmarket 9:7a8fb72f9a93 30 void process() {
tntmarket 9:7a8fb72f9a93 31 state.next(_sensor.thigh());
tntmarket 9:7a8fb72f9a93 32 paceDetector.process(_sensor.time(), _sensor.thigh(), _sensor.shin());
tntmarket 9:7a8fb72f9a93 33 // actuator.setPeriod(paceDetector.pace());
tntmarket 9:7a8fb72f9a93 34
tntmarket 9:7a8fb72f9a93 35 /* -actuator.getHip(_sensor.time()); */
tntmarket 9:7a8fb72f9a93 36 /* actuator.getKneeDiff(_sensor.time()); */
tntmarket 9:7a8fb72f9a93 37 }
tntmarket 9:7a8fb72f9a93 38
tntmarket 9:7a8fb72f9a93 39 int getState() {
tntmarket 9:7a8fb72f9a93 40 return state.state;
tntmarket 9:7a8fb72f9a93 41 }
tntmarket 9:7a8fb72f9a93 42
tntmarket 9:7a8fb72f9a93 43 void setStateToStop() {
tntmarket 9:7a8fb72f9a93 44 state.state = State::STOP;
tntmarket 9:7a8fb72f9a93 45 }
tntmarket 9:7a8fb72f9a93 46
tntmarket 9:7a8fb72f9a93 47 float getPace() {
tntmarket 9:7a8fb72f9a93 48 return paceDetector.pace();
tntmarket 9:7a8fb72f9a93 49 }
tntmarket 9:7a8fb72f9a93 50
tntmarket 9:7a8fb72f9a93 51 float getHip() {
tntmarket 9:7a8fb72f9a93 52 return -actuator.getHip(_sensor.time());
tntmarket 9:7a8fb72f9a93 53 }
tntmarket 9:7a8fb72f9a93 54
tntmarket 9:7a8fb72f9a93 55 float getKneeDiff() {
tntmarket 9:7a8fb72f9a93 56 return actuator.getKneeDiff(_sensor.time());
tntmarket 9:7a8fb72f9a93 57 }
tntmarket 9:7a8fb72f9a93 58