Reimplemented as a more general State Machine.

Ripped out old hackey code with a more proper first cut an extensible
state machine skeleton to allow adding in other sensors later. Need to
test on hardware.