Agentes Reflexivos - 4 SALIR DE UN LABERINTO

OBJETIVO
Programar el robot para que utilizando el sensor ultrasónico pueda localizar obstáculos y girar para evitarlos. El robot también se debe programar para que utilizando estas habilidades (localizar obstáculos y girar) pueda salir de un laberinto de ángulos rectos.

import lejos.nxt.*;

import lejos.robotics.navigation.*;

import lejos.robotics.subsumption.*;

public class Labyrinth {

public static void main(String [] args){

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

UltrasonicSensor ultrasonic = new UltrasonicSensor(SensorPort.S1);

Behavior getOut = new GetOut(pilot, ultrasonic);

Behavior [] behaviorArray = {getOut};

Arbitrator arbitrator = new Arbitrator(behaviorArray);

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

Button.waitForPress();

arbitrator.start();

}

}

import lejos.nxt.*;

import lejos.robotics.subsumption.*;

import lejos.robotics.navigation.*;

public class GetOut implements Behavior{

private TachoPilot pilot;

private UltrasonicSensor ultrasonic;

private int distance;

private int lastDistance;

private int distanceDifference;

private int veryCloseLeft;

private int slightlyCloseLeft;

private int notCloseLeft;

private int slightlyFarRight;

private int veryFarRight;

private int basePowerLeft;

private int basePowerRight;

private Motor rightMotor;

private Motor leftMotor;

public GetOut(TachoPilot pilot, UltrasonicSensor ultrasonic){

this.pilot = pilot;

this.ultrasonic = ultrasonic;

distance = 0;

lastDistance = 0;

veryCloseLeft = 7;

slightlyCloseLeft = veryCloseLeft + 5;

notCloseLeft = slightlyCloseLeft + 5;

slightlyFarRight = notCloseLeft + 5;

veryFarRight = slightlyFarRight + 5;

basePowerLeft = 100;

basePowerRight = 100;

rightMotor = (Motor)pilot.getRight();

leftMotor = (Motor)pilot.getLeft();

}

public boolean takeControl(){

LCD.clear();

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

return true;

}

public void suppress(){

pilot.stop();

}

public void action(){

pilot.forward();

while(true){

int regSpeed = 270;

int changeSpeed = 333;

int noSpeed = 0;

rightMotor.setSpeed(regSpeed);

leftMotor.setSpeed(regSpeed);

lastDistance = distance;

distance = ultrasonic.getDistance();

distanceDifference = lastDistance - distance;

if(distance <= notCloseLeft){

if(distance <= veryCloseLeft){

rightMotor.setPower(basePowerRight);

rightMotor.backward();

leftMotor.setPower(basePowerRight);

leftMotor.forward();

}

else if(distance <= slightlyCloseLeft){

rightMotor.setSpeed(noSpeed);

rightMotor.setPower(0);

rightMotor.forward();

leftMotor.setSpeed(changeSpeed);

leftMotor.setPower(basePowerLeft);

leftMotor.forward();

}

else if(distance <= notCloseLeft){

leftMotor.setPower(basePowerLeft);

leftMotor.forward();

rightMotor.setPower(0);

rightMotor.flt();

}

}

else{

if(distance >= slightlyFarRight){

if(distanceDifference < 2){

leftMotor.setSpeed(noSpeed);

leftMotor.setPower(0);

leftMotor.forward();

rightMotor.setSpeed(changeSpeed);

rightMotor.setPower(basePowerRight);

rightMotor.forward();

}

}

else if(distance >= veryFarRight){

leftMotor.setPower(basePowerLeft/2);

leftMotor.forward();

rightMotor.setPower(basePowerLeft);

rightMotor.forward();

}

}

try {

Thread.sleep(30);

}

catch(InterruptedException e){

}

}

}

}