Added I controller for long term distance stability, tuned parameters