debugged followtrajectory

still doesn't work