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@11:425dff6a4af9, 2015-03-25 (annotated)

Committer:
tntmarket
Date:
Wed Mar 25 18:11:09 2015 +0000
Revision:
11:425dff6a4af9
Parent:
10:22c44650c7c1 
Working

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 11:425dff6a4af9 13 Sensor* _sensor;
tntmarket 9:7a8fb72f9a93 14
tntmarket 9:7a8fb72f9a93 15 void onStopToSwing() {
tntmarket 9:7a8fb72f9a93 16 paceDetector.lookForPeak();
tntmarket 11:425dff6a4af9 17 actuator.startHeelStrike(_sensor->time());
tntmarket 9:7a8fb72f9a93 18 }
tntmarket 9:7a8fb72f9a93 19 void onStopToMaybe() {
tntmarket 11:425dff6a4af9 20 actuator.startMidStance(_sensor->time());
tntmarket 9:7a8fb72f9a93 21 }
tntmarket 9:7a8fb72f9a93 22
tntmarket 11:425dff6a4af9 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 11:425dff6a4af9 32 state.next(_sensor->thigh());
tntmarket 11:425dff6a4af9 33 paceDetector.process(_sensor->time(), _sensor->thigh(), _sensor->shin());
tntmarket 11:425dff6a4af9 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 11:425dff6a4af9 53 return -actuator.getHip(_sensor->time());
tntmarket 9:7a8fb72f9a93 54 }
tntmarket 9:7a8fb72f9a93 55
tntmarket 9:7a8fb72f9a93 56 float getKneeDiff() {
tntmarket 11:425dff6a4af9 57 return actuator.getKneeDiff(_sensor->time());
tntmarket 9:7a8fb72f9a93 58 }
tntmarket 9:7a8fb72f9a93 59