Pid fix

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