I have problem with Behavior/Arbitrator pattern with leJOS 0.9.1.3 version.
My robot is similar to the BumperCar sample (in source code sample list).
It was upgraded with ColorSensor that is used as floodlight. I would like that floodlight is turned on in dark room otherwise is off.
For that reason, I have used ColorSensor (NXT) and implemented additional Behavior (UseLight). Behavior was tested separately and it works just fine, but combined with DriveForward and HitWallShoot behaviors, it stops working as I would expected. It looks like behavior UseLight is taking control (I am counting calls in method-see code) but behaviors action method is not called (maybe in 20% of the time otherwise is skipped). Behavior UseLight has highes priority.
Any Idea where I am doing wrong?
Thanks in advance
My code:
- Code: Select all
public class Labirint {
public DifferentialPilot pilot;
public static void main(String[] args) {
DifferentialPilot pilot = new DifferentialPilot(40f, 180f, Motor.A, Motor.C);
Behavior drive = new DriveForward(pilot);
Behavior wall = new HitWallShoot(SensorPort.S4, pilot);
Behavior [] bArray = {drive, wall, new UseLight(SensorPort.S3)};
//Behavior [] bArray = {new UseLight(SensorPort.S3)};
Arbitrator arby = new Arbitrator(bArray);
LCD.drawString("PRESS TO START", 0, 0);
Button.waitForAnyPress();
LCD.clear();
arby.start();
}
}
- Code: Select all
public class UseLight implements Behavior {
private boolean suppressed = false;
private ColorSensor light;
private static final int MIN_LIGHT_PROCENTAGE = 22;
private static final long DELAY_CONTROL = 3000; // in ms
private long lastLightReadingTime; // to manage delay
private long time;
private int lastLightValue = 0;
public static boolean debug = false;
private int debug_action_indicator=0; //counts actions
private int debug_take_indicator=0; //counts take on
private void readLight(long time) {
lastLightValue = light.getLightValue();
lastLightReadingTime = time;
if (debug) {
LCD.clear(1);
LCD.drawString(
lastLightValue + " l " + light.isFloodlightOn()+" ", 0, 1);
}
}
private void setLight() {
if (lastLightValue < MIN_LIGHT_PROCENTAGE) {
light.setFloodlight(true);
if (debug)
LCD.drawString("SET ON! " + debug_take_indicator+","+debug_action_indicator, 0, 4);
} else {
light.setFloodlight(false);
if (debug)
LCD.drawString("SET OFF! " + debug_take_indicator+","+debug_action_indicator, 0, 4);
}
}
public UseLight(SensorPort port) {
light = new ColorSensor(port, Color.NONE); // start floodlight off
lastLightReadingTime = System.currentTimeMillis(); // start takeControl
// with DELAY
}
public boolean takeControl() {
time = System.currentTimeMillis();
if ((time - lastLightReadingTime) < DELAY_CONTROL)
return false;
if (debug) {
LCD.clear(0);
LCD.drawString("1",1,0);
}
if (debug) {
if (light.isFloodlightOn())
LCD.drawString("is ON? ", 0, 5);
else
LCD.drawString("is OFF?", 0, 5);
}
if (debug) LCD.drawString("2",2,0);
readLight(time);
if (debug) LCD.drawString("3",3,0);
if ((lastLightValue < MIN_LIGHT_PROCENTAGE) && (light.isFloodlightOn())) {
if (debug) LCD.drawString("4",4,0);
return false;
}
if ((lastLightValue >= MIN_LIGHT_PROCENTAGE) && (!light.isFloodlightOn())) {
if (debug) LCD.drawString("5",5,0);
return false;
}
if (debug) {
LCD.drawString("6",6,0);
LCD.drawString("take " + debug_take_indicator+","+debug_action_indicator, 0, 2);
debug_take_indicator++;
}
return true; // change light action
}
public void suppress() {
suppressed = true;
}
public void action() {
suppressed = false;
if (debug) {
LCD.drawString("7",7,0);
LCD.drawString("action " + debug_action_indicator, 0, 3);
}
setLight(); // time calculated in take control
//Delay.msDelay(2000); // do I need delay (it is not helping) ??
if (debug) {
debug_action_indicator++;
LCD.drawString("8",8,0);
}
}
}
In DriveForward class I am using DifferentialPilot class instead of RegulatedMotor class.
- Code: Select all
public class DriveForward implements Behavior {
private boolean suppressed = false;
private DifferentialPilot pilot;
public DriveForward(DifferentialPilot pilot) {
this.pilot = pilot;
}
public boolean takeControl() {
return true;
}
public void suppress() {
suppressed = true;
}
public void action() {
suppressed = false;
pilot.forward();
while( !suppressed ) {
Thread.yield();
if(Button.ESCAPE.isDown()) {
System.exit(1);
}
}
pilot.stop();
}
}
- Code: Select all
public class HitWallShoot implements Behavior {
private UltrasonicSensor sonar;
private boolean suppressed = false;
private DifferentialPilot pilot;
public HitWallShoot(SensorPort sonarPort,DifferentialPilot pilot)
{
this.pilot = pilot;
sonar = new UltrasonicSensor( sonarPort );
}
public boolean takeControl() {
sonar.ping();
return sonar.getDistance() < 80;
}
public void suppress() {
suppressed = true;
}
public void action() {
suppressed = false;
Motor.B.rotate(720);
while( Motor.B.isMoving() && !suppressed ) {
Thread.yield();
}
Delay.msDelay(200);
Motor.B.stop();
pilot.rotate(-90);
while( pilot.isMoving() && !suppressed ) {
Thread.yield();
if(Button.ESCAPE.isDown()) {
System.exit(1);
}
}
pilot.stop();
}
}
