Commit b62b7e1f authored by Mikkel's avatar Mikkel
Browse files

Added files for lesson 10

parents
<?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>
<?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>
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);
}
}
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();
}
}
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);
}
}
}
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;
}
}
<?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>
<?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>
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
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