Added a "D" term to PID

Also added a compensation on the motors for the servos turning. (Not
tested; need to go offline for that.)