Wrote the outer loop position control along with the optional PID control., completed it.