Initial state logic code

This is a pretty rough draft. I'm going to need to play around with the mbed a little bit more to see what the optimal way of constructing our state machine logic is.