Check subsystem states logic

[Ido-Sobol]

Write an opmode that is taking the inputs from the driver and printing the states of the robot and the subsystems according to it.