Commit 5e9feeea authored by Peder Kronsgaard Detlefsen's avatar Peder Kronsgaard Detlefsen
Browse files

Added lesson 10 code.

parent fb080a29
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 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();
travel(50);
rotate(90);
travel(20);
rotate(-90);
travel(50);
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();
}
}
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.5;
double travelSpeed = 5, rotateSpeed = 45;
NXTRegulatedMotor left = Motor.A;
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();
for(int i = 0; i < 4; i++)
{
pilot.travel(50);
show(poseProvider.getPose());
Delay.msDelay(1000);
pilot.rotate(90);
show(poseProvider.getPose());
Delay.msDelay(1000);
}
pilot.stop();
LCD.drawString("Program stopped", 0, 0);
Button.waitForAnyPress();
Thread.sleep(2000);
RConsole.close();
}
}
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 PilotStraight
{
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;
double travelSpeed = 5, rotateSpeed = 45;
NXTRegulatedMotor left = Motor.A;
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();
pilot.travel(25);
show(poseProvider.getPose());
pilot.stop();
LCD.drawString("Program stopped", 0, 0);
Button.waitForAnyPress();
Thread.sleep(2000);
RConsole.close();
}
}
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 PilotTurn
{
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.45;
double travelSpeed = 5, rotateSpeed = 45;
NXTRegulatedMotor left = Motor.A;
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();
pilot.rotate(90);
show(poseProvider.getPose());
pilot.stop();
LCD.drawString("Program stopped", 0, 0);
Button.waitForAnyPress();
Thread.sleep(2000);
RConsole.close();
}
}
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;
}
}
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.*;
import lejos.robotics.mapping.RangeMap;
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 applyMove(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));
}
}
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 = 0.2f; //0.2f;
private float angleNoiseFactor = 4; //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