Added torque saturation to PID