File content as of revision 9:7a8fb72f9a93:

#include "State.hpp"

typedef void (*Void2Void)(void);

void noop(void) {}

int State::currentState(int prevState, float pitch) {
   switch(prevState) {
      case UNCALIBRATED:
         return UNCALIBRATED;
      case STOP:
         if(pitch > 7) {
            onStopToSwing();
            return SWING;
         } else if(pitch < -13) {
            onStopToMaybe();
            return MAYBESTOP;
         }
         return STOP;
      case SWING:
         if(pitch < -7) {
            return MAYBESTOP;
         }
         return SWING;
      case MAYBESTOP:
         if(pitch > -5) {
            return STOP;
         }
         return MAYBESTOP;
      default:
         return UNCALIBRATED;
   }
}

State::State() {
   onStopToSwing = &noop;
   onStopToMaybe = &noop;
   state = UNCALIBRATED;
}

void State::onSwing(Void2Void handler) {
   onStopToSwing = handler;
}

void State::onMaybe(Void2Void handler) {
   onStopToMaybe = handler;
}

void State::next(float thigh) {
   state = currentState(state, thigh);
}