Added basic VisionAction to vision

Renamed original to VisionWaitAction
Fixed auto actions in main.cpp
Fixed RotateIfAction functionality
Added debugging info for sonic sensor