Práctica 3 { parte 4: Comportamiento de ir hacia la luz evitando obstáculos}

En esta parte haremos que el robot siga la luz evitando los obstáculos que encuentre por el camino.

Evitaremos los obstáculos mediante el sensor de contacto, es decir, cuando choque con el objeto retrocederá y lo esquivará rodeándolo.

El montaje será el mismo que para la parte anterior, el robot con los dos sensores de luz y el de contacto.

Usaremos los comportamientos andar, luz y EvitarObs, explicados en entradas anteriores.

El "arbitro" para esta parte será el siguiente código:

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

public class LuzObs {

public static void main(String [] args) {

Behavior b1 = new Andar();

Behavior b2 = new Luz();

Behavior b3 = new Evitar();

Behavior [] bArray = {b1, b2,b3};

Arbitrator arby = new Arbitrator(bArray);

arby.start();

}

}

Aquí podemos ver el vídeo:

Práctica 3 { parte 3: Comportamiento de ir hacia la luz}

En esta parte el objetivo será que el robot vaya siguiendo una luz.

Se montará con dos sensores que nos han proporcionado los profesores para esta parte.

Aquí vemos una foto del montaje:

El funcionamiento es sencillo, si el sensor de la derecha capta más luz que el de la izquierda girará a la derecha y al revés.

Para ello seguiremos basándonos en comportamientos y usaremos andar y luz. (estos comportamientos están mostrados en la entrada anterior)

Como los sensores de luz que usamos son del robot anterior, tendremos que usar la clase lejos.nxt.addon.RCXLightSensor.


En el comportamiento luz nos declararemos luz derecha y luz izquierda de RCXLightSensor. También deberemos declararnos viajar de la clase TachoPilot.
Guardaremos los valores de luz del sensor en variables derecha/izquierda y compararemos para ver dónde debemos ir.
Con take control hacemos que entre al comportamiento si izquierda - dcha es mayor que 30 o al revés.
En supress lo que haremos será que se pare, y la acción será que si el de la izquierda tiene más luz de la derecha gira a la izquierda, y si el de la derecha tiene más luz que el de la izquierda, irá a la derecha.



El "árbitro" será SeguirLuz y el código es el siguiente:

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

public class SeguirLuz {

public static void main(String [] args) {

Behavior b1 = new Andar();

Behavior b2 = new Luz();

Behavior [] bArray = {b1, b2};

Arbitrator arby = new Arbitrator(bArray);

arby.start();

}

}

Aquí podemos ver el vídeo:

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

Práctica 3 { parte 1: Comportamiento de evitación de obstáculos usando sensores de contacto}

El objetivo en esta parte será que el robot camine por el terreno, y cuando el sensor de contacto detecte un objeto lo esquive retrocediendo y haciendo un arco adecuado.

El montaje será el robot con las ruedas y un sensor de contacto en el centro como vemos en la foto:

 

En la parte del código, en esta práctica usaremos una nueva forma de programar el robot. Esta forma es mediante comportamientos.

Los comportamientos se heredan de la clase behavior. Hay que implementar tres métodos:

- boolean takeControl(). Devuelve verdadero si se dan las condiciones necesarias para que cierto comportamiento tome el control del robot.

- void action(). El código de este método se ejecuta cuando el comportamiento se activa.

- void suppress(). Este método debe terminar inmediatamente el código que

ejecuta el método action(). Puede terminarlo de inmediato (si es un bucle
infinito, o la ejecución es larga) o esperar a que termine, si la ejecución es
corta.

A lo largo de toda esta práctica vamos a usar los siguientes comportamientos:

>>Andar


import lejos.robotics.subsumption.Behavior;

import lejos.nxt.*;

public class Andar implements Behavior {

public boolean takeControl() {

return true;

}

public void suppress() {

Motor.B.stop();

Motor.C.stop();

}

public void action() {

Motor.B.forward();

Motor.C.forward();

}

}



>>Evitar



import lejos.robotics.navigation.TachoPilot;

import lejos.robotics.subsumption.Behavior;

import lejos.nxt.*;


public class Evitar implements Behavior {

TouchSensor touch = new TouchSensor(SensorPort.S4);

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

public boolean takeControl() {

return touch.isPressed();

}

public void suppress() {

viajar.stop();

}

public void action() {

viajar.stop();

viajar.travel(-5);

viajar.rotate(-90);

viajar.steer(40,120);

viajar.rotate(-45);

}

}


>>Luz


import lejos.robotics.navigation.TachoPilot;

import lejos.robotics.subsumption.Behavior;

import lejos.nxt.*;

import lejos.nxt.addon.RCXLightSensor;


public class Luz implements Behavior {

RCXLightSensor LuzIzda = new RCXLightSensor(SensorPort.S1);

RCXLightSensor LuzDcha = new RCXLightSensor(SensorPort.S2);

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

int Izda,Dcha,i;

public boolean takeControl() {

Izda=LuzIzda.getNormalizedLightValue();

Dcha=LuzDcha.getNormalizedLightValue();

return (((Izda-Dcha)>30)||((Dcha-Izda)>30));

}

public void suppress() {

viajar.stop();

}

public void action() {

Izda=LuzIzda.getNormalizedLightValue();

Dcha=LuzDcha.getNormalizedLightValue();

if ((Izda-Dcha)>30){

viajar.rotate(5);

if ((Dcha-Izda)>30){

viajar.rotate(-5);

}

}

}

Para esta primera parte que consistía en esquivar un obstáculo usaremos andar y evitar, y el árbitro será obstáculos:

Para el comportamiento evitar nos definiremos touch de la clase TouchSensor y viajar de la clase TachoPilot. En take control pondremos si está presionado el sensor para que de control al comportamiento. En supress haremos que el robot se pare, y en action haremos que se pare, retroceda y gire mediante el steer para bordear el obstáculo. Antes y después del steer usamos rotate para que el robot se oriente en su dirección de destino, es decir, que después de esquivar el objeto siga con la dirección que llevaba.



Aquí podemos ver el código:

import lejos.robotics.subsumption.Arbitrator;

import lejos.robotics.subsumption.Behavior;


public class Obstaculos {

public static void main(String [] args) {

Behavior b1 = new Andar();

Behavior b2 = new Evitar();

Behavior [] bArray = {b1, b2};

Arbitrator arby = new Arbitrator(bArray);

arby.start();

}

}

Aquí podemos ver el vídeo del funcionamiento

Práctica 2 { parte 6: Calibración del sensor de ultrasonidos}

En este punto, vamos a desarrollar una aplicación que lea los valores del sensor del ultrasonido y los muestre por el LCD, para ver que distancias son críticas para el correcto funcionamiento del robot.

Construiremos el robot con el sensor de ultrasonidos en su parte superior, paralelo al suelo y mirando hacia delante.

Compararemos las medidas reales con las mostradas en el LCD. Lo haremos en cuatro puntos:

1º Medida: Distancia real máxima y mínima.

Colocaremos el robot con el sensor perpendicular a la pared. Acercaremos y alejaremos el robot de la pared para ver cuales son las distancias máxima y mínima que puede medir.

Los resultados son:

Distancia máxima: 219 cm 

Distancia mínima:  6 cm

2ª Medida: Máximo ángulo respecto a la pared.

Situaremos el robot a 40cm de la pared, e iremos girando el robot con giros <10º hasta obtener desde -90º - 90º Los resultados son:

Rango de ángulos: -51º .. +51º

3ª Medida: ¿Tiene el sensor error sistemático?

Colocaremos el robot perpendicularmente a la pared a distancias de 20,30,40,50,60,70,80,90,100 cm.

Una vez tomadas las medidas, calcularemos la media de la diferencia entre la medida y la distancia real.

Medida Real (cm)                                  Medida LCD (cm)

         20                                                              22

         30                                                              31

         40                                                              41

         50                                                              51

         60                                                              60

         70                                                              70

         80                                                              81

         90                                                              91

         100                                                            101

                             110                                                            111
                          
                             120                                                            122


La media del error entre la medida tomada y la real es 1.2 cm

4ª Medida: Incertidumbre del valor del sensor

Vamos a calcular por separado la incertidumbre en el eje X y en el eje Y.

- Cálculo incertidumbre del eje X:

Para el cálculo del eje X, colocamos el robot a distintas distancias de la pared, y tomaremos 10 medidas por cada distancia. Una vez tomadas todas las medidas calcularemos la media para cada una de las distancias.

Aquí vemos el resultado:

 

Vemos que el error es más alto cuanto más lejos está de la pared.

- Cálculo incertidumbre del eje Y:

Para el cálculo del eje Y colocaremos el robot en el origen de coordenadas, y determinaremos a qué distancia del eje X comienza a detectar el obstáculo.

Aquí vemos el resultado:

Como podemos comprobar, si forma un cono, pero es en las distancias medias, cuando más alejado es capaz de ver, por lo que se asemeja más a un rombo.

Ahora calcularemos la matriz de covarianza empleando el código que utilizamos en la primera práctica ( lo podéis ver en entradas anteriores del blog.

La matriz de covarianza queda:

P = 725.4             -11.1

-11.1               2.7

Práctica 2 { parte 5: Comportamiento sigue-pared para salir de un laberinto}

El algoritmo más simple para salir de un laberinto es seguir una misma pared sin soltarla hasta la salida. Por eso, en esta parte programaremos el robot para que siga una pared a una cierta distancia.

Para ello conectaremos al robot el sensor de ultrasonidos al puerto 1, y orientado 45º a la izquierda para que enfoque a la pared.

Aqui podemos ver una foto del montaje:

El codígo consistirá en mientras no presionemos el botón ESCAPE, que haga dos bucles separados. El primero comprobará si está más cerca de la pared de la distancia que le ponemos como límite. De ser así se parará, rotará hacia el lado contrario de la pared y volverá a medir para ver si puede salir del bucle. El otro bucle será si se aleja de la pared, acercarlo, rotando al lado contrario.
Entre ambos bucles se desplazará hacia delante.

Para desplazarlo, pararlo y girarlo usaremos lo métodos de la clase TachoPilot, stop,travel y rotate.

En resumen, hasta que queramos salir, irá moviendose mientras va tomando medidas, si mide que está cerca de la pared se aleja, y si ve que está lejos se acerca.

Como dato curioso, comenar que para este objetivo es muy importante la orientación del sensor, porque nos ha ocurrido que si estaba en una posición incorrecta (60º o 30º por ejemplo) no seguía correctamente las esquinas ya que tomaba otra medida diferente y giraba sobre sí mismo.

Aquí vemos el vídeo del funcionamiento correcto siguiendo varias esquinas, una puerta, el marco de una puerta...:




Aquí podemos ver el código:

import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;


public class Laberinto {

public static void main(String[] args) throws InterruptedException {

Motor Rizda = Motor.C;

Motor Rdcha = Motor.B;

UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S1);

TachoPilot viajar = new TachoPilot(5.6f,10.85f,Rizda,Rdcha);

sonar.continuous();

viajar.forward();

while (!Button.ESCAPE.isPressed()){

int dist = sonar.getDistance();

while (dist< 25){

viajar.stop();

viajar.rotate(-10);

dist = sonar.getDistance();

}

viajar.travel(5);

while (dist>35){

viajar.stop();

viajar.rotate(10);

dist = sonar.getDistance();

}

viajar.stop();

viajar.travel(5);

}

}

}

Práctica 2 { parte 4: Bump & Go! usando sensores de ultrasonido}

El objetivo de esta parte será que el robot deambule por el entorno y cuando choque con un obstáculo retroceda y cambie de dirección.


El montaje será el utilizado hasta ahora con los dos motores a las ruedas, pero en la parte frontal se le añadirá un sensor de ultrasonidos al puerto 1.

Aquí vemos una foto del montaje:

Para programar este comportamiento lo primero que debemos hacer es definir rueda izquierda y rueda derecha, y crear un objeto del tipo UltrasonicSensor. Las instrucciones serán forward a ambas ruedas para que empieze a moverse hacia delante, y luego en un for la condición de que si el sensor detecta un obstáculo a 20cm vuelva para atrás durante 2 segundos, gire la rueda izquierda un ángulo aleatorio, y vuelva hacia delante. Para calcular si hay un objeto a 20 cm se hace con el if (sonar.getDistance()<=25. El 25 es porque el sensor está a 5 cm por detrás de la rueda. Volver atrás lo haremos con backward, los 2 segundos lo pondremos con Thread.sleep(2000), y el ángulo aleatorio con Rizda.rotate((int) Math.random());
Aquí vemos un vídeo de ejemplo:



Aquí podemos ver el código:

import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;


public class BumpAndGo {

public static void main(String[] args) throws InterruptedException {

Motor Rizda = Motor.B;

Motor Rdcha = Motor.C;

UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S1);

Rizda.forward();

Rdcha.forward();

for(;;){

if (sonar.getDistance()<=25){

Rizda.stop();

Rdcha.stop();

Rizda.backward();

Rdcha.backward();

Thread.sleep(2000);

Rizda.rotate((int) Math.random());

Rizda.forward();

Rdcha.forward();

}

}

}

}

Práctica 2 { parte 3: Bump & Go! usando sensores de contacto}

El objetivo de esta parte será que el robot deambule por el entorno y cuando choque con un obstáculo retroceda y cambie de dirección.


El montaje será el utilizado hasta ahora con los dos motores a las ruedas, pero en la parte frontal se le añadirá un sensor de contacto al puerto 4. A este sensor de contacto, ha habido que ponerle unas piezas que detecten mejor cuando choca porque sino con sólo el sensor, a veces falla.

Aquí vemos una foto del montaje:

Para programar este comportamiento lo primero que debemos hacer es definir rueda izquierda y rueda derecha, y crear un objeto del tipo TouchSensor. Las instrucciones serán forward a ambas ruedas para que empieze a moverse hacia delante, y luego en un for la condición de que si toca algo el sensor (método touch.isPressed()) vuelva para atrás durante 3 segundos, gire la rueda izquierda un ángulo aleatorio, y vuelva hacia delante. Volver atrás lo haremos con backward, los 3 segundos lo pondremos con Thread.sleep(3000), y el ángulo aleatorio con Rizda.rotate((int) Math.random());
Aquí vemos un vídeo de ejemplo:



Aquí podemos ver el código:

import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;


public class BumpAndGo {

public static void main(String[] args) throws InterruptedException {

Motor Rizda = Motor.B;

Motor Rdcha = Motor.C;

TouchSensor touch = new TouchSensor(SensorPort.S4);

Rizda.forward();

Rdcha.forward();

for(;;){

if (touch.isPressed()){

Rizda.stop();

Rdcha.stop();

Rizda.backward();

Rdcha.backward();

Thread.sleep(3000);

Rizda.rotate((int) Math.random());

Rizda.forward();

Rdcha.forward();

}

}

}

}

Práctica 2 { parte 2: Control del robot por sonido}

El objetivo de esta parte será controlar a "Linguo" mediante su sensor de sonido (micrófono).
El comportamiento será sencillo, hacer que el robot se mueva en línea recta, y cuando detecte una palmada se para, y con la siguiente palmada vuelva a andar.

El montaje será parecido al de la primera práctica exceptuando que se añade un sensor de sonido en el puerto 2.

Aquí vemos una foto del montaje:

Para programar este comportamiento lo primero que debemos hacer es calibrar el sensor. Para ello haremos un for de 1 a 100 que vaya tomando valores, y luego haga una media de ellos. Esta media será el ruido en dB. Los valores los tomaremos con el método readValue que haremos sobre la variable sonido de tipo SoundSensor que nos habremos definido antes.
Después de calcular la media haremos que se mueva hacia delante. Para ello aplicaremos el método forward() a la rueda izquierda y rueda derecha definidas previamente.
Posteriormente, y dentro de un for, haremos que la primera palmada, lo pare aplicando el método stop a las ruedas,y con la siguiente palmada lo vuelva a mover.
Para saber si hay una palmada o no, será hacer un if, recogiendo el valor con readValue, y viendo si es un valor superior a la media + el 15%.

Aquí vemos un vídeo de ejemplo:



Aquí podemos ver el código:

import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.SoundSensor;
public class ClapControl {

public static void main(String[] args) throws InterruptedException {

Motor Rizda = Motor.B;

Motor Rdcha = Motor.C;

int i;

int media=0;

SoundSensor sonido = new SoundSensor(SensorPort.S2);

for(i=1;i<=100;i++){

media=media+sonido.readValue();

}

media=(int) ((media/100)*1,15);

System.out.println("Sonido limite: "+media);

Rizda.forward();

Rdcha.forward();

for(;;){

if (sonido.readValue()>media){

Rizda.stop();

Rdcha.stop();

}

if (sonido.readValue()>media){

Rizda.forward();

Rdcha.forward();

}

}

}

}

Práctica 2 { parte 1: Obteniendo información}

En esta segunda práctica nos encargaremos de obtener información útil mediante los sensores del robot.
Comenzaremos en esta primera parte haciendo un programa sencillo que muestre por pantalla cierta información. Esta información será :

- nombre del robot.

- valor en mm del sensor de ultrasonidos.

- valor del sensor de la luz en crudo.

- valor del sensor de luz en modo pasivo (mínimo y máximo).

- tensión de la batería en Milivoltios.

- memoria libre en Bytes.

- El nombre lo meteremos a pelo con un LCD.drawString.

- Para calcular el valor del sensor de ultrasonidos, conectaremos el sensor al robot en el puerto 1.
El valor lo obtendremos mediante la función getDistance. Para ello definimos una variable sonar del tipo UltrasonicSensor, y hacemos en ella una llamada a getDistance.

- Para calcular la luz, conectaremos el sensor de luz del robot al puerto 3. Devolvemos el valor en crudo con el readNormalizedValue, y con el readValue devolvermos el valor del sensor en la escala 0-100% (en esta ocasión el sensor aún no está calibrado). Para ello, lo de siempre, nos creamos una variable luz del tipo LightSensor, y sobre ella aplicaciones las funciones nombradas anteriormente.

- La información de la batería la mostraremos con el método getVoltageMiliVolt de la clase Battery, y por último, la memoria libre utilizaremos el método getRuntime().freeMemory() de la clase Runtime.

Todo esto lo haremos en un bucle for infinito para que se actualicen cada 10segundos ( estos tiempo hasta que se actualice lo haremos con Thread.sleep(10000).

Aquí vemos un vídeo de cómo se muestra la información:



Aquí podemos ver el código:

import lejos.nxt.Battery;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;

public class Informacion {

public static void main(String[] args) throws InterruptedException {

UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S1);

LightSensor luz = new LightSensor(SensorPort.S3);

for(;;){

LCD.clear();

LCD.drawString("Nombre:Linguo",1,2);

LCD.drawString("Sonar:",1,3);

LCD.drawInt(sonar.getDistance(),7,3);

LCD.drawString("Luz:",1,4);

LCD.drawInt(luz.readNormalizedValue(),5,4);

LCD.drawInt(luz.readValue(),10,4);

LCD.drawString("Bateria:", 1,5);

LCD.drawInt(Battery.getVoltageMilliVolt(),9,5);

LCD.drawString("Memoria:",1,6);

LCD.drawInt((int)(Runtime.getRuntime().freeMemory()),9,6);

Thread.sleep(10000);

}

}

}