Added files for lesson 10

Lesson A - Localization/.classpath

+<?xml version="1.0" encoding="UTF-8"?>
+<classpath>
+	<classpathentry kind="src" path="src"/>
+	<classpathentry kind="con" path="org.lejos.nxt.ldt.LEJOS_LIBRARY_CONTAINER/nxt"/>
+	<classpathentry kind="output" path="bin"/>
+</classpath>

Lesson A - Localization/.project

+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>Lesson A - Localization</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.lejos.nxt.ldt.leJOSBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.lejos.nxt.ldt.leJOSNature</nature>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+	</natures>
+</projectDescription>

Lesson A - Localization/src/PilotAvoid.java

+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;
+
+/**
+ * A program that uses the DifferentialPilot to make a differential driven car
+ * drive in a square. During the drive the OdometryPoseProvider is used to
+ * display the Pose of the car on the LCD after each movement of the car.
+ * 
+ * Use the PC tool nxjconsoleviewer to show the LCD output on a PC for easier
+ * inspection.
+ * 
+ * @author Ole Caprani
+ * @version 13.05.15
+ */
+public class PilotAvoid {
+	private static void show(Pose p) {
+		LCD.clear();
+		LCD.drawString("Pose X " + p.getX(), 0, 2);
+		LCD.drawString("Pose Y " + p.getY(), 0, 3);
+		LCD.drawString("Pose V " + p.getHeading(), 0, 4);
+	}
+	
+	static interface Action {
+		void perform();
+	}
+	
+	private static double wheelDiameter = 5.5765, trackWidth = 16.6025;
+
+	private static double travelSpeed = 5, rotateSpeed = 45;
+	private static NXTRegulatedMotor left = Motor.B;
+	private static NXTRegulatedMotor right = Motor.C;
+
+	private static DifferentialPilot pilot = new DifferentialPilot(wheelDiameter, trackWidth, left, right);
+	private static OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);
+	
+	static class Rotation implements Action {
+		
+		private float angle;
+
+		Rotation(float angle) {
+			this.angle = angle;
+		}
+
+		@Override
+		public void perform() {
+			pilot.rotate(angle, false);
+		}
+		
+	}
+	
+	static class Movement implements Action {
+		
+		private float distance;
+
+		Movement(float distance) {
+			this.distance = distance;
+		}
+		
+		@Override
+		public void perform() {
+			pilot.travel(distance, true);
+		}
+	}
+	
+	private static Action[] actions = new Action[] {
+		new Movement(75),
+		new Rotation(90),
+		new Movement(75),
+		new Rotation(-90),
+		new Movement(75)		
+	};
+
+	private static UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1);
+	private static int DANGER_DISTANCE = 15;
+
+	private static boolean interruptedByUltrasonicSensor = false;
+
+	private static double AVOID_DISTANCE = 20;
+	
+	public static void main(String[] args) throws Exception {
+		Thread thread = new Thread(
+				new Runnable() {
+					@Override
+					public void run() {
+						while(true) {
+							sensor.ping();
+							
+							if(sensor.getDistance() < DANGER_DISTANCE) {
+								interruptedByUltrasonicSensor = true;
+							}
+						}						
+					}
+				}
+		);
+		thread.setDaemon(true);
+		thread.start();
+		
+		pilot.setTravelSpeed(travelSpeed);
+		pilot.setRotateSpeed(rotateSpeed);
+		poseProvider.setPose(new Pose(0, 0, 0));
+
+		LCD.clear();
+		LCD.drawString("Pilot square", 0, 0);
+		Button.waitForAnyPress();
+
+		int currentAction = 0;
+		while(currentAction != actions.length) {
+			Point preactionPosition = new Point(poseProvider.getPose().getX(), poseProvider.getPose().getY());
+			
+			actions[currentAction].perform();
+			
+			while(pilot.isMoving()) {
+				if(interruptedByUltrasonicSensor && actions[currentAction] instanceof Movement) {
+					Movement movement = (Movement) actions[currentAction];
+					
+					pilot.stop();
+					
+					float distanceTraveled = poseProvider.getPose().distanceTo(preactionPosition);
+					
+					float distanceLeft = movement.distance - distanceTraveled;
+														
+					if(distanceLeft < AVOID_DISTANCE) {
+						LCD.clear();
+						LCD.drawString("I GIVE UP", 0, 0);
+						Delay.msDelay(10000);
+						shutdown();
+						return;
+					}
+					
+					// Avoid obstacle...
+					pilot.rotate(90);
+					pilot.travel(AVOID_DISTANCE);
+					pilot.rotate(-90);
+					pilot.travel(AVOID_DISTANCE * 2);
+					pilot.rotate(-90);
+					pilot.travel(AVOID_DISTANCE);
+					pilot.rotate(90);
+					
+					// Finish movement...
+					pilot.travel(distanceLeft);
+					
+					interruptedByUltrasonicSensor = false;
+				}
+			}
+			
+			currentAction++;
+		}
+		
+		show(poseProvider.getPose());
+		Delay.msDelay(10000);
+
+		shutdown();
+	}
+	
+	static void shutdown() {
+		pilot.stop();
+		LCD.drawString("Program stopped", 0, 0);
+		Button.waitForAnyPress();
+		Delay.msDelay(2000);
+	}
+
+}

Lesson A - Localization/src/PilotRoute.java

+import lejos.nxt.*;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.robotics.navigation.Move;
+
+public class PilotRoute {
+	private SlaveIOStreams PC;
+
+	//private double wheelDiameter = 5.5, trackWidth = 16.0;
+	private double wheelDiameter = 5.5765, trackWidth = 16.6025;
+	
+	private double travelSpeed = 5, rotateSpeed = 45;
+	private NXTRegulatedMotor left = Motor.B;
+	private NXTRegulatedMotor right = Motor.C;
+
+	private DifferentialPilot pilot = new DifferentialPilot(wheelDiameter,
+			trackWidth, left, right);
+
+	public PilotRoute(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());
+	}
+
+	private void rotate(double angle) {
+		pilot.rotate(angle);
+		sendMove(pilot.getMovement());
+	}
+
+	public void go() {
+		Sound.beep();
+		while (!Button.ENTER.isDown())
+			Thread.yield();
+		Sound.twoBeeps();
+
+		for(int i = 0; i < 15; i++) {
+			travel(10);
+			rotate(90);
+			travel(10);
+			rotate(-90);
+		}
+
+		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) {
+		PilotRoute route = new PilotRoute(false);
+
+		LCD.clear();
+		LCD.drawString("Pilot route", 0, 0);
+		route.go();
+	}
+}

Lesson A - Localization/src/PilotSquare.java

+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;
+/**
+ * A program that uses the DifferentialPilot to make a differential
+ * driven car drive in a square. During the drive the OdometryPoseProvider
+ * is used to display the Pose of the car on the LCD after each
+ * movement of the car.
+ * 
+ * Use the PC tool nxjconsoleviewer to show the LCD output on a PC
+ * for easier inspection.
+ * 
+ * @author  Ole Caprani
+ * @version 13.05.15
+ */
+public class PilotSquare 
+{
+   private static void show(Pose p)
+   {
+      LCD.clear();
+       LCD.drawString("Pose X " + p.getX(), 0, 2);
+       LCD.drawString("Pose Y " + p.getY(), 0, 3);
+       LCD.drawString("Pose V " + p.getHeading(), 0, 4);
+   }
+
+   public static void main(String [] args)  
+   throws Exception 
+   {
+       //double wheelDiameter = 5.5, trackWidth = 16.0;
+       
+	   // ca. 1 cm forkert hver gang i 3 forsg.
+	   // =>
+	   
+	   //double wheelDiameter = 5.45, trackWidth = 16.0;
+	   // Krte lngere
+	   
+	   //double wheelDiameter = 5.5765, trackWidth = 16.0;
+	   
+	   
+	   // Roterer et par grader lidt
+	   //double wheelDiameter = 5.5765, trackWidth = 16.1;
+	   
+	   //double wheelDiameter = 5.5765, trackWidth = 16.255;
+	   
+	   //double wheelDiameter = 5.5765, trackWidth = 16.65;
+	   double wheelDiameter = 5.5765, trackWidth = 16.6025;
+	   
+	   double travelSpeed = 5, rotateSpeed = 45;
+       NXTRegulatedMotor left = Motor.B;
+       NXTRegulatedMotor right = Motor.C;
+	   
+       DifferentialPilot pilot = new DifferentialPilot(wheelDiameter, trackWidth, left, right);
+       OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);
+       Pose initialPose = new Pose(0,0,0);
+       //RConsole.open();
+       pilot.setTravelSpeed(travelSpeed);
+       pilot.setRotateSpeed(rotateSpeed);
+       poseProvider.setPose(initialPose);
+       
+       LCD.clear();
+       LCD.drawString("Pilot square", 0, 0);
+       Button.waitForAnyPress();
+	   
+       
+       //opg0(pilot, poseProvider);
+       //opg1(pilot, poseProvider);
+       opg2(pilot, poseProvider);
+       
+       pilot.stop();
+       LCD.drawString("Program stopped", 0, 0);
+       Button.waitForAnyPress();
+       Thread.sleep(2000);
+       RConsole.close();
+   }
+
+private static void opg2(DifferentialPilot pilot,
+		OdometryPoseProvider poseProvider) {
+	pilot.rotate(180);
+	Delay.msDelay(2000);
+}
+
+private static void opg1(DifferentialPilot pilot,
+		OdometryPoseProvider poseProvider) {
+    pilot.travel(50);
+    show(poseProvider.getPose());
+    Delay.msDelay(2500);
+
+}
+
+private static void opg0(DifferentialPilot pilot, OdometryPoseProvider poseProvider) {
+    for(int i = 0; i < 4; i++)
+    {
+        pilot.travel(40);           
+        show(poseProvider.getPose());
+ 	   Delay.msDelay(2000);
+                   
+        pilot.rotate(90);
+        show(poseProvider.getPose());
+        Delay.msDelay(1000);
+    }
+}
+}

Lesson A - Localization/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;
+	}
+
+}

Lesson A - PC/.classpath

+<?xml version="1.0" encoding="UTF-8"?>
+<classpath>
+	<classpathentry kind="src" path="src"/>
+	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.8"/>
+	<classpathentry kind="con" path="org.lejos.nxt.ldt.LEJOS_LIBRARY_CONTAINER/pc"/>
+	<classpathentry kind="con" path="org.lejos.nxt.ldt.LEJOS_LIBRARY_CONTAINER/nxt"/>
+	<classpathentry kind="output" path="bin"/>
+</classpath>

Lesson A - PC/.project

+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>Lesson A - PC</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+	</natures>
+</projectDescription>

Lesson A - PC/.settings/org.eclipse.jdt.core.prefs

+eclipse.preferences.version=1
+org.eclipse.jdt.core.compiler.codegen.inlineJsrBytecode=enabled
+org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.8
+org.eclipse.jdt.core.compiler.codegen.unusedLocal=preserve
+org.eclipse.jdt.core.compiler.compliance=1.8
+org.eclipse.jdt.core.compiler.debug.lineNumber=generate
+org.eclipse.jdt.core.compiler.debug.localVariable=generate
+org.eclipse.jdt.core.compiler.debug.sourceFile=generate
+org.eclipse.jdt.core.compiler.problem.assertIdentifier=error
+org.eclipse.jdt.core.compiler.problem.enumIdentifier=error
+org.eclipse.jdt.core.compiler.source=1.8