control.cpp

00001 #include "PaceDetector.hpp"
00002 #include "Actuator.hpp"
00003 #include "State.hpp"
00004 #include "Sensor.hpp"
00005 #include "io.h"
00006 #include "rtos.h"
00007 
00008 State state;
00009 
00010 PaceDetector paceDetector;
00011 Actuator actuator;
00012 
00013 Sensor* _sensor;
00014 
00015 void onStopToSwing() {
00016    paceDetector.lookForPeak();
00017    actuator.startHeelStrike(_sensor->time());
00018 }
00019 void onStopToMaybe() {
00020    actuator.startMidStance(_sensor->time());
00021 }
00022 
00023 void setup(Sensor* s) {
00024    _sensor = s;
00025    state.state = State::UNCALIBRATED;
00026    state.onSwing(&onStopToSwing);
00027    state.onMaybe(&onStopToMaybe);
00028    actuator.setPeriod(2);
00029 }
00030 
00031 void process() {
00032    state.next(_sensor->thigh());
00033    paceDetector.process(_sensor->time(), _sensor->thigh(), _sensor->shin());
00034    actuator.setPeriod(paceDetector.pace());
00035 
00036    /* -actuator.getHip(_sensor.time()); */
00037    /* actuator.getKneeDiff(_sensor.time()); */
00038 }
00039 
00040 int getState() {
00041    return state.state;
00042 }
00043 
00044 void setStateToStop() {
00045    state.state = State::STOP;
00046 }
00047 
00048 float getPace() {
00049    return paceDetector.pace();
00050 }
00051 
00052 float getHip() {
00053    return -actuator.getHip(_sensor->time());
00054 }
00055 
00056 float getKneeDiff() {
00057    return actuator.getKneeDiff(_sensor->time());
00058 }
00059