/** Adds a waypoint to the current path */
 @Override
 public void addWaypoint(Waypoint wp) {
   forward.add(wp);
   Path temp = new Path();
   for (Waypoint way : forward) {
     temp.add(way);
   }
   this.setPath(temp);
 }
  /** The method used for immediate mode. Robot goes to the point. */
  @Override
  public void goToImmediate(Waypoint wp) {
    if (path != null) {
      path.clear();
      nav.clearPath();
    }
    nav.addWaypoint(wp);
    path.add(wp);

    Thread t =
        new Thread(
            new Runnable() {
              @Override
              public void run() {
                nav.followPath();
                nav.waitForStop();
                path.clear();
                nav.clearPath();
              }
            });

    t.start();
  }
  /** Set the path to be followed. */
  @Override
  public void setPath(Path path) {
    forward.clear();
    this.path.clear();
    nav.clearPath();
    for (int i = 0; i < path.size(); i++) {
      forward.add(new Waypoint(path.get(i)));
      this.path.add(new Waypoint(path.get(i)));
    }
    for (int i = path.size() - 2; i > -1; i--) {
      this.path.add(new Waypoint(path.get(i)));
    }

    this.path.add(new Waypoint(0, 0));

    for (Waypoint wp : this.path) {
      nav.addWaypoint(wp);
    }
  }