Diff: Control/control.cpp

Revision:

9:7a8fb72f9a93

Child:

10:22c44650c7c1

--- /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());
+}
+