better pid tuning, fixed bug in pid control that derv term was not using 2nd derivative, fixed bug in sanity check to stop edge cases from triggering it