/** 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);
}
}