Added sliding mode and FSM for demo.
Formatted skeleton code and added test.