Working locks

PII controller fixed. I Gain was applied after the last value and the
new error value were added causing the system to unlock. This has been
changed to apply I Gain to the error signal before addition