Added simple controller to make the robot stand (only keeps it upright).