![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
IMU and knee angle. No servo yet
Fork of FYDP_Final2 by
Diff: Control/control.cpp
- Revision:
- 9:7a8fb72f9a93
- Child:
- 10:22c44650c7c1
diff -r 3d5a84b897be -r 7a8fb72f9a93 Control/control.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Control/control.cpp Wed Mar 25 14:34:23 2015 +0000 @@ -0,0 +1,58 @@ +#include "PaceDetector.hpp" +#include "Actuator.hpp" +#include "State.hpp" +#include "Sensor.hpp" +#include "io.h" + +State state; + +PaceDetector paceDetector; +Actuator actuator; + +Sensor _sensor; + +void onStopToSwing() { + paceDetector.lookForPeak(); +// actuator.startHeelStrike(_sensor.time()); +} +void onStopToMaybe() { +// actuator.startMidStance(_sensor.time()); +} + +void setup(Sensor &s) { + _sensor = s; + state.state = State::UNCALIBRATED; + state.onSwing(&onStopToSwing); + state.onMaybe(&onStopToMaybe); +// actuator.setPeriod(2); +} + +void process() { + state.next(_sensor.thigh()); + paceDetector.process(_sensor.time(), _sensor.thigh(), _sensor.shin()); +// actuator.setPeriod(paceDetector.pace()); + + /* -actuator.getHip(_sensor.time()); */ + /* actuator.getKneeDiff(_sensor.time()); */ +} + +int getState() { + return state.state; +} + +void setStateToStop() { + state.state = State::STOP; +} + +float getPace() { + return paceDetector.pace(); +} + +float getHip() { + return -actuator.getHip(_sensor.time()); +} + +float getKneeDiff() { + return actuator.getKneeDiff(_sensor.time()); +} +