Added control over PID - I term to compensate initial offset