Practica 3

Ante esta nueva prueba de esfuerzo (y pensabamos que el cubo de rubick se nos quedaba grande....) hemos tenido que modificar el diseño del robot.
Consultando con personalidades doctoradas en la estetica del robot (aflelou?Llongueras?) conseguimos lo siguiente:

Incorporamos dos sensores infrarrojos del modelo anterior (RCX) formando 45º con su eje, al igual que un sensor de ultrasonidos montando sobre un motor para poder obtener lecturas en distintos angulos.

Sin escatimar esfuerzo y derrochando pruebas hemos conseguido lo siguiente.

COMPORTAMIENTO DE EVITACION DE OBSTACULOS USANDO SENSORES DE CONTACTO


Este caso consta de los comportamientos IrHaciaDelante y EsquivarObstaculos.

El comportamiento IrHaciaDelante es simplemente moverse en linea recta continuamente.



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

public class IrHaciaDelante implements Behavior{

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

public boolean takeControl() {
return true;
}

public void suppress() {
tp.stop();
}

public void action() {
tp.forward();
}
}




El comportamiento EsquivarObstaculos consiste en que cada vez que alguno de los sensores de contacto sea presionado el metodo action se activa. Como consecuencia Hansf10 retrocederia, rodearia el objeto trazando un arco y pararia en el mismo sentido en el que empezo.



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

public class EsquivarObstaculo implements Behavior{

static TachoPilot tp = new TachoPilot(5.6f,11.25f,Motor.C,Motor.B);
static TouchSensor touch1=new TouchSensor(SensorPort.S1);
static TouchSensor touch2=new TouchSensor(SensorPort.S2);

public boolean takeControl() {
return (touch1.isPressed() || touch2.isPressed());
}

public void suppress() {
tp.stop();
}

public void action() {
tp.travel(-19);
tp.steer(75, 60);
tp.steer(-30, 120);
tp.steer(75, 60);
}
}



El programa que los utiliza es Ejercicio1. El comportamiento IrHaciaDelante siempre se esta ejecutando hasta que un sensor de contacto sea pulsado. Por prioridad pasa a ejecutarse EsquivarObstaculo y una vez esquivado el obstaculo devolvuelve el control a IrHaciaDelante.



import lejos.robotics.subsumption.*;

public class Ejercicio1 {
public static void main(String [] args) {
Behavior b1 = new IrHaciaDelante();
Behavior b2 = new EsquivarObstaculo();
Behavior [] bArray = {b1, b2};
Arbitrator arby = new Arbitrator(bArray);
arby.start();
}
}



COMPORTAMIENTO DE EVITACION DE OBSTACULOS USANDO EL SENSOR DE ULTRASONIDOS


INCOMPLETO. No creemos en la derrota(normalmente) pero esta vez si que hemos admitido no haber superado este obstaculo. El ejercicio planteado, a nuestro juicio, hubiese requerido de más tiempo para completarlo. No obstante, tenemos la sensación de habernos faltado bien poco.

Colgamos las clases implementadas, las cuales creemos están a punto de caramelo de funcionar.
El problema se estructura en 3 clases.
-Clase Vector, para implementar VFF's
-Clase OrientaRobot, para describir el comportamiento.
-Clase Ejercicio2, que inicializa el objeto arbitrator y que gestiona el (buen?) funcionamiento del robot.

CLASE VECTOR

public class Vector{
private float coordX=0;
private float coordY=0;

//CONSTRUCTORES
public Vector(float x, float y, boolean construyeXY){
if(construyeXY==true){
coordX=x;
coordY=y;
}else{
coordX=(float) (x * Math.cos(Math.toRadians(y)));
coordY=(float) (x * Math.sin(Math.toRadians(y)));
}
}

//NON-STATIC
public float getX(){
return coordX;
}
public float getY(){
return coordY;
}

public void setX(float x){
coordX=x;
}
public void setY(float y){
coordY=y;
}
public float Modulo(){
return (float) (Math.sqrt((coordX*coordX) + (coordY*coordY)));
}
public float Angulo(){
//Determinar cuadrantes
float angulos=0;
if(coordX>0 && coordY>0){
//1er. cuadrante
angulos=(float) (Math.toDegrees(Math.atan(coordY/coordX)));
}
if(coordX<0>0){
//2o. cuadrante
angulos=(float) (180 + Math.toDegrees(Math.atan(coordY/coordX)));
}
if(coordX<0 angulos="(float)">0 && coordY<0){ angulos="(float)" coordx="=" coordy="=" oldx="coordX;" oldy="coordY;" coordx="(float)" coordy="(float)" coordx="coordX+v.getX();" coordy="coordY+v.getY();" coordx="=" coordy="=" coordx="coordX" coordy="coordY">

CLASE ORIENTAROBOT

import lejos.nxt.*; import lejos.robotics.navigation.TachoPilot; import lejos.robotics.subsumption.Behavior; import java.util.*; public class OrientaRobot implements Behavior{ public final int MEDIDA_MINIMA=30;//cm. Es la medida con la que se activa el comportamiento public final int MEDIDA_DETECTABLE=100;//cm. Es la medida a partir de la cual empieza a tener en cuenta obstaculos private TachoPilot tp=new TachoPilot(5.6f, 11.2f, Motor.C, Motor.B); private ArrayList distancias=new ArrayList();
private Vector destinoRobot;
private Vector vectorResultante=new Vector(0,0,true);

private float distanciaRecorrida=0;

public OrientaRobot(Vector destino){
destinoRobot=new Vector(destino.getX(),destino.getY(),true);
tp.reset();
tp.setSpeed(90);
Motor.A.resetTachoCount();
Motor.A.setSpeed(90);
}
//volver sobre la idea de "hayObstaculo"....
private void escanear(){
UltrasonicSensor US=new UltrasonicSensor(SensorPort.S1);
int actual;
US.continuous();
Motor.A.rotate(46);
actual=US.getDistance();
for(int i=0;i<4+1;i++){ actual="US.getDistance();"> it=distancias.iterator();
if(dist!=0){
while(it.hasNext()){
Vector actual=it.next();
actual.setX(actual.getX()-dist);
}
destinoRobot.setX(destinoRobot.getX()-dist);
}
if(ang!=0){
it=distancias.iterator();
while(it.hasNext()){
Vector actual=it.next();
actual.RotarVector(ang);
}
destinoRobot.RotarVector(ang);
}
}
private void HallarResultante(){
vectorResultante=new Vector(0,0,true);
Iterator it=distancias.iterator();
while(it.hasNext()){
Vector actual=it.next();

actual.Inverso();
vectorResultante.Resultante(actual);
actual.Inverso();

}
vectorResultante.Resultante(destinoRobot);

LCD.drawString(vectorResultante.getX() + ";" + vectorResultante.getY(),1,4);
LCD.drawString((int)vectorResultante.Modulo() + ";" + vectorResultante.Angulo(),1,5);
LCD.drawString("N.Vect:" + distancias.size(),1,6);

}

public boolean takeControl(){
ActualizarVectores(tp.getTravelDistance()-distanciaRecorrida,0);
distanciaRecorrida+=tp.getTravelDistance()-distanciaRecorrida;

escanear();
HallarResultante();
return (destinoRobot.Modulo()>0);
}

public void suppress(){

}

public void action(){
if(tp.getTravelDistance()!=0){

tp.rotate(vectorResultante.Angulo());
ActualizarVectores(0,vectorResultante.Angulo());
}
tp.travel(vectorResultante.Modulo(),true);
}
}

CLASE EJERCICIO2

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

public class Ejercicio2 {

public static void main(String[] args) {
Behavior b1 = new OrientaRobot(new Vector(100,0,true));
Behavior [] comportamientos = {b1};
Arbitrator ar = new Arbitrator(comportamientos);
ar.start();
}

}


COMPORTAMIENTO IR HACIA LA LUZ

Lo realiza el comportamiento Orientar luz con alma de polilla suicida. Se utilizan los dos sensores de luz del rcx para ello. Antes de iniciarlo hemos creido oportuno que se pudiese calibrar segun la luz disponible para conseguir un funcionamiento correcto en cualquier situacion.El comportamiento es muy sencillo. Si alguno de los dos sensores de luz detecta una cantidad diferente que su homologo significara que la luz esta a uno de los lados. La reaccion a dicho estimulo sera girar hasta que la luz vuelva a ser la misma para ambos.Hemos añadido un margen a los valores leidos para evitar que las minimas variaciones de luz del ambiente pudiesen afectar.La inclusion de @SuppressWarnings("deprecation") esta solo para evitar los warnings resultantes por haber utilizado clases obsoletas para leer de los sensores del RCX.

import lejos.nxt.*;

import lejos.nxt.addon.RCXLightSensor;

import lejos.robotics.navigation.*;

import lejos.robotics.subsumption.Behavior;

public class OrientarALuz implements Behavior{

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

static RCXLightSensor luzIzq=new RCXLightSensor(SensorPort.S3);

static RCXLightSensor luzDer=new RCXLightSensor(SensorPort.S4);

static final int MARGEN=3;

@SuppressWarnings("deprecation")

public OrientarALuz(int minLight,int maxLight){

luzIzq.activate();

luzDer.activate();

luzIzq.setHigh(maxLight);

luzIzq.setLow(minLight);

luzDer.setHigh(maxLight);

luzDer.setLow(minLight);

MostrarLecturas();

}

@SuppressWarnings("deprecation")

private void MostrarLecturas(){

LCD.clear();

LCD.drawInt(luzIzq.readValue(),2,2);

LCD.drawInt(luzDer.readValue(),2,4);

}

@SuppressWarnings("deprecation")

public boolean takeControl() {

return ((luzIzq.readValue()>luzDer.readValue()+MARGEN)||(luzIzq.readValue()+MARGEN

}

public void suppress() {

}

@SuppressWarnings("deprecation")

public void action() {

while(luzIzq.readValue()>luzDer.readValue()+MARGEN){

tp.steer(75, 360, true);

}

tp.stop();

while(luzIzq.readValue()+MARGEN

tp.steer(-75,360, true);

}

tp.stop();

}

}

CLASE EJERCICIO3

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.RCXLightSensor;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;

public class Ejercicio3 {
static RCXLightSensor luzIzq=new RCXLightSensor(SensorPort.S3);
static RCXLightSensor luzDer=new RCXLightSensor(SensorPort.S4);

@SuppressWarnings("deprecation")
public static void main(String [] args) throws Exception {
int min;
int max;
luzIzq.activate();
luzDer.activate();
Thread.sleep(150);
LCD.drawString("MinValue:", 2, 2);
while(!Button.ENTER.isPressed()){}
min=luzIzq.readValue();
Thread.sleep(150);
LCD.drawString("MaxValue:", 2, 2);
while(!Button.ENTER.isPressed()){}
max=luzIzq.readValue();

Behavior b1 = new IrHaciaDelante();
Behavior b2 = new OrientarALuz(min,max);
Behavior [] bArray = {b1, b2};
Arbitrator arby = new Arbitrator(bArray);
arby.start();
}
}

COMPORTAMIENTO DE IR HACIA LA LUZ EVITANDO OBSTACULOS

Por razones evidentes, al no poder completar el segundo ejercicio, el 4º nos pareció imposible :S

Problemas encontrados

En general los mayores problemas los hemos tenido con el comportamiento de esquivar un obstaculo con el ultrasonido.

Conseguimos una posible solucion usando una clase Vector auxiliar para que almacenase vectores y sus metodos asociados.
Los problemas vinieron cuando el robot actuaba como si no detectase el obstaculo.Barajamos varias posibilidades, como que la velocidad de giro del ultrasonido impidiera una lectura correcta,que calculase mal las resultantes, etc. Descubrimos que los metodos estaticos de Vector tenian parte de culpa pero no era suficiente.

Una vez arreglado no tenia en cuenta los obstaculos fuera del alcance de vision de ultrasonidos, asique rediseñamos parte del codigo.