Refactored velocity configuration in control algorithm. Made default PID constants the good ones. Corrected the velocity sign. There might be sign errors with the velocity PID constants.