refactored code and made a Command Based Robot.
Hoping it works.