En el artículo anterior vimos como implementar el sonar básico. En este artículo voy a trabajar un sonar mejorado en RobotC para que tenga una mayor funcionalidad, y se puedan observar mejor los objetos por pantalla. Para ello usaremos nuevas funciones del LCD del NXT, y seguiremos utilizando la trigonometría para mostrar de forma precisa objetos por pantalla. Funciones para el LCD: La nueva función del LCD que vamos a utilizar es nxtDrawLine, que dibuja una línea entre dos puntos píxeles). La sintaxis es: nxtDrawLine(punto1x, punto1y, punto2x, punto2y): Dibuja la recta que une los píxeles cuyas coordenas son las de punto1 y punto2. Usaremos esta función para dibujar líneas negras entre donde detecta el objeto y el borde de la circunferencia del sonar. De esta manera los objetos quedarán representados por píxeles de color negro, mientras que el espacio libre será todo lo que esté en blanco. Dibujo de objetos: En el programa anterior, cada vez que detectábamos un objeto, lo representábamos como un punto, dibujándolo a la misma altura y en la que hubiéramos dibujado el borde de la circunferencia: nxtSetPixel(distancia*cosDegrees(i) + 50, 31*sinDegrees(i) + 32); Sin embargo es una representación muy poco precisa, ya que en realidad el objeto se debería dibujar en una coordenada y distinta, ya que es un punto intermedio de la recta que une el punto central de la circunferencia con su borde: Por tanto habría que modificar la coordenada y, de forma que su valor sea igual a la distancia por el seno, igual que la coordenada x es la distancia por el coseno: nxtSetPixel(distancia*cosDegrees(i) + 50, distancia*sinDegrees(i) + 32); Para dibuja la recta necesitamos dos puntos. El otro punto será por supuesto el borde de la circunferencia de radio 32 píxeles donde estamos representando el sonar. Por tanto para pintar el recta sería: nxtDrawLine(distancia*cosDegrees(i) + 50, distancia*sinDegrees(i) + 32, 31*cosDegrees(i) + 50, 31*sinDegrees(i) + 32); A la hora de detectar un objeto grande, dibujará varias lineas negras, una por cada grado que lo detecte, quedando como resultado un bloque negro representando dicho objeto. Eliminación de errores: Es sabido que el sensor de ultrasonidos falla bastante, generando una gran cantidad de lecturas erróneas con valor 255. Por ello es importante eliminar estos errores de medida para que no aparezcan representados en el dibujo (por ejemplo que este dibujando en el sonar una caja que tenga enfrente y de repente dibuje una línea de color blanco en medio de todas las negras). Una buena idea es controlar cuando el el sensor marque 255 y actuar en consecuencia. Pero, ¿qué se podría hacer?, tratar de dibujar una línea negra cuando el objeto quizás se encuentre a más de 2 metros es igual de erróneo. Por tanto nos debemos fijar más bien en las medidas tomadas recientemente, y basarnos en ellas para saber si dibujar o no una línea, y de que longitud. Para ello tenemos que guardar todas las distancias medidas anteriormente. Una vez tenemos las distancias anteriores guardadas podemos hacer que cada vez que detecte 255 cm, tome en cuenta un pequeño espectro de medidas anteriores (por ejemplo las diez medidas anteriores), calcule una media de esas medidas, y la use como referencia. Suponiendo que tengamos un vector llamado distancias, quedaría de la siguiente manera: if (valor == 255){ aux = 0; for (int j = 1; j <= 10; j++){ aux = aux + distancias[actual-j]; } valor = aux / 10; } Con estos dos conceptos podemos crear un radar bastante mas eficaz, aunque como siempre dependemos de la poca precisión del sensor de ultrasonidos de LEGO. Programa del Sonar: Aplicando los conceptos vistos anteriormente, el programa del sonar ampliado gana precisión en la representación de los objetos que detecta. Todo esto nos facilitará comprender más el funcionamiento del sensor de ultrasonidos. Recordad repasar el artículo sobre ultrasonidos, para tener más claro el uso avanzado de motores. El programa completo es el siguiente: #pragma config(Sensor, S4, sonarSensor, sensorSONAR) task main() { int grado, valor, aux, alcance = 50, espectro = 10; nMotorEncoder[motorB] = 0; int distancias[360]; for(int i = 1; i <= 359; i++){ while(nMotorEncoder[motorB] < i) { motor[motorB] = 30; } motor[motorB] = 0; valor = SensorValue[sonarSensor]; wait1Msec(50); distancias[i] = valor; if ((valor == 255) && (i <= espectro)){ aux = 0; for (int j = 1; j <= espectro; j++){ aux = aux + distancias[i-j]; } valor = aux / espectro; } if (valor <= alcance){ int aux = valor*32/alcance; nxtDrawLine(aux*cosDegrees(i) + 50, aux*sinDegrees(i) + 32, 31*cosDegrees(i) + 50, 31*sinDegrees(i) + 32); } else{ nxtSetPixel(31*cosDegrees(i) + 50, 31*sinDegrees(i) + 32); } } while(nMotorEncoder[motorB] > 0) { motor[motorB] = -30; } motor[motorB] = 0; wait1Msec(5000); } 5 – Creamos bastantes variables que nos ayudarán a manejar nuestro programa, como rango del sonar, el valor actual actual, o espectro para realizar la media en caso de lectura errónea. 7 – Así es como se define una tabla de 360 elementos, en ella guardaremos todas las distancias. 18 a 24 – Aquí controlamos los valores 255. 26 a 33 – Aquí realizamos el dibujo. Y aquí está el video que os prometí:

Por favor visitad el foro si tenéis alguna duda sobre el sonar o sobre RobotC.