Remove unused code
Added thread synchronization into DriveController
Refactor PID controller code to reduce processor load