now it works with a trajectory.. need to tune KPID...