Commit f55f1f8a authored by Peder Kronsgaard Detlefsen's avatar Peder Kronsgaard Detlefsen
Browse files

Added final result.

parent a711a3a4
import java.io.File;
import java.util.Vector;
import lejos.geom.Point;
import lejos.nxt.*; import lejos.nxt.*;
import lejos.nxt.comm.RConsole;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.DifferentialPilot; import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Move; import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Pose;
import lejos.util.Delay;
public class PilotAvoid { public class PilotAvoid {
private SlaveIOStreams PC; private SlaveIOStreams PC;
...@@ -14,13 +22,24 @@ public class PilotAvoid { ...@@ -14,13 +22,24 @@ public class PilotAvoid {
DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter,
rightWheelDiameter, trackWidth, left, right, false); rightWheelDiameter, trackWidth, left, right, false);
public OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);
public Point desPoint;
public PilotAvoid(boolean usb) { public PilotAvoid(boolean usb, float desX, float desY) {
PC = new SlaveIOStreams(usb); PC = new SlaveIOStreams(usb);
PC.open(); PC.open();
// Send destination to PC.
PC.output(desX);
PC.output(desY);
pilot.setTravelSpeed(travelSpeed); pilot.setTravelSpeed(travelSpeed);
pilot.setRotateSpeed(rotateSpeed); pilot.setRotateSpeed(rotateSpeed);
Pose initialPose = new Pose(0,0,0);
poseProvider.setPose(initialPose);
desPoint = new Point(desX, desY);
} }
public void sendMove(Move move) { public void sendMove(Move move) {
...@@ -29,74 +48,39 @@ public class PilotAvoid { ...@@ -29,74 +48,39 @@ public class PilotAvoid {
PC.output(move.getAngleTurned()); PC.output(move.getAngleTurned());
} }
public void travel(double distance) { public void travel(double distance, boolean immediateReturn) {
pilot.travel(distance, true); pilot.travel(distance, immediateReturn);
}
public void rotate(double angle) {
pilot.rotate(angle, true);
} }
public void go() { public void rotate(double angle, boolean immediateReturn) {
Sound.beep(); pilot.rotate(angle, immediateReturn);
while (!Button.ENTER.isDown()) }
Thread.yield();
Sound.twoBeeps();
travel(500);
rotate(90);
travel(200);
rotate(-90);
travel(500);
/*
* travel(500); rotate(90); travel(200); rotate(-90); travel(500);
* rotate(90); travel(200); rotate(90); travel(700); rotate(90);
* travel(200);
*/
while (!Button.ENTER.isDown())
Thread.yield();
LCD.clear();
LCD.drawString("Closing", 0, 0);
if (PC.close())
LCD.drawString("Closed", 0, 0);
try {
Thread.sleep(2000);
} catch (Exception e) {
}
}
public static void main(String[] args) { public static void main(String[] args) {
PilotAvoid pilot = new PilotAvoid(false); PilotAvoid pilot = new PilotAvoid(false, 1000, 500);
LCD.clear(); LCD.clear();
LCD.drawString("PilotRouteMM", 0, 0); LCD.drawString("PilotAvoid", 0, 0);
// route.go();
Sound.beep(); Sound.beep();
while (!Button.ENTER.isDown()) while (!Button.ENTER.isDown())
Thread.yield(); Thread.yield();
Sound.twoBeeps(); Sound.twoBeeps();
Behavior b1 = new FollowRoute(pilot); Behavior b1 = new FindGoal(pilot);
Behavior b2 = new AvoidObstacle(pilot); Behavior b2 = new AvoidObstacle(pilot);
Behavior[] behaviorList = { b1, b2}; Behavior b3 = new RecognizeGoal(pilot);
Behavior[] behaviorList = { b1, b2, b3};
Arbitrator arbitrator = new Arbitrator(behaviorList); Arbitrator arbitrator = new Arbitrator(behaviorList);
arbitrator.start(); arbitrator.start();
} }
} }
class FollowRoute implements Behavior { class FindGoal implements Behavior {
private static final int TRAVEL = 0;
private static final int ROTATE = 1;
private boolean _suppressed = false; private boolean _suppressed = false;
private PilotAvoid pilot; private PilotAvoid pilot;
private int stepsDone = 0;
public FollowRoute(PilotAvoid pilot) { public FindGoal(PilotAvoid pilot) {
this.pilot = pilot; this.pilot = pilot;
} }
...@@ -110,62 +94,66 @@ class FollowRoute implements Behavior { ...@@ -110,62 +94,66 @@ class FollowRoute implements Behavior {
public void action() { public void action() {
_suppressed = false; _suppressed = false;
Sound.beepSequence();
Pose p = pilot.poseProvider.getPose();
if (stepsDone < 1 && !_suppressed) { // Calculate angle based on destination.
doStep(TRAVEL, 500); float angle = p.angleTo(pilot.desPoint);
} // float degrees = (angle > 0) ? angle - p.getHeading() : angle + p.getHeading();
if (stepsDone < 2 && !_suppressed) { float degrees = - p.getHeading() + angle;
doStep(ROTATE, 90);
}
if (stepsDone < 3 && !_suppressed) {
doStep(TRAVEL, 200);
}
if (stepsDone < 4 && !_suppressed) {
doStep(ROTATE, -90);
}
if (stepsDone < 5 && !_suppressed) {
doStep(TRAVEL, 500);
}
Motor.A.stop(true); // not strictly necessary, but good programming practice // if (angle < 0 && p.getHeading() < 0) {
Motor.C.stop(true); // degrees = angle - p.getHeading();
} // } else if (angle > 0 && p.getHeading() < 0) {
// degrees = angle + p.getHeading();
public void doStep(int moveType, double value) { // } else if (angle > 0 && p.getHeading() > 0) {
switch (moveType) { // degrees = angle + p.getHeading();
case TRAVEL : pilot.travel(value); break; // } else if (angle < 0 && p.getHeading() > 0) {
case ROTATE : pilot.rotate(value); break; // degrees = angle - p.getHeading(); // done
default : System.err.println("Invalid moveType in doStep(): " + moveType); return; // }
}
stepsDone++; pilot.rotate(degrees, false);
pilot.sendMove(pilot.pilot.getMovement());
pilot.travel(p.distanceTo(pilot.desPoint), true);
while (!_suppressed && Motor.A.isMoving()) { while (!_suppressed && Motor.A.isMoving()) {
Thread.yield(); // don't exit till suppressed Thread.yield();
} }
pilot.sendMove(pilot.pilot.getMovement()); pilot.sendMove(pilot.pilot.getMovement());
}
Motor.A.stop(true); // not strictly necessary, but good programming practice
Motor.C.stop(true);
}
} }
class AvoidObstacle extends Thread implements Behavior { class AvoidObstacle extends Thread implements Behavior {
UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S1); UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S2);
private float distance = 255; private int distance = 255;
private int count = 0;
private boolean active = false;
private boolean _suppressed = false; private boolean _suppressed = false;
private PilotAvoid pilot; private PilotAvoid pilot;
public AvoidObstacle(PilotAvoid pilot) { public AvoidObstacle(PilotAvoid pilot) {
this.pilot = pilot; this.pilot = pilot;
this.isDaemon(); this.setDaemon(true);
this.start(); this.start();
} }
public void run() { public void run() {
while (true) { while (true) {
distance = sonar.getDistance(); distance = sonar.getDistance();
LCD.drawInt(distance, 3, 0, 2);
} }
} }
public int takeControl() { public int takeControl() {
return (distance < 15) ? 20 : 0; count++;
int t = (distance < 25 || active) ? 20 : 0;
LCD.drawInt(t, 2, 0, 3);
LCD.drawInt(count, 6, 0, 4);
return t;
} }
public void suppress() { public void suppress() {
...@@ -174,15 +162,75 @@ class AvoidObstacle extends Thread implements Behavior { ...@@ -174,15 +162,75 @@ class AvoidObstacle extends Thread implements Behavior {
public void action() { public void action() {
_suppressed = false; _suppressed = false;
active = true;
Sound.buzz(); Sound.buzz();
// Look for a clear passage.
pilot.rotate(45, true);
while (!_suppressed && Motor.A.isMoving()) {
Thread.yield();
}
pilot.sendMove(pilot.pilot.getMovement());
float disLeft = sonar.getDistance() * 10;
pilot.rotate(-90, true);
while (!_suppressed && Motor.A.isMoving()) {
Thread.yield();
}
pilot.sendMove(pilot.pilot.getMovement());
float disRight = sonar.getDistance() * 10;
// Take evasive action. // Take evasive action.
if (disLeft > disRight) {
pilot.rotate(90, true);
while (!_suppressed && Motor.A.isMoving()) {
Thread.yield();
}
pilot.sendMove(pilot.pilot.getMovement());
}
pilot.travel(Math.min(Math.max(disLeft - 200, disRight - 200), 300), true);
while (!_suppressed) { while (!_suppressed && Motor.A.isMoving()) {
Thread.yield(); // don't exit till suppressed Thread.yield();
} }
// Motor.A.stop(true); // not strictly necessary, but good programming practice pilot.sendMove(pilot.pilot.getMovement());
// Motor.C.stop(true);
Motor.A.stop(true); // not strictly necessary, but good programming practice
Motor.C.stop(true);
active = false;
} }
} }
class RecognizeGoal implements Behavior {
private boolean _suppressed = false;
private PilotAvoid pilot;
private Pose p;
public RecognizeGoal(PilotAvoid pilot) {
this.pilot = pilot;
}
public int takeControl() {
// Check if we reached the goal.
p = pilot.poseProvider.getPose();
float d = p.distanceTo(pilot.desPoint);
LCD.drawInt((int)d, 5, 0, 5);
return (d <= 10) ? 50 : 0;
}
public void suppress() {
_suppressed = true;// standard practice for suppress methods
}
public void action() {
_suppressed = false;
// We reached the target!
Sound.beepSequenceUp();
while (!Button.ESCAPE.isDown()) {
Delay.msDelay(100);
}
System.exit(0);
}
}
\ No newline at end of file
...@@ -29,7 +29,7 @@ public class InputOutputStreams ...@@ -29,7 +29,7 @@ public class InputOutputStreams
} }
else { else {
// Connect to any NXT using Bluetooth // Connect to any NXT using Bluetooth
if (!conn.connectTo("btspp://")){ if (!conn.connectTo("btspp://NXT8")){
return "No NXT found using Bluetooth"; return "No NXT found using Bluetooth";
} }
} }
......
...@@ -21,8 +21,8 @@ public class PilotGUI extends JFrame { ...@@ -21,8 +21,8 @@ public class PilotGUI extends JFrame {
// program will automatically adjust if these three constants are changed // program will automatically adjust if these three constants are changed
static final int VIS_HEIGHT=1080; // visualization height static final int VIS_HEIGHT=1080; // visualization height
static final int VIS_WIDTH=1920; // visualization width static final int VIS_WIDTH=1920; // visualization width
static final int Y_OFFSET=30; // vertical offset to visualization static final int Y_OFFSET=200; // vertical offset to visualization
static final int X_OFFSET=20; // horizontal offset to visualization static final int X_OFFSET=200; // horizontal offset to visualization
static final int POINT_WIDTH=1; // width of points shown in the visualization static final int POINT_WIDTH=1; // width of points shown in the visualization
// VIS_HEIGHT + 2 * Y_OFFSET is the window height // VIS_HEIGHT + 2 * Y_OFFSET is the window height
// VIS_WIDTH + 2 * X_OFFSET is the window width // VIS_WIDTH + 2 * X_OFFSET is the window width
...@@ -34,6 +34,8 @@ public class PilotGUI extends JFrame { ...@@ -34,6 +34,8 @@ public class PilotGUI extends JFrame {
ArrayList<Point> route; // the route as seen from the vehicle ArrayList<Point> route; // the route as seen from the vehicle
ArrayList<ParticleSet> particleSets = new ArrayList<ParticleSet>(); ArrayList<ParticleSet> particleSets = new ArrayList<ParticleSet>();
private int desX;
private int desY;
// an inner class to handle the visualization window being closed manually // an inner class to handle the visualization window being closed manually
class MyWindowAdapter extends WindowAdapter { class MyWindowAdapter extends WindowAdapter {
...@@ -43,9 +45,11 @@ public class PilotGUI extends JFrame { ...@@ -43,9 +45,11 @@ public class PilotGUI extends JFrame {
} }
// the constructor for the Visualize class // the constructor for the Visualize class
public PilotGUI(ParticleSet particles) public PilotGUI(ParticleSet particles, int desX, int desY)
{ {
this.particles = particles; this.particles = particles;
this.desX = desX;
this.desY = desY;
this.setSize(new Dimension(VIS_WIDTH+X_OFFSET*2, VIS_HEIGHT+Y_OFFSET*2)); this.setSize(new Dimension(VIS_WIDTH+X_OFFSET*2, VIS_HEIGHT+Y_OFFSET*2));
this.setTitle("Monte Carlo Localization"); this.setTitle("Monte Carlo Localization");
this.setVisible(true); this.setVisible(true);
...@@ -101,7 +105,9 @@ public class PilotGUI extends JFrame { ...@@ -101,7 +105,9 @@ public class PilotGUI extends JFrame {
} }
} }
// Draw destination.
g2.setColor(Color.RED);
g2.fillOval(winX(desX) - 5, winY(desY) + 5, 10, 10);
} }
private int winX(int x) private int winX(int x)
......
...@@ -19,18 +19,23 @@ public class PilotMonitor ...@@ -19,18 +19,23 @@ public class PilotMonitor
private boolean USB = false; private boolean USB = false;
private Route route = new Route(); private Route route = new Route();
private ParticleSet particles = new ParticleSet(1000); private ParticleSet particles = new ParticleSet(1000);
private PilotGUI view = new PilotGUI(particles); private PilotGUI view;
private Move move; private Move move;
public PilotMonitor() public PilotMonitor()
{ {
String m; String m;
view.update(route.getRoute(), false);
NXT = new InputOutputStreams(USB); NXT = new InputOutputStreams(USB);
do { do {
m = NXT.open(); m = NXT.open();
System.out.println(m); System.out.println(m);
} while ( m != "Connected"); } while ( m != "Connected");
// Receive destination from NXT.
int desX = (int)NXT.input();
int desY = (int)NXT.input();
view = new PilotGUI(particles, desX, desY);
view.update(route.getRoute(), false);
} }
private Move getMove() private Move getMove()
......
...@@ -54,9 +54,9 @@ public class ArenaRobot { ...@@ -54,9 +54,9 @@ public class ArenaRobot {
Behavior b1 = new Wander(pilot); Behavior b1 = new Wander(pilot);
Behavior b2 = new AvoidEdge(pilot); Behavior b2 = new AvoidEdge(pilot);
Behavior b3 = new Exit(pilot); Behavior b3 = new Exit(pilot, 180);
Behavior[] behaviorList = {b1, b2, b3}; Behavior[] behaviorList = {b1, b2, b3};
Arbitrator arbitrator = new Arbitrator(behaviorList); Arbitrator arbitrator = new Arbitrator(behaviorList);
arbitrator.start(); arbitrator.start();
} }
} }
...@@ -85,7 +85,7 @@ class Wander implements Behavior { ...@@ -85,7 +85,7 @@ class Wander implements Behavior {
double rand = Math.random(); double rand = Math.random();
if (rand > 0.5) { if (rand > 0.5) {
pilot.travel(rand * 2000); pilot.travel(rand * 200);
} else { } else {
pilot.rotate((Math.random() > 0.5) ? rand * 180 : rand * -180); pilot.rotate((Math.random() > 0.5) ? rand * 180 : rand * -180);
} }
...@@ -160,13 +160,18 @@ class AvoidEdge extends Thread implements Behavior { ...@@ -160,13 +160,18 @@ class AvoidEdge extends Thread implements Behavior {
class Exit implements Behavior { class Exit implements Behavior {
private boolean _suppressed = false; private boolean _suppressed = false;
private ArenaRobot pilot; private ArenaRobot pilot;
private int seconds;
private long startTime;
public Exit(ArenaRobot pilot) { public Exit(ArenaRobot pilot, int seconds) {
this.pilot = pilot; this.pilot = pilot;
this.seconds = seconds;
startTime = System.currentTimeMillis();
} }
public int takeControl() { public int takeControl() {
return (Button.ESCAPE.isDown()) ? 100 : 0 ; // this behavior always wants control. return (Button.ESCAPE.isDown() ||
System.currentTimeMillis() > (startTime + seconds * 1000)) ? 100 : 0 ;
} }
public void suppress() { public void suppress() {
......
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.SensorPort;
public class LightTest {
public static void main (String[] args) {
LightSensor sensor = new LightSensor(SensorPort.S1);
while (!Button.ESCAPE.isDown()) {
LCD.drawString("Light: ", 0, 5);
LCD.drawInt(sensor.readNormalizedValue(), 4, 10, 5);
}
}
}
import lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Move;
import lejos.util.Delay;
public class PilotControl2D
{
private SlaveIOStreams PC;
private LightSensor light = new LightSensor(SensorPort.S1);
double mult = 1.007;
double leftWheelDiameter = 55 * mult, rightWheelDiameter = 54.84 * mult, trackWidth = 165;
double travelSpeed = 50, rotateSpeed = 45;
NXTRegulatedMotor left = Motor.A;
NXTRegulatedMotor right = Motor.C;
DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, left, right, false);
private boolean haveLocation = false;
public PilotControl2D(boolean usb)
{
PC = new SlaveIOStreams(usb);
PC.open();
pilot.setTravelSpeed(travelSpeed);
pilot.setRotateSpeed(rotateSpeed);
}
private void sendMove(Move move)
{
PC.output((move.getMoveType() == Move.MoveType.TRAVEL? 0:1 ));
PC.output(move.getDistanceTraveled());
PC.output(move.getAngleTurned());
}
private void sendLight() {
PC.output(light.getLightValue());
}
private void travel(double distance)
{
pilot.travel(distance);
sendMove(pilot.getMovement());
PC.output(light.readNormalizedValue());
}
private void rotate(double angle)