Cleaned up

THIS IS WORKING DRIVE CODE USING PID AND SPEED