Commit b62b7e1f authored by Mikkel's avatar Mikkel
Browse files

Added files for lesson 10

parents
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import lejos.pc.comm.NXTConnector;
public class InputOutputStreams
{
private NXTConnector conn;
private boolean USB;
private DataInputStream inDat;
private DataOutputStream outDat;
public InputOutputStreams(boolean USB)
{
conn = new NXTConnector();
this.USB = USB;
}
public String open()
{
if ( USB )
{
// Connect to any NXT using USB
if (!conn.connectTo("usb://")){
return "No NXT found using USB";
}
}
else {
// Connect to any NXT using Bluetooth
if (!conn.connectTo("btspp://")){
return "No NXT found using Bluetooth";
}
}
inDat = new DataInputStream(conn.getInputStream());
outDat = new DataOutputStream(conn.getOutputStream());
return "Connected";
}
public boolean output(float param)
{
boolean result;
try {
outDat.writeFloat(param);
outDat.flush();
result = true;
} catch (Exception e) {
result = false;
}
return result;
}
public float input()
{
float result;
try {
result = inDat.readFloat();
} catch (Exception e) {
result = -1;
}
return result;
}
public String close()
{
String result;
try {
inDat.close();
outDat.close();
result = "Closed streams";
} catch (Exception e) {
result = "IO Exception streams";
}
try {
conn.close();
result = result + " Closed connection";
} catch (Exception e) {
result = result + " IO Exception connection";
}
return result;
}
}
import java.util.Random;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Pose;
import lejos.geom.*;
/**
* Represents a particle for the particle filtering algorithm. The state of the
* particle is the pose, which represents a possible pose of the robot.
*
* A version of the leJOS class MCLParticle modified for position tracking. The
* weights and weight updates have been left out.
*
* @author Ole Caprani
* @version 18.05.15
*/
public class Particle {
private static Random rand = new Random();
private Pose pose;
/**
* Create a particle with a specific pose
*
* @param pose
* the pose
*/
public Particle(Pose pose) {
this.pose = pose;
}
/**
* Return the pose of this particle
*
* @return the pose
*/
public Pose getPose() {
return pose;
}
/**
* Apply the robot's move to the particle with a bit of random noise. Only
* works for rotate or travel movements.
*
* This is the original version from MCLParticle
*
* @param move
* the robot's move
*/
public void applyMoveOld(Move move, float distanceNoiseFactor, float angleNoiseFactor) {
float ym = (move.getDistanceTraveled() * ((float) Math.sin(Math.toRadians(pose.getHeading()))));
float xm = (move.getDistanceTraveled() * ((float) Math.cos(Math.toRadians(pose.getHeading()))));
pose.setLocation(new Point(
(float) (pose.getX() + xm + (distanceNoiseFactor * xm * rand.nextGaussian())),
(float) (pose.getY() + ym + (distanceNoiseFactor * ym * rand.nextGaussian())))
);
pose.setHeading((float) (pose.getHeading() + move.getAngleTurned() + (angleNoiseFactor * rand.nextGaussian())));
pose.setHeading((float) ((int) (pose.getHeading() + 0.5f) % 360));
}
public void applyMove(Move move, float distanceNoiseFactor, float angleNoiseFactor) {
float theta = pose.getHeading() + move.getAngleTurned() + move.getAngleTurned() * angleNoiseFactor * 0.1f * (float) rand.nextGaussian();
float dx = move.getDistanceTraveled() * (float) Math.cos(Math.toRadians(theta)) + move.getDistanceTraveled() * distanceNoiseFactor * (float) rand.nextGaussian();
float dy = move.getDistanceTraveled() * (float) Math.sin(Math.toRadians(theta)) + move.getDistanceTraveled() * distanceNoiseFactor * (float) rand.nextGaussian();
pose.setLocation(pose.getX() + dx, pose.getY() + dy);
pose.setHeading(theta);
}
}
import lejos.geom.*;
import lejos.robotics.*;
import lejos.robotics.mapping.RangeMap;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Pose;
import java.util.Random;
/**
* Represents a particle set for the particle filtering algorithm.
*
* A version of the leJOS class MCLParticleSet modified for position tracking.
* The initial set consist only of a known start position Pose(0,0,0).
*
* @author Ole Caprani
* @version 18.05.15
*
*/
public class ParticleSet
{
// Instance variables
private float distanceNoiseFactor = (float) Math.sqrt(0.0064f * 0.0064f + 0.0056f * 0.0056f);//0.2f; //0.2f;
private float angleNoiseFactor = 0.2333f; //4f;
private int numParticles;
private Particle[] particles;
/**
* Create a set of particles. All particles are identical and equal to
* Pose(0,0,0), i.e. the robot starts in (0,0) heading in the positive
* direction of the x-axis.
*
*/
public ParticleSet(int numParticles)
{
this.numParticles = numParticles;
particles = new Particle[numParticles];
for (int i = 0; i < numParticles; i++)
{
particles[i] = new Particle(new Pose(0, 0, 0));
}
}
/**
* Return the number of particles in the set
*
* @return the number of particles
*/
public int numParticles()
{
return numParticles;
}
/**
* Get a specific particle
*
* @param i the index of the particle
* @return the particle
*/
public Particle getParticle(int i)
{
return particles[i];
}
/**
* Apply a move to each particle
*
* @param move the move to apply
*/
public void applyMove(Move move)
{
for (int i = 0; i < numParticles; i++)
{
particles[i].applyMove(move, distanceNoiseFactor, angleNoiseFactor);
}
}
/**
* Set the distance noise factor
* @param factor the distance noise factor
*/
public void setDistanceNoiseFactor(float factor)
{
distanceNoiseFactor = factor;
}
/**
* Set the distance angle factor
* @param factor the distance angle factor
*/
public void setAngleNoiseFactor(float factor)
{
angleNoiseFactor = factor;
}
}
/**
* The PilotGUI Class
*
* This class can be used to visualize the actions of a particle filter
* Localization algorithm while a vehicle drives a route.
*
* @author Ole Caprani
* @version 14.05.2015
*/
import java.awt.*;
import java.awt.event.*;
import java.util.ArrayList;
import javax.swing.JFrame;
import lejos.geom.Point;
import lejos.robotics.navigation.Pose;
public class PilotGUI extends JFrame {
// program will automatically adjust if these three constants are changed
static final int VIS_HEIGHT=400; // visualization height
static final int VIS_WIDTH=600; // 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 POINT_WIDTH=4; // 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
ParticleSet particles; // the set of sample possible locations
ArrayList<Point> route; // the route as seen from the vehicle
// an inner class to handle the visualization window being closed manually
class MyWindowAdapter extends WindowAdapter {
public void windowClosing(WindowEvent w) {
System.exit(0);
}
}
// the constructor for the Visualize class
public PilotGUI(ParticleSet particles)
{
this.particles = particles;
this.setSize(new Dimension(VIS_WIDTH+X_OFFSET*2, VIS_HEIGHT+Y_OFFSET*2));
this.setTitle("Monte Carlo Localization");
this.setVisible(true);
this.setBackground(Color.WHITE);
addWindowListener(new MyWindowAdapter());
}
// update the visualization display based on the estimated vehicle location and
// and the locations of the samples
public void update(ArrayList<Point> route)
{
this.route = route;
repaint();
}
// paint the visualization window - called by repaint in update method, and also by the
// run-time system whenever it decides the window need to be updated (e.g., when uncovered)
public void paint(Graphics g)
{
Graphics2D g2 = ( Graphics2D ) g;
Pose p; Float w;
super.paint(g);
g2.setColor(Color.RED);
g2.drawRect(winX(0), winY(VIS_HEIGHT), VIS_WIDTH, VIS_HEIGHT);
g2.setColor(Color.BLUE);
int x1 = winX(0), y1 = winY(0), x2, y2;
int size = route.size();
for (int i = 1; i < size ; i++)
{
Point current = route.get(i);
x2 = winX((int)current.getX());
y2 = winY((int)current.getY());
g2.drawLine(x1, y1, x2, y2);
x1 = x2; y1 = y2;
}
for (int i = 0; i < particles.numParticles(); i++)
{
p = particles.getParticle(i).getPose();
g2.setColor(Color.BLACK);
g2.fillRect(winX((int)p.getX()-POINT_WIDTH/2) ,
winY((int)p.getY()+POINT_WIDTH/2),
POINT_WIDTH, POINT_WIDTH);
}
}
private int winX(int x)
{
return x + X_OFFSET;
}
private int winY(int y)
{
return VIS_HEIGHT - y + Y_OFFSET;
}
}
/**
* The PilotMonitor receives move parameters from a
* vehicle drining in a route of DifferentialPilot
* travel or rotate steps. The move parameters are used to update
* a visualization of the route and possible locations of
* the vehicle after each step.
*
* @author Ole Caprani
* @version 18-5-2015
*/
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Pose;
public class PilotMonitor
{
private InputOutputStreams NXT;
private boolean USB = false;
private Route route = new Route();
private ParticleSet particles = new ParticleSet(10);
private PilotGUI view = new PilotGUI(particles);
private Move move;
public PilotMonitor()
{
String m;
view.update(route.getRoute());
NXT = new InputOutputStreams(USB);
do {
m = NXT.open();
System.out.println(m);
} while ( m != "Connected");
}
private Move getMove()
{
Move m;
int type = (int)NXT.input();
float distance = NXT.input();
float angle = NXT.input();
m = new Move( (type == 0)? Move.MoveType.TRAVEL : Move.MoveType.ROTATE ,
distance, angle, false);
System.out.println("Move " + distance + " " + angle);
return m;
}
public void go()
{
while ( true )
{
move = getMove();
particles.applyMove(move);
route.update(move);
view.update(route.getRoute());
Pose p = route.getCurrentPose();
System.out.println("Pose " + p.getX() + " " + p.getY() + " " + p.getHeading());
}
}
public static void main (String [] args)
{
PilotMonitor monitor = new PilotMonitor();
monitor.go();
}
}
\ No newline at end of file
import java.util.ArrayList;
import lejos.geom.Point;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Pose;
public class Route
{
// The route driven as seen from the robot
private ArrayList<Point> route = new ArrayList<Point>();
private Pose currentPose = new Pose();
private float x, y;
public Route()
{
currentPose.setLocation(0, 0); currentPose.setHeading(0);
route.add(new Point(0,0));
}
private float normalize(float angle)
{
float a = angle;
if ( a >= 0 )
{
while ( a > 360 )
a = a - 360;
}
else
{
while ( a < -360 )
a = a + 360;
}
return a;
}
public void update(Move move)
{
if ( move.getMoveType() == Move.MoveType.TRAVEL)
{
float d = move.getDistanceTraveled();
double aRad = Math.toRadians(currentPose.getHeading());
x = currentPose.getX() + d * ((float) Math.cos(aRad));
y = currentPose.getY() + d * ((float) Math.sin(aRad));
currentPose.setLocation(x, y);
route.add(new Point(x,y));
}
else
{
currentPose.setHeading(
normalize(currentPose.getHeading() + move.getAngleTurned()));
}
}
public ArrayList<Point> getRoute()
{
return route;
}
public Pose getCurrentPose()
{
return currentPose;
}
}
import java.io.DataInputStream;
import java.io.DataOutputStream;
import lejos.nxt.LCD;
import lejos.nxt.Sound;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.USB;
import lejos.nxt.comm.NXTConnection;
public class SlaveIOStreams {
private NXTConnection conn;
private boolean usb;
private DataOutputStream dOut;
private DataInputStream dIn;
public SlaveIOStreams(boolean usb)
{
this.usb = usb;
}
public void open()
{
Sound.beep();
if ( usb ){
LCD.drawString("Waiting USB", 0, 0);
conn = USB.waitForConnection();
LCD.drawString("Connected USB", 0, 0);
}
else
{
LCD.drawString("Waiting BT", 0, 0);
conn = Bluetooth.waitForConnection();
LCD.drawString("Connected BT", 0, 0);
}
Sound.twoBeeps();
dIn = conn.openDataInputStream();
dOut = conn.openDataOutputStream();
}
public float input()
{
float result;
try
{
result = dIn.readFloat();
}
catch (Exception e)
{
result = -1;
}
return result;
}
public boolean output(float r)
{
boolean result;
try
{
dOut.writeFloat(r);
dOut.flush();
result = true;
}
catch (Exception e)
{
result = false;
}
return result;
}
public boolean close()
{
boolean result;
try
{
dOut.close();
dIn.close();
conn.close();
result = true;
}
catch (Exception e)
{
result = false;
}
return result;
}
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment