Commit 03a3171c authored by lildholdt's avatar lildholdt
Browse files

Monitor + Route

parent b90e5349
<?xml version="1.0" encoding="UTF-8"?>
<classpath>
<classpathentry kind="src" path="ArenaBot/src"/>
<classpathentry kind="src" path="ParticleBot/src"/>
<classpathentry kind="src" path="RobotMonitor/src"/>
<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER"/>
<classpathentry kind="output" path="RobotMonitor/bin"/>
</classpath>
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>PilotRoute</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>
<?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>PilotRoute</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.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.0;
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();
for(int i = 0; i < 4; i++)
{
pilot.travel(20);
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 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;
}
}
......@@ -20,6 +20,8 @@ public class RobotMonitor
private RobotGUI view = new RobotGUI(particles, m);
private Move move;
private int pause = 1000; // ms between views
private InputOutputStreams NXT;
private boolean USB = false;
public RobotMonitor()
{
......@@ -28,6 +30,15 @@ public class RobotMonitor
m.setColor(8, 0, Color.BLACK);
view.update(route.getRoute());
String m;
NXT = new InputOutputStreams(USB);
do {
m = NXT.open();
System.out.println(m);
} while ( m != "Connected");
System.out.println(m);
Delay.msDelay(pause);
}
......
Markdown is supported
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