Fixed PID calculating wrong Diff

Doh!
Added new checkpoints