pid controller: Fixing initialization problems, adding more robustness to bad inputs