Wrote the outer loop position control along with the optional PID control.