Merge steve's PID code and fix bugs