Added files for lesson 10

Lesson A - PC/src/InputOutputStreams.java

+
+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;
+	}
+}

Lesson A - PC/src/Particle.java

+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);
+	}
+	
+}

Lesson A - PC/src/ParticleSet.java

+
+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;
+  }
+
+}

Lesson A - PC/src/PilotGUI.java

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

Lesson A - PC/src/PilotMonitor.java

+/**
+ * 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

Lesson A - PC/src/Route.java

+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;
+    }
+}

Lesson A - PC/src/SlaveIOStreams.java

+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;
+	}
+
+}