File content as of revision 11:425dff6a4af9:

#include "PaceDetector.hpp"
#include "Actuator.hpp"
#include "State.hpp"
#include "Sensor.hpp"
#include "io.h"
#include "rtos.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());
}