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@10:22c44650c7c1, 2015-03-25 (annotated)

Committer:
tntmarket
Date:
Wed Mar 25 16:36:11 2015 +0000
Revision:
10:22c44650c7c1
Parent:
9:7a8fb72f9a93
Child:
11:425dff6a4af9
running out of RAM?

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