- Modified PID to account for changes in dt
- More efficient PID winds up tests.