Add Working Teleop Trajectory

[kaaaien]

Added trajectory that runs on command with a trajectory passed from vision. TODO: Fix trajectory tuning