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.comm.RConsole;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Pose;
import lejos.util.Delay;
public class PilotAvoid {
private SlaveIOStreams PC;
......@@ -14,13 +22,24 @@ public class PilotAvoid {
DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter,
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.open();
// Send destination to PC.
PC.output(desX);
PC.output(desY);
pilot.setTravelSpeed(travelSpeed);
pilot.setRotateSpeed(rotateSpeed);
Pose initialPose = new Pose(0,0,0);
poseProvider.setPose(initialPose);
desPoint = new Point(desX, desY);
}
public void sendMove(Move move) {
......@@ -29,74 +48,39 @@ public class PilotAvoid {
PC.output(move.getAngleTurned());
}
public void travel(double distance) {
pilot.travel(distance, true);
public void travel(double distance, boolean immediateReturn) {
pilot.travel(distance, immediateReturn);
}
public void rotate(double angle) {
pilot.rotate(angle, true);
}
public void go() {
Sound.beep();
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 void rotate(double angle, boolean immediateReturn) {
pilot.rotate(angle, immediateReturn);
}
public static void main(String[] args) {
PilotAvoid pilot = new PilotAvoid(false);
PilotAvoid pilot = new PilotAvoid(false, 1000, 500);
LCD.clear();
LCD.drawString("PilotRouteMM", 0, 0);
// route.go();
LCD.drawString("PilotAvoid", 0, 0);
Sound.beep();
while (!Button.ENTER.isDown())
Thread.yield();
Sound.twoBeeps();
Behavior b1 = new FollowRoute(pilot);
Behavior b1 = new FindGoal(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.start();
}
}
class FollowRoute implements Behavior {
private static final int TRAVEL = 0;
private static final int ROTATE = 1;
class FindGoal implements Behavior {
private boolean _suppressed = false;
private PilotAvoid pilot;
private int stepsDone = 0;
public FollowRoute(PilotAvoid pilot) {
public FindGoal(PilotAvoid pilot) {
this.pilot = pilot;
}
......@@ -110,62 +94,66 @@ class FollowRoute implements Behavior {
public void action() {
_suppressed = false;
Sound.beepSequence();
Pose p = pilot.poseProvider.getPose();
// Calculate angle based on destination.
float angle = p.angleTo(pilot.desPoint);
// float degrees = (angle > 0) ? angle - p.getHeading() : angle + p.getHeading();
float degrees = - p.getHeading() + angle;
// if (angle < 0 && p.getHeading() < 0) {
// degrees = angle - p.getHeading();
// } else if (angle > 0 && p.getHeading() < 0) {
// degrees = angle + p.getHeading();
// } else if (angle > 0 && p.getHeading() > 0) {
// degrees = angle + p.getHeading();
// } else if (angle < 0 && p.getHeading() > 0) {
// degrees = angle - p.getHeading(); // done
// }
pilot.rotate(degrees, false);
pilot.sendMove(pilot.pilot.getMovement());
if (stepsDone < 1 && !_suppressed) {
doStep(TRAVEL, 500);
}
if (stepsDone < 2 && !_suppressed) {
doStep(ROTATE, 90);
}
if (stepsDone < 3 && !_suppressed) {
doStep(TRAVEL, 200);
}
if (stepsDone < 4 && !_suppressed) {
doStep(ROTATE, -90);
}
if (stepsDone < 5 && !_suppressed) {
doStep(TRAVEL, 500);
pilot.travel(p.distanceTo(pilot.desPoint), true);
while (!_suppressed && Motor.A.isMoving()) {
Thread.yield();
}
pilot.sendMove(pilot.pilot.getMovement());
Motor.A.stop(true); // not strictly necessary, but good programming practice
Motor.C.stop(true);
}
public void doStep(int moveType, double value) {
switch (moveType) {
case TRAVEL : pilot.travel(value); break;
case ROTATE : pilot.rotate(value); break;
default : System.err.println("Invalid moveType in doStep(): " + moveType); return;
}
stepsDone++;
while (!_suppressed && Motor.A.isMoving()) {
Thread.yield(); // don't exit till suppressed
}
pilot.sendMove(pilot.pilot.getMovement());
}
}
class AvoidObstacle extends Thread implements Behavior {
UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S1);
private float distance = 255;
UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S2);
private int distance = 255;
private int count = 0;
private boolean active = false;
private boolean _suppressed = false;
private PilotAvoid pilot;
public AvoidObstacle(PilotAvoid pilot) {
this.pilot = pilot;
this.isDaemon();
this.setDaemon(true);
this.start();
}
public void run() {
while (true) {
distance = sonar.getDistance();
LCD.drawInt(distance, 3, 0, 2);
}
}
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() {
......@@ -174,15 +162,75 @@ class AvoidObstacle extends Thread implements Behavior {
public void action() {
_suppressed = false;
active = true;
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.
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 && Motor.A.isMoving()) {
Thread.yield();
}
pilot.sendMove(pilot.pilot.getMovement());
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;
}
while (!_suppressed) {
Thread.yield(); // don't exit till suppressed
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);
}
// Motor.A.stop(true); // not strictly necessary, but good programming practice
// Motor.C.stop(true);
System.exit(0);
}
}
\ No newline at end of file
......@@ -29,7 +29,7 @@ public class InputOutputStreams
}
else {
// Connect to any NXT using Bluetooth
if (!conn.connectTo("btspp://")){
if (!conn.connectTo("btspp://NXT8")){
return "No NXT found using Bluetooth";
}
}
......
......@@ -21,8 +21,8 @@ public class PilotGUI extends JFrame {
// program will automatically adjust if these three constants are changed
static final int VIS_HEIGHT=1080; // visualization height
static final int VIS_WIDTH=1920; // visualization width
static final int Y_OFFSET=30; // vertical offset to visualization
static final int X_OFFSET=20; // horizontal offset to visualization
static final int Y_OFFSET=200; // vertical 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
// VIS_HEIGHT + 2 * Y_OFFSET is the window height
// VIS_WIDTH + 2 * X_OFFSET is the window width
......@@ -34,6 +34,8 @@ public class PilotGUI extends JFrame {
ArrayList<Point> route; // the route as seen from the vehicle
ArrayList<ParticleSet> particleSets = new ArrayList<ParticleSet>();
private int desX;
private int desY;
// an inner class to handle the visualization window being closed manually
class MyWindowAdapter extends WindowAdapter {
......@@ -43,9 +45,11 @@ public class PilotGUI extends JFrame {
}
// the constructor for the Visualize class
public PilotGUI(ParticleSet particles)
public PilotGUI(ParticleSet particles, int desX, int desY)
{
this.particles = particles;
this.desX = desX;
this.desY = desY;
this.setSize(new Dimension(VIS_WIDTH+X_OFFSET*2, VIS_HEIGHT+Y_OFFSET*2));
this.setTitle("Monte Carlo Localization");
this.setVisible(true);
......@@ -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)
......
......@@ -19,18 +19,23 @@ public class PilotMonitor
private boolean USB = false;
private Route route = new Route();
private ParticleSet particles = new ParticleSet(1000);
private PilotGUI view = new PilotGUI(particles);
private PilotGUI view;
private Move move;
public PilotMonitor()
{
String m;
view.update(route.getRoute(), false);
NXT = new InputOutputStreams(USB);
do {
m = NXT.open();
System.out.println(m);
} 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()
......
......@@ -54,7 +54,7 @@ public class ArenaRobot {
Behavior b1 = new Wander(pilot);
Behavior b2 = new AvoidEdge(pilot);
Behavior b3 = new Exit(pilot);
Behavior b3 = new Exit(pilot, 180);
Behavior[] behaviorList = {b1, b2, b3};
Arbitrator arbitrator = new Arbitrator(behaviorList);
arbitrator.start();
......@@ -85,7 +85,7 @@ class Wander implements Behavior {
double rand = Math.random();
if (rand > 0.5) {
pilot.travel(rand * 2000);
pilot.travel(rand * 200);
} else {
pilot.rotate((Math.random() > 0.5) ? rand * 180 : rand * -180);
}
......@@ -160,13 +160,18 @@ class AvoidEdge extends Thread implements Behavior {
class Exit implements Behavior {
private boolean _suppressed = false;
private ArenaRobot pilot;
private int seconds;
private long startTime;
public Exit(ArenaRobot pilot) {
public Exit(ArenaRobot pilot, int seconds) {
this.pilot = pilot;
this.seconds = seconds;
startTime = System.currentTimeMillis();
}
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() {
......
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)
{
pilot.rotate(angle);
sendMove(pilot.getMovement());
PC.output(light.readNormalizedValue());
}
public void go()
{
Sound.beep();
while ( ! Button.ENTER.isDown()) Thread.yield();
Sound.twoBeeps();
travel(0);
PC.input();
for (int i = 0; i < 6; i++) {
travel(115);
Delay.msDelay(500);
if (PC.input() == 1) {
// I have my location!
Sound.buzz();
haveLocation = true;
break;
}
}
if (!haveLocation) {
rotate(90);
for(int j = 0; j < 4; j++) {
travel(115);
Delay.msDelay(500);
if (PC.input() == 1) {
// I have my location!
Sound.buzz();
haveLocation = true;
break;
}
}
}
if (!haveLocation) {
rotate(90);
for(int j = 0; j < 4; j++) {
travel(115);
Delay.msDelay(500);
if (PC.input() == 1) {
// I have my location!
Sound.buzz();
haveLocation = true;
break;
}
}
}
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)
{
PilotControl2D route = new PilotControl2D(false);
LCD.clear();
LCD.drawString("PilotControl", 0, 0);
route.go();
}
}
import lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Move;
public class PilotController
{
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);
public PilotController(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 travel(double distance)
{
pilot.travel(distance);
sendMove(pilot.getMovement());
PC.output(light.readNormalizedValue());
}
private void rotate(double angle)
{
pilot.rotate(angle);
sendMove(pilot.getMovement());
}
public void go()
{
Sound.beep();
while ( ! Button.ENTER.isDown()) Thread.yield();