Implement proper output saturation for PID - partial fix for issue 32

Also switches the input filtering and derivative action to working on
the process value, not the error term. This should improve stability
when the setpoint jumps around.

This code is untested, because I currently don't have a machine set up
and am in class. Probably won't even compile...
Fix typo & remember to filter measurements before sending to PID controller
Formatting fixes
Merge pull request #34 from Wallacoloo/pid-fix

Pid fix