Update followTrajectory.c