Agentes Reflexivos - 1 SEGUIR UNA LÍNEA NEGRA

OBJETIVO
Programar el robot para que utilizando el sensor de luz pueda encontrar y seguir una trayectoria marcada con una línea negra pintada sobre una superficie plana. La trayectoria puede ser recta, con giros en varios ángulos, zig-zag, etc.

import lejos.nxt.*;

import lejos.robotics.navigation.*;

import lejos.robotics.subsumption.*;

public class FollowBlackLine {

public static void main(String[] args){

Pilot pilot = new TachoPilot(5.6f, 11.25f, Motor.B, Motor.C);

LightSensor light = new LightSensor(SensorPort.S3);

pilot.setTurnSpeed(180);

Behavior getBackToLine = new GetBackToLine(pilot, light);

Behavior goForward = new GoForward(pilot, light);

Behavior [] behaviorArray = {getBackToLine, goForward};

Arbitrator arbitrator = new Arbitrator(behaviorArray);

LCD.drawString("Follow Black Line", 2, 2);

Button.waitForPress();

arbitrator.start();

}

}

import lejos.nxt.*;

import lejos.robotics.subsumption.*;

import lejos.robotics.navigation.*;

public class GetBackToLine implements Behavior, SensorPortListener{

private Pilot pilot;

private LightSensor light;

private int sweep;

private boolean suppress;

private boolean outOfBlackLine;

public GetBackToLine(Pilot pilot, LightSensor light){

this.pilot = pilot;

this.light = light;

suppress = false;

outOfBlackLine = false;

SensorPort.S3.addSensorPortListener(this);

}

public void stateChanged(SensorPort port, int oldValue, int newValue){

if(light.readValue() > 35){

outOfBlackLine = true;

}

}

public boolean takeControl(){

if(outOfBlackLine){

LCD.clear();

LCD.drawString("GetBackToLine", 2, 2);

outOfBlackLine = false;

return true;

}

else {

return false;

}

}

public void suppress(){

suppress = true;

}

public void action(){

sweep = 10;

while(!suppress){

pilot.rotate(sweep, true);

while(!suppress && pilot.isMoving()){

Thread.yield();

}

sweep*=-2;

}

pilot.stop();

suppress = false;

}

}

import lejos.nxt.*;

import lejos.robotics.subsumption.*;

import lejos.robotics.navigation.*;

public class GoForward implements Behavior, SensorPortListener{

private Pilot pilot;

private LightSensor light;

private boolean foundBlackLine;

public GoForward(Pilot pilot, LightSensor light){

this.pilot = pilot;

this.light = light;

foundBlackLine = false;

SensorPort.S3.addSensorPortListener(this);

}

public void stateChanged(SensorPort port, int oldValue, int newValue){

if(light.readValue() <= 35){

foundBlackLine = true;

}

}

public boolean takeControl(){

if(foundBlackLine){

LCD.clear();

LCD.drawString("GoForward", 2, 2);

foundBlackLine = false;

return true;

}

else{

return false;

}

}

public void suppress(){

pilot.stop();

}

public void action(){

pilot.forward();

while(light.readValue() <= 35){

Thread.yield();

}

}

}