Basic outline of general SM (Not tested)

Added a basic outline of the general control (and device identification)
State Machine.
*This code has not been tested!*