Fixes to pid system