Updated code with the changes in the robot. Got rid of the extra PID class we dont need. Added constants