Commit 664231fc authored by René Nilsson's avatar René Nilsson
Browse files

Add image and particle filter project

parent f4e0d1f2
<?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>ParticleBot</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>
/**
* Arbitrator controls which Behavior object will become active in
* a behavior control system.
*
* Make sure to call start() after the Arbitrator is instantiated.<br>
*
* This class has three major responsibilities: <br>
* 1. Determine the highest priority behavior that returns <b> true </b> to takeControl()<br>
* 2. Suppress the active behavior if its priority is less than highest priority. <br>
* 3. When the action() method exits, call action() on the Behavior of highest priority.
* <br> The Arbitrator assumes that a Behavior is no longer active when action() exits,
* <br> therefore it will only call suppress() on the Behavior whose action() method is running.
* <br> It can make consecutive calls of action() on the same Behavior.
* <br> Requirements for a Behavior:
* <br> When suppress() is called, terminate action() immediately.
* <br> When action() exits, the robot is in a safe state (e.g. motors stopped)
* @see Behavior
* @author Roger Glassey
*
* Modified so the Behavior with the highest integer priority returned by
* takeControl is the active behavior. Also modified so start() never exits,
* there should always be a behavior that wants to be active.
*
* Ole Caprani, 24-12-2012
*
*/
public class Arbitrator
{
private final int NONE = -1;
private Behavior[] behavior;
private BehaviorAction actionThread;
private int currentBehavior = NONE;
private int currentPriority = NONE;
public Arbitrator(Behavior[] behaviorList)
{
behavior = behaviorList;
actionThread = new BehaviorAction();
actionThread.setDaemon(true);
}
/**
* This method starts the arbitration of Behaviors and runs an endless loop.
* The start() method will never return
*/
public void start()
{
int highest, maxPriority;
actionThread.start();
while (true)
{
// Find behavior with highest priority
maxPriority = -1; highest = -1;
for (int i = 0; i < behavior.length; i++)
{
int priority = behavior[i].takeControl();
if (priority > maxPriority )
{
highest = i;
maxPriority = priority;
}
}
// Start highest priority process and update currentPriority
if ( actionThread.current == NONE)
{
currentBehavior = highest;
currentPriority = maxPriority;
actionThread.execute(highest);
}
else if ( currentPriority < maxPriority )
{
behavior[currentBehavior].suppress();
currentBehavior = highest;
currentPriority = maxPriority;
actionThread.execute(highest);
}
else
currentPriority = maxPriority;
Thread.yield();
}
}
/**
* Local thread that runs the action method for the currently
* highest priority behavior
*/
private class BehaviorAction extends Thread
{
public int current = NONE;
public void run()
{
while (true)
{
synchronized (this)
{
if ( current != NONE )
{
behavior[current].action();
current = NONE;
}
}
Thread.yield();
}
}
public synchronized void execute(int index)
{
current = index;
}
}
}
import lejos.robotics.navigation.DifferentialPilot;
/**
* The Behavior interface represents an object embodying a specific
* behavior belonging to a robot. Each behavior must define three things: <BR>
* 1) The circumstances to make this behavior seize control of the robot.
* e.g. When the touch sensor determines the robot has collided with an object.<BR>
* 2) The action to perform when this behavior takes control.
* e.g. Back up and turn.<BR>
* 3) A way to quickly exit from the action when the Arbitrator selects a higher
* priority behavior to take control.
* These are represented by defining the methods takeControl(), action(),
* and suppress() respectively. <BR>
* A behavior control system has one or more Behavior objects. When you have defined
* these objects, create an array of them and use that array to initialize an
* Arbitrator object.
*
* @see Arbitrator
* @version 0.9 May 2011
* Modified so takeControl returns an integer, Ole Caprani 24-12-2012
*/
public abstract class Behavior {
protected volatile boolean _suppressed;
protected DifferentialPilot pilot;
/**
* The integer returned indicates how much this behavior wants control of the robot.
* For example, a robot that reacts if a touch sensor is pressed: <BR>
* public int takeControl() { <BR>
* if ( touch.isPressed() ) return 100;
* return 0; <BR>
* } <BR>
* @return integer Indicates if this Behavior should seize control.
*/
public abstract int takeControl();
/**
* The code in action() represents the tasks the robot performs when this
* behavior becomes active. It can be as complex as navigating around a
* room, or as simple as playing a tune.<BR>
* <B>The contract for implementing this method is:</B><BR>
* If its task is is complete, the method returns.
* It also <B> must </B> return promptly when the suppress() method
* is called, for example by testing the boolean suppress flag. <br>
* When this method exits, the robot is in a safe state for another behavior
* to run its action() method
*/
public void action()
{
_suppressed = false;
}
/**
* The code in suppress() should cause the current behavior to exit. <BR>
* <B>The contract for implementing this method is:</B><BR>
* Exit quickly, for example, just set boolean flag.
*/
public void suppress()
{
_suppressed = true;
pilot.stop();
ParticleBot.show();
}
protected void waitWhileActive()
{
while (!_suppressed )
{
Thread.yield(); //don't exit till suppressed
}
}
protected void wait(int waitTime)
{
int now = (int)System.currentTimeMillis();
while (!_suppressed && ((int)System.currentTimeMillis()< now + waitTime) )
{
Thread.yield(); //don't exit till suppressed
}
}
protected void waitForMovement()
{
while (!_suppressed && (pilot.isMoving()) )
{
Thread.yield(); //don't exit till suppressed
}
}
}
\ No newline at end of file
import java.util.Random;
public class BetterRandom extends Random {
@Override
public boolean nextBoolean()
{
//since nextInt(int n) always returns an uneven number, nextBoolean will currently always return true. This fixes that.
int unevenNumber = nextInt(101);
return unevenNumber > 50;
}
}
import lejos.nxt.*;
import lejos.nxt.comm.RConsole;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Pose;
import lejos.util.Delay;
public class ParticleBot
{
private static double distFactor = 0.99;
private static double wheelDiameterL = 5.595*distFactor, wheelDiameterR = 5.6*distFactor, trackWidth = 16.27;
private static double travelSpeed = 15, rotateSpeed = 45;
private static NXTRegulatedMotor left = Motor.C;
private static NXTRegulatedMotor right = Motor.B;
private static DifferentialPilot pilot = new DifferentialPilot(wheelDiameterL,wheelDiameterR,trackWidth, left, right,false);
private static OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);
public static void main(String[] args)
{
RConsole.open();
pilot.setTravelSpeed(travelSpeed);
pilot.setRotateSpeed(rotateSpeed);
Pose initialPose = new Pose(0,0,0);
poseProvider.setPose(initialPose);
Button.ESCAPE.addButtonListener(new ButtonListener() {
@Override
public void buttonReleased(Button b) {
}
@Override
public void buttonPressed(Button b) {
RConsole.close();
System.exit(0);
}
});
StayInRing stay = new StayInRing(pilot);
stay.calibrate();
Wander wander = new Wander(pilot);
Behavior[] behaviorList =
{
wander, stay
};
Arbitrator arbitrator = new Arbitrator(behaviorList);
LCD.drawString("Press to start ", 0, 3);
Button.waitForAnyPress();
LCD.clear();
show();
Delay.msDelay(1000);
arbitrator.start();
}
public static void show()
{
Pose p = poseProvider.getPose();
LCD.clear();
LCD.drawString("Pose X " + p.getX(), 0, 0);
LCD.drawString("Pose Y " + p.getY(), 0, 1);
LCD.drawString("Pose V " + p.getHeading(), 0, 2);
}
}
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 lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;
public class StayInRing extends Behavior {
LightSensor rightLight = new LightSensor(SensorPort.S1, true);
LightSensor leftLight = new LightSensor(SensorPort.S4, true);
TouchSensor touchSensor = new TouchSensor(SensorPort.S2);
int rightThreshold = 0;
int leftThreshold = 0;
final int ActivePrio = Integer.MAX_VALUE; //self preservation!
final int ROTATION = 200;
volatile int rlightVal = 0;
volatile int llightVal = 0;
volatile boolean touch = false;
boolean isEdgeAtRight = false;
boolean isEdgeAtLeft = false;
int whiteDetections = 0;
Thread lightReader;
public StayInRing(DifferentialPilot pilot)
{
this.pilot = pilot;
lightReader = new Thread(new Runnable() {
@Override
public void run() {
while(true)
{
rlightVal = rightLight.getLightValue();
llightVal = leftLight.getLightValue();
touch = touchSensor.isPressed();
}
}
});
lightReader.setDaemon(true);
lightReader.start();
}
public void calibrate()
{
int rblackLight = 0, rwhiteLight = 0;
int lblackLight = 0, lwhiteLight = 0;
while(Button.ENTER.isDown());
LCD.clear();
LCD.drawString("Calibrate black.", 0, 0);
while(Button.ENTER.isUp())
{
rblackLight = rightLight.getLightValue();
lblackLight = leftLight.getLightValue();
LCD.drawInt(rblackLight, 3, 4, 1);
LCD.drawInt(lblackLight, 3, 0, 1);
}
while(Button.ENTER.isDown());
LCD.clear();
LCD.drawString("Calibrate white.", 0, 0);
while(Button.ENTER.isUp())
{
rwhiteLight = rightLight.getLightValue();
lwhiteLight = leftLight.getLightValue();
LCD.drawInt(rwhiteLight, 3, 4, 1);
LCD.drawInt(lwhiteLight, 3, 0, 1);
}
final float alpha = 0.3f;
rightThreshold = (int) (rblackLight * (1-alpha) + rwhiteLight*alpha);
leftThreshold = (int)(lblackLight * (1-alpha) + lwhiteLight*alpha);
LCD.clear();
LCD.drawString("Thrsh: ", 0, 0);
LCD.drawInt(rightThreshold, 3, 11, 0);
LCD.drawInt(leftThreshold, 3, 7, 0);
while(Button.ENTER.isDown());
}
@Override
public int takeControl() {
isEdgeAtRight = rlightVal < rightThreshold;
isEdgeAtLeft = llightVal < leftThreshold;
return isEdgeAtRight || isEdgeAtLeft || touch ? ActivePrio : 0;
}
@Override
public void action() {
super.action();
//take local copies, since takeControl is called continuously, updating these variables.
boolean _isEdgeAtRight = isEdgeAtRight;
boolean _isEdgeAtLeft = isEdgeAtLeft;
boolean _touch = touch;
LCD.drawString("Light or touch!", 0, 4);
pilot.travel(-10, true);
waitForMovement();
LCD.clear(4);
if(_isEdgeAtRight && _isEdgeAtLeft)
{
LCD.drawString("Both light!", 0, 4);
pilot.rotate(90, true);
waitForMovement();
}
else if (_touch)
{
LCD.drawString("Touch!", 0, 4);
pilot.rotate(90, true);
waitForMovement();
}
else if (_isEdgeAtRight)
{
pilot.rotate(45, true);
LCD.drawString("Light right!", 0, 4);
waitForMovement();
}
else if (_isEdgeAtLeft)
{
pilot.rotate(-45, true);
LCD.drawString("Light left!", 0, 4);
waitForMovement();
}
ParticleBot.show();
LCD.clear(4);
}
}
import java.util.Random;
import lejos.nxt.LCD;
import lejos.robotics.navigation.DifferentialPilot;
public class Wander extends Behavior {
final int MINANGLE = 10;
final int MAXANGLE = 180;
final int MINFORWARD = 10;
final int MAXFORWARD = 50;
Random rnd = new BetterRandom();
public Wander(DifferentialPilot pilot) {
this.pilot = pilot;
}
@Override
public int takeControl() {
return 1; //lowest active priority. Always wander when nothing else happens.
}
@Override
public void action() {
super.action();
boolean doRotation = rnd.nextBoolean();
if(doRotation)
{