With new GoToGoal traj but the motors are all fucked up. Pushing before I change the motor functions