Made improvement to PID controller, now the integral clears when the
error sign flips, so that the controller will have fast response but
won't over adjust anymore.