/** 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);
}
}
/** 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();
}
/** The constructor instantiates the pilot and navigator. Single step is false. */
public RobotControllerLejos() {
path = new Path();
pilot = new DifferentialPilot(5.6, 18, Motor.A, Motor.B);
nav = new Navigator(pilot);
nav.singleStep(false);
}
/** Stops the robot right away. */
@Override
public void stopImmediate() {
nav.stop();
Logger.INSTANCE.log(this.getClass().getSimpleName(), "Emergency stopped");
}
/** Stops the robot when it hits the next waypoint. */
@Override
public void stop() {
nav.singleStep(true);
}
/**
* Returns a waypoint of the current location.
*
* @return Waypoint, current location
*/
@Override
public Waypoint getLocation() {
PoseProvider provider = nav.getPoseProvider();
Pose pose = provider.getPose();
return (Waypoint) pose.getLocation();
}
/** Sets the robot to single step mode. */
@Override
public void singleStep(boolean setting) {
check = setting;
nav.singleStep(setting);
}