Next: 課題
Up: 7 Subsumption1サンプル
Previous: 7.1 クラス構造
import josx.platform.rcx.*;
/**
* Entry point for the program.
* Creates an instance of Subsumption
* and kicks of the lowest priority task (wander)
*/
public class Main {
public static Subsumption main;
public static void main (String[] arg)
throws Exception {
main = new Subsumption();
main.tasks[0].execute();
}
}
/**
* Coordinates a prioritised set of tasks.
* Tasks with higher priority
* completely block those of a lower priority.
* Creates the following
* tasks with increasing order of priority:
* - Wander: wander around
* - RightBumber: avoid obstacles on the right.
* - LeftBumber: avoid obstacles on the left.
* Plus it gates access to the effectors
* (i.e. the motors) ensuring
* that it's priority policy is
* strictly enforced.
*/
class Subsumption implements ButtonListener {
public boolean running;
public int owner;
Task tasks[];
public Subsumption() {
running = true;
Button.RUN.addButtonListener(this);
tasks = new Task[3];
tasks[0]=new Wander();
tasks[1]=new RightBumber();
tasks[2]=new LeftBumber();
tasks[0].start();
tasks[1].start();
tasks[2].start();
}
int getPriority(Task t) {
for (int i=0; i<tasks.length; i++) {
if (tasks[i] == t)
return i;
}
return -1;
}
/**
* Arbitrates between the various tasks.
*/
public synchronized void execute(Task requestor) {
int pri = getPriority(requestor);
// If its a lower priorty than
// the current task, ignore it.
if (pri < owner)
return;
// This is the new owner of
// the single output that we have
owner = pri;
// Start new owner from beginning
// (even if it was the same one)
tasks[owner].reset();
}
/**
* Only allow the owner to do stuff,
* just in case some other task calls us
*/
public synchronized void setMotor(Task requestor,
Motor motor, int power, boolean forward) {
if (owner == getPriority(requestor)) {
motor.setPower(power);
if (forward)
motor.forward();
else
motor.backward();
}
}
/**
* Task has finished.
* Re-start next runnable task.
*/
public synchronized void release(Task releaser) {
int pri = getPriority(releaser);
// If it isn't the owner releasing,
// ignore it.
if (owner == pri) {
// Search for the first runnable task.
// There is always one.
for(int i=pri-1; i >= 0; i--) {
if (tasks[i].running()) {
owner = i;
tasks[owner].reset();
}
}
}
}
/**
* Called within the scope of a thread
* defined by Button.addButtonListener().
*/
public void buttonPressed(Button b) {
running=false;
}
public void buttonReleased(Button b) {
}
}
/**
* Functor interface. Or, to put it another
* way, the interface to
* actions stored in a finite state
* machine (fsm).
*/
interface Action {
public int act();
}
/**
* All tasks (wander, leftbumber, rightbumber)
* extend this class. This class
* defines the lifecycle of a task. Namely:
* Reset: Re-initialise task
* Execute: Attempt to run
* (may not succeed if a higher priority
* task is running).
* Run: Execute an FSM.
* Release: Stop running.
*/
abstract class Task extends Thread
implements SensorConstants {
public static final Motor LEFT_MOTOR =
Motor.C;
public static final Motor RIGHT_MOTOR =
Motor.A;
public static final Sensor LEFT_BUMBER =
Sensor.S3;
public static final Sensor RIGHT_BUMBER =
Sensor.S1;
public static final boolean FORWARD =
true;
public static final boolean BACKWARD =
false;
public static final int END = -1;
public static final int START = 0;
public Action actions[];
public int fsm[];
public int state = END;
/**
* Reset the FSM to its initial state.
*/
public void reset() {
state = START;
}
/**
* The thread entry point.
* Either runs the action's FSM to completion -
* sleeping or yielding between each state -
* or until 'running' is false.
* When finished call 'release'.
* <P>
* FSM is really a bit of a misnomer as
* there are no input events so
* there is only one transition
* from each state to the next.
*/
public void run() {
// Keep running until the program should exit.
do {
// Quiesce
while (state != START && Main.main.running) {
yield();
}
// Execute the FSM until it stops...
do {
int toSleepFor;
synchronized (Main.main) {
toSleepFor = actions[state].act();
state = fsm[state];
}
if (toSleepFor > 0) {
try {
sleep(toSleepFor);
} catch (InterruptedException ie) {
}
}
else
yield();
} while (state != END &&
Main.main.running);
// Its over, release the actuators.
release();
} while (Main.main.running);
}
/**
* Inform the coordinator that
* we have released the actuators.
*/
public void release() {
Main.main.release(this);
}
/**
* Request control of the actuators
*/
public void execute() {
if (Main.main != null)
Main.main.execute(this);
}
/**
* Return true if the FSM is
* executing, false otherwise.
*/
public boolean running() {
return state != END;
}
/**
* Convenience function to make
* it appear to subclasses that
* they have direct control of
* the actuators when they are in
* fact gated by the controller.
*/
public void setMotor(Motor motor,
int power, boolean forward) {
Main.main.setMotor(this,
motor, power, forward);
}
}
/**
* Defines a finite state machine
* to avoid an obstacle on the left.
*/
class LeftBumber extends Task
implements SensorListener {
public LeftBumber() {
LEFT_BUMBER.setTypeAndMode
(SENSOR_TYPE_TOUCH, SENSOR_MODE_BOOL);
actions = new Action[3];
actions[0] = new Action() {
public int act() {
setMotor(LEFT_MOTOR, 7, BACKWARD);
setMotor(RIGHT_MOTOR, 7, BACKWARD);
return 200;
}
};
actions[1] = new Action() {
public int act() {
setMotor(LEFT_MOTOR, 7, FORWARD);
return 200;
}
};
// Shouldn't really need to do this one,
// but reset()
// may not be immediate
// on lower priority tasks.
actions[2] = new Action() {
public int act() {
setMotor(RIGHT_MOTOR, 7, FORWARD);
return 0;
}
};
fsm = new int[3];
fsm[0] = 1;
fsm[1] = 2;
fsm[2] = END;
LEFT_BUMBER.activate();
LEFT_BUMBER.addSensorListener(this);
}
/**
* This is actually executed
* in a thread established by
* LEFT_BUMBER.addSensorListener().
*/
public void stateChanged(Sensor bumber,
int oldValue, int newValue) {
Sound.playTone(440, 10);
if (bumber.readBooleanValue()) {
Sound.playTone(500, 10);
execute();
}
}
}
/**
* Defines a finite state machine to
* avoid an obstacle on the right.
*/
class RightBumber extends Task
implements SensorListener {
public RightBumber() {
RIGHT_BUMBER.setTypeAndMode
(SENSOR_TYPE_TOUCH, SENSOR_MODE_BOOL);
actions = new Action[3];
actions[0] = new Action() {
public int act() {
setMotor(LEFT_MOTOR, 7, BACKWARD);
setMotor(RIGHT_MOTOR, 7, BACKWARD);
return 200;
}
};
actions[1] = new Action() {
public int act() {
setMotor(RIGHT_MOTOR, 7, FORWARD);
return 200;
}
};
// Shouldn't really need to do
// this one, but reset()
// may not be immediate on
// lower priority tasks.
actions[2] = new Action() {
public int act() {
setMotor(LEFT_MOTOR, 7, FORWARD);
return 0;
}
};
fsm = new int[3];
fsm[0] = 1;
fsm[1] = 2;
fsm[2] = END;
RIGHT_BUMBER.activate();
RIGHT_BUMBER.addSensorListener(this);
}
/**
* This is actually executed
* in a thread established by
* RIGHT_BUMBER.addSensorListener().
*/
public void stateChanged(Sensor bumber,
int oldValue, int newValue) {
Sound.playTone(1000, 10);
if (bumber.readBooleanValue()) {
Sound.playTone(1400, 10);
execute();
}
}
}
/**
* Defines a finite state machine
* to wander around aimlessley. Note that
* this does not really work very well
* as higher priority behaviours
* could occur in the middle
* of a sleep which means that once the higher
* priority behaviour terminates
* the robot will continue on its last
* trajectory.
*/
class Wander extends Task {
public Wander() {
actions = new Action[3];
actions[0] = new Action() {
public int act() {
setMotor(LEFT_MOTOR, 7, FORWARD);
setMotor(RIGHT_MOTOR, 7, FORWARD);
return 5000;
}
};
actions[1] = new Action() {
public int act() {
setMotor(LEFT_MOTOR, 3, FORWARD);
setMotor(RIGHT_MOTOR, 7, FORWARD);
return 2000;
}
};
actions[2] = new Action() {
public int act() {
setMotor(LEFT_MOTOR, 7, BACKWARD);
setMotor(RIGHT_MOTOR, 7, FORWARD);
return 700;
}
};
fsm = new int[3];
fsm[0] = 1;
fsm[1] = 2;
fsm[2] = 0;
}
}
generated through LaTeX2HTML. M.Inaba 平成18年5月6日