Práctica 3 { parte 2: Comportamiento de evitación de obstáculos usando el sensor de ultrasonidos}

En esta parte el objetivo será que el robot evite un obstáculo sin tocarlo. Para ello se hará uso del sensor de ultrasonidos, montado en la cabeza del robot.

No hemos logrado el objetivo pero ponemos aqui los avances:

Hemos creado un comportamiento EvitarUltra que es el siguiente:

import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.subsumption.Behavior;
import lejos.nxt.*;

public class EvitarUltra implements Behavior {

UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S4);

TachoPilot viajar = new TachoPilot(5.6f,10.85f,Motor.C,Motor.B);

int dist,menor,angulo;

final double dx=70;

final double dy=70;

double ox,oy,rx,ry,df;

public boolean takeControl() {

sonar.continuous();

dist=sonar.getDistance();

return dist< 30;

}

public void suppress() {

viajar.stop();

}

public void action() {

viajar.stop();

Motor.A.rotate(-45);

sonar.continuous();

menor=sonar.getDistance();

while(Motor.A.getTachoCount()< 45){

Motor.A.rotate(5);

dist=sonar.getDistance();

if(dist< menor){

menor=dist;

angulo=Motor.A.getTachoCount();

}

}

Motor.A.rotateTo(0);

oy=Math.sin(Math.toRadians(angulo))*menor;

ox=Math.cos(Math.toRadians(angulo))*menor;

rx=ox+dx;

ry=oy+dy;

angulo=(int) Math.atan2(ry, rx);

df=Math.sqrt(rx*rx+ry*ry);

viajar.rotate((float) Math.toDegrees(angulo));

viajar.travel((float) df);

viajar.rotate((float) Math.toDegrees(-angulo));

}

}

Este comportamiento junto con el andar (comentado en entradas anteriores) será utilizado por el arbitro siguiente:

import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;

public class UltraObs {

public static void main(String [] args) {

Behavior b1 = new Andar();

Behavior b2 = new EvitarUltra();

Behavior [] bArray = {b1, b2};

Arbitrator arby = new Arbitrator(bArray);

arby.start();

}

}

Pese a esto, no hemos conseguido el objetivo