IMU and knee angle. No servo yet

Dependencies:   mbed

Fork of FYDP_Final2 by Dave Lu

Control/control.cpp

Committer:
tntmarket
Date:
2015-03-25
Revision:
10:22c44650c7c1
Parent:
9:7a8fb72f9a93
Child:
11:425dff6a4af9

File content as of revision 10:22c44650c7c1:

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