Fine tuning PID
Iterated on square example; fixed Infinity issue