NXT Sonar 3D en RobotC con coordenadas esféricas

This entry is part 13 of 14 in the series RobotC

Coord_sysEl objetivo de este experimento es lograr una representación de la visión que obtiene el robot mediante un movimiento del sonar en dos ejes. Para ello se ha situado al sensor ultrasónico en una plataforma capaz de moverse con dos grados de libertad y se han realizado lecturas medidas en varias posiciones. NXT Sonar 3D en RobotC con coordenadas esféricas: haremos una representación transformando los datos obtenidos anteriormente a coordenadas esféricas.

El objetivo de este artículo es doble: por un lado realizar una representación más acorde a la realidad, puesto que los datos se están capturando mediante un sistema de coordenadas esféricas y la representación que hemos elegido en gnuplot es mediante representación cartesiana. Aprovechando el hecho de que hemos realizado previamente un artículo sobre tratamiento de ficheros en RobotC y de que debemos hacer una transformación de coordenadas, para trabajar ambos conceptos dividiremos el proceso en dos partes:

  • En una primera parte haremos uso del programa implementado en el artículo anterior, NXT Sonar 3D en RobotC, modificándolo para que escriba la información de las lecturas realizadas en coordenadas esféricas como floats en un fichero.
  • La segunda parte la realiza un segundo programa que se encarga de leer la información del fichero que ha creado el primero, convertir los datos a coordenadas de esféricas a cartesianas, y crear un segundo fichero que será el que gnuplot leerá y representará.

Coordenadas esféricas:

El sistema de coordenadas esféricas se utiliza para determinar la posición espacial de un punto mediante una distancia y dos ángulos. En este caso contamos con esos datos, ya que tenemos los grados que ha girado horizontalmente, la elevación (grados verticales), y la distancia, medida por el sensor de ultrasonidos. Teniendo entonces un punto de la siguiente manera:

Spherical_with_grid

Podríamos hallar las coordenadas cartesianas (x, y, z) del punto mediante las siguientes formulas:

Coordenas Esféricas
Puesto que RobotC dispone de funciones trigonométricas para hallar senos y cosenos es posible implementar esta conversión.

Modificación del primer programa:

Modificaremos el primer programa para que escriba los datos como float, y así practicaremos lectura y escritura de ficheros (revisad el artículo sobre manejo de ficheros en RobotC). La parte del código donde se escribe en el fichero es la siguiente:

WriteFloat(hFileHandle, nIoResult, (float)angle);
WriteFloat(hFileHandle, nIoResult, (float)elevation);
WriteFloat(hFileHandle, nIoResult, (float)distance_in_cm );
//sString = "" + angle + ".0 " + elevation + ".0 " + distance_in_cm + ".0n";
//WriteText(hFileHandle, nIoResult, sString);

La parte que está comentada es lo que teníamos escrito anteriormente. Ahora ya podemos crear nuestro programa que leerá de este archivo y escribirá los datos ya procesados en uno nuevo.

Programa de conversión de coordenadas esféricas a cartesianas:

Este programa lee datos de tipo float de un archivo, calcula las correspondientes coordenadas (x, y, z) , y las escribe en un nuevo archivo de texto que gnuplot sea capaz de interpretar. El código es el siguiente:

const string nombre_fichero1 = "DatosRadar3D.dat";
const string nombre_fichero2 = "NDatosRadar3D.dat";
TFileIOResult error_IO;
TFileHandle fichero1;
TFileHandle fichero2;
int tamano = 40000;

task main() {
float x, y, z;
float distance_in_cm, elevation, angle;

Delete(nombre_fichero2, error_IO);
OpenWrite(fichero2, error_IO, nombre_fichero2, tamano);
OpenRead(fichero1, error_IO, nombre_fichero1, tamano);

for(int i = 1; i <= 1025; i++){
ReadFloat(fichero1, error_IO, angle);
ReadFloat(fichero1, error_IO, elevation);
ReadFloat(fichero1, error_IO, distance_in_cm);
if (distance_in_cm != 255){
x = distance_in_cm*cosDegrees(elevation)*cosDegrees(angle);
y = distance_in_cm*cosDegrees(elevation)*sinDegrees(angle);
z = distance_in_cm*sinDegrees(elevation);
string cadena;
cadena = (int)x + ” ” + (int)y + ” ” +(int)z + “n”;
WriteText (fichero2, error_IO, cadena);
}
}

Close(fichero1, error_IO);
Close(fichero2, error_IO);
}

Ahora una explicación de algunas líneas:

12 - Borramos el archivo, para que al abrirlo cree uno nuevo y vacío.

13 y 14 - Abrimos tanto para lectura el que tiene los datos, como para escritura el que vamos a crear.

16 - Este bucle nos asegura que leeremos el fichero entero, ya que sabiendo que tenemos 40 lecturas horizontales por 25 lecturas verticales.

20 – Aquí realizamos un filtrado que tiene por objeto eliminar las medidas que hayan podido ser erróneas (eliminamos los 255).

21 a 23 - Hacemos la transformación de coordenadas esféricas a cartesianas.

24 a 26 – Y finalmente las escribimos en el nuevo fichero.

Ya solo nos queda representar los datos que hemos calculado con el gnuplot.

Representación con el gnuplot:

Hemos puesto a nuestro radar enfrente de un enorme camión de LEGO, y después de pasarle el radar3D, y usar el nuevo programa para convertir las coordenadas esféricas estamos listos para representarlo. Lo primero que hay que hacer es descargar el archivo del NXT, y emplazarlo en la carpeta binary del gnuplot. Una vez hecho esto ejecutamos el wgnuplot.exe, se abrirá una ventana donde tenemos que copiar estos datos:

set title "Lecturas del sensor ultrasónico"
unset hidden3d
set ticslevel 2.0
set view 38,45
set autoscale
set parametric
set style data lines
set key box
set dgrid3d 30,30,100
set xlabel "www.electricBricks.com"
splot "NDatosRadar3D.dat"
pause -1 "Hit return to continue (1)"
reset

11 – Aquí es donde va el nombre de vuestro archivo.

Si todo sale bien os saldrá una gráfica, donde se verá más o menos representado el objeto que hubiese enfrente del NXT. En nuestro caso sale lo siguiente:

Representacion_Grafica

Se ve el perfil del camión, con su altura aproximada de 30cm y posicionado en diagonal.

Series Navigation<< Manejo de Ficheros en robotCControl de tareas en RobotC >>
facebooktwittergoogle_plusredditpinterestlinkedinmailfacebooktwittergoogle_plusredditpinterestlinkedinmail

facebooktwittergoogle_pluslinkedinrssyoutubefacebooktwittergoogle_pluslinkedinrssyoutube

Comments are closed.