Commit 62fb2d25 authored by Peder Kronsgaard Detlefsen's avatar Peder Kronsgaard Detlefsen
Browse files

Added inital sumo robot code that detects white.

parent dc27753e
/**
* 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;
}
}
}
/**
* 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 interface Behavior {
/**
* 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 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();
/**
* 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();
}
\ No newline at end of file
import lejos.nxt.*;
import lejos.robotics.RegulatedMotor;
/**
* Demonstration of the Behavior subsumption classes.
*
* Requires a wheeled vehicle with two independently controlled motors connected
* to motor ports A and C, and a touch sensor connected to sensor port 1 and an
* ultrasonic sensor connected to port 3;
*
* @author Brian Bagnall and Lawrie Griffiths, modified by Roger Glassey
*
* Uses a new version of the Behavior interface and Arbitrator with
* integer priorities returned by takeCaontrol instead of booleans.
*
* Exit behavior inserted, local distance sampling thread and backward
* drive added in DetectWall by Ole Caprani, 23-4-2012
*/
public class BumperCar {
public static void main(String[] args) {
Motor.B.setSpeed(2000);
Motor.A.setSpeed(400);
Motor.C.setSpeed(400);
Behavior b1 = new DriveForward();
Behavior b2 = new DetectWhite();
Behavior b3 = new Exit();
Behavior[] behaviorList = { b1, b2, b3 };
Arbitrator arbitrator = new Arbitrator(behaviorList);
LCD.drawString("Bumper Car", 0, 1);
Button.waitForAnyPress();
try {
Thread.sleep(3000);
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
Motor.B.backward();
arbitrator.start();
}
}
class DriveForward implements Behavior {
private boolean _suppressed = false;
public int takeControl() {
return 10; // this behavior always wants control.
}
public void suppress() {
_suppressed = true;// standard practice for suppress methods
}
public void action() {
_suppressed = false;
Motor.A.forward();
Motor.C.forward();
LCD.drawString("Drive forward", 0, 2);
while (!_suppressed) {
Thread.yield(); // don't exit till suppressed
}
Motor.A.stop(); // not strictly necessary, but good programming practice
Motor.C.stop();
LCD.drawString("Drive stopped", 0, 2);
}
}
class DetectWhite extends Thread implements Behavior {
private LightSensor light;
// private UltrasonicSensor sonar;
private boolean _suppressed = false;
private boolean active = false;
private int distance = 255;
public DetectWhite() {
light = new LightSensor(SensorPort.S3);
// sonar = new UltrasonicSensor(SensorPort.S3);
this.setDaemon(true);
this.start();
}
public void run() {
// while (true)
// distance = sonar.getDistance();
}
public int takeControl() {
return Math.max(0, light.readValue() - 40);
}
public void suppress() {
_suppressed = true;// standard practice for suppress methods
}
public void action() {
_suppressed = false;
active = true;
Sound.beepSequenceUp();
// Backward for 1000 msec
LCD.drawString("Drive backward", 0, 3);
Motor.A.backward();
Motor.C.backward();
long now = System.currentTimeMillis();
while (!_suppressed && (System.currentTimeMillis() < now + 1000)) {
Thread.yield(); // don't exit till suppressed
}
// Turn
LCD.drawString("Turn ", 0, 3);
Motor.A.rotate(-180, true);// start Motor.A rotating backward
Motor.C.rotate(-800, true); // rotate C farther to make the turn
while (!_suppressed && Motor.C.isMoving()) {
Thread.yield(); // don't exit till suppressed
}
Motor.A.stop();
Motor.C.stop();
LCD.drawString("Stopped ", 0, 3);
Sound.beepSequence();
active = false;
}
}
class Exit implements Behavior {
private boolean _suppressed = false;
public int takeControl() {
if (Button.ESCAPE.isDown())
return 200;
return 0;
}
public void suppress() {
_suppressed = true;// standard practice for suppress methods
}
public void action() {
System.exit(0);
}
}
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