Dave Lu / Mbed 2 deprecated FYDP

IMU and knee angle. No servo yet

Dependencies:   mbed

Fork of FYDP_Final2 by Dave Lu

Control/State.cpp@11:425dff6a4af9, 2015-03-25 (annotated)

Committer:
tntmarket
Date:
Wed Mar 25 18:11:09 2015 +0000
Revision:
11:425dff6a4af9
Parent:
9:7a8fb72f9a93 
Working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tntmarket 9:7a8fb72f9a93 1 #include "State.hpp"
tntmarket 9:7a8fb72f9a93 2
tntmarket 9:7a8fb72f9a93 3 typedef void (*Void2Void)(void);
tntmarket 9:7a8fb72f9a93 4
tntmarket 9:7a8fb72f9a93 5 void noop(void) {}
tntmarket 9:7a8fb72f9a93 6
tntmarket 9:7a8fb72f9a93 7 int State::currentState(int prevState, float pitch) {
tntmarket 9:7a8fb72f9a93 8 switch(prevState) {
tntmarket 9:7a8fb72f9a93 9 case UNCALIBRATED:
tntmarket 9:7a8fb72f9a93 10 return UNCALIBRATED;
tntmarket 9:7a8fb72f9a93 11 case STOP:
tntmarket 9:7a8fb72f9a93 12 if(pitch > 7) {
tntmarket 9:7a8fb72f9a93 13 onStopToSwing();
tntmarket 9:7a8fb72f9a93 14 return SWING;
tntmarket 9:7a8fb72f9a93 15 } else if(pitch < -13) {
tntmarket 9:7a8fb72f9a93 16 onStopToMaybe();
tntmarket 9:7a8fb72f9a93 17 return MAYBESTOP;
tntmarket 9:7a8fb72f9a93 18 }
tntmarket 9:7a8fb72f9a93 19 return STOP;
tntmarket 9:7a8fb72f9a93 20 case SWING:
tntmarket 9:7a8fb72f9a93 21 if(pitch < -7) {
tntmarket 9:7a8fb72f9a93 22 return MAYBESTOP;
tntmarket 9:7a8fb72f9a93 23 }
tntmarket 9:7a8fb72f9a93 24 return SWING;
tntmarket 9:7a8fb72f9a93 25 case MAYBESTOP:
tntmarket 9:7a8fb72f9a93 26 if(pitch > -5) {
tntmarket 9:7a8fb72f9a93 27 return STOP;
tntmarket 9:7a8fb72f9a93 28 }
tntmarket 9:7a8fb72f9a93 29 return MAYBESTOP;
tntmarket 9:7a8fb72f9a93 30 default:
tntmarket 9:7a8fb72f9a93 31 return UNCALIBRATED;
tntmarket 9:7a8fb72f9a93 32 }
tntmarket 9:7a8fb72f9a93 33 }
tntmarket 9:7a8fb72f9a93 34
tntmarket 9:7a8fb72f9a93 35 State::State() {
tntmarket 9:7a8fb72f9a93 36 onStopToSwing = &noop;
tntmarket 9:7a8fb72f9a93 37 onStopToMaybe = &noop;
tntmarket 9:7a8fb72f9a93 38 state = UNCALIBRATED;
tntmarket 9:7a8fb72f9a93 39 }
tntmarket 9:7a8fb72f9a93 40
tntmarket 9:7a8fb72f9a93 41 void State::onSwing(Void2Void handler) {
tntmarket 9:7a8fb72f9a93 42 onStopToSwing = handler;
tntmarket 9:7a8fb72f9a93 43 }
tntmarket 9:7a8fb72f9a93 44
tntmarket 9:7a8fb72f9a93 45 void State::onMaybe(Void2Void handler) {
tntmarket 9:7a8fb72f9a93 46 onStopToMaybe = handler;
tntmarket 9:7a8fb72f9a93 47 }
tntmarket 9:7a8fb72f9a93 48
tntmarket 9:7a8fb72f9a93 49 void State::next(float thigh) {
tntmarket 9:7a8fb72f9a93 50 state = currentState(state, thigh);
tntmarket 9:7a8fb72f9a93 51 }