Final PID adjustments.

Before sensor integration and refactoring.