domingo, 10 de julio de 2011

Horizonte Artificial realizado en Processing vía Arduino

La presente entrada en el blog describe el prototipo creado para visualizar y gestionar los llamados "Ángulos de Euler" de aeronáutica/náutica -a fin de ser correctos deberían llamarse ángulos de Cardano. El objetivo, determinar la orientación de un objeto que puede rotar (cabeceo, guiñada y alabeo/balanceo) sobre los ejes intrínsecos.

La siguiente imagen muestra la interficie gráfica creada en Processing para visualizar en tiempo real la información que nos proviene de nuestra plataforma hardware (en este caso vía un acelerómetro de 3 ejes montada sobre una plataforma Arduino)


La primera prueba de horizonte artificial será en este caso sólo tratar y visualizar el ROLL (balanceo/alabeo)


Hardware
- Arduino UNO
- Acelerómetro ADXL335
- LED Tricolor
- Resistencias (1x8.2K, 2x100, 1x180)
- MiniPulsador
- Cables + miniplaca de prototipado


La siguiente imagen muestra el circuito completo de nuestro prototipo. El pulsador se utiliza para calibrar el acelerómetro de 3 ejes, es decir, cuando consideramos una posición estacionaria (sin movimiento) y totalmente horizontal, lo pulsamos y así establecemos el valor llamado zero-g, que sirve para compensar la fuerza gravitatoria según nuestra localización (en Barcelona es 9.803 m/s^2).


El led se enciende sólo durante el periodo de calibración.


En el programa almacenado en Arduino se utiliza el factor scale para fijar al máximo la gravedad a fin que sea considerada 1 gee = 9.8m/s^2.

Software
- Arduino 0021
- Processing 1.5


Se puede apreciar en la consola de la aplicación los grupos de 9 valores que se van recibiendo desde el Arduino, siendo estos:
  • los valores en mV de los ejes X,Y,Z
  • el valor XY respecto la posición de calibraje
  • el factor correctivo de la gravedad versus ADC
  • los milligee (la gravedad en miles)
  • el pitch
  • el roll
A continuación se facilita la primera versión del código fuente para ambos entornos, necesarios para que interactuen entre ellos.

Código fuente de Arduino
 /*  
Proyecto : Horizonte Artificial
Autor....: Ricard Forner (RFKsolutions)
Version..: 0.0.1
Fecha....: 10/07/2011

Código fuente: Arduino

Circuito
Pin 8: Lectura Pulsador Resistencia: 8.2 k
Pin 9: LED RGB (Azul) Resistencia: 100 ohm
Pin 10: LED RGB (Verde) Resistencia: 100 ohm
Pin 11: LED RGB (Rojo) Resistencia: 180 ohm

*/
class Accelerometer {
int p[3]; // pins ejes XYZ analog
int a[3]; // aceleracion (zero-based)
int b[3]; // aceleracion bias/calibracion
int g, t, r; // copia cache de calculos
int scale; // factor escala entre ADC y gravedad
int ledPin; // pin del led de calibracion

public:
Accelerometer(int pinX, int pinY, int pinZ, int pScale, int pLedPin) {
pinMode((p[0] = pinX), INPUT);
pinMode((p[1] = pinY), INPUT);
pinMode((p[2] = pinZ), INPUT);
for (int i = 0; i < 3; i++) {
b[i] = 512;
}
g = t = r = 0;
scale = pScale;
pinMode((ledPin = pLedPin), OUTPUT);
}

void update() {
for (int i = 0; i < 3; i++) {
a[i] = analogRead(p[i]) - b[i];
}
g = t = r = 0;
}

void calibrate() {
digitalWrite(ledPin, HIGH);
for (int i = 0; i < 3; i++) {
b[i] = analogRead(p[i]);
}
b[2] -= scale;
update();
digitalWrite(ledPin, LOW);
}

int milligee() {
if (g != 0) return g;
long squared = 0.0;
for (int i = 0; i < 3; i++) {
squared += (long)a[i] * (long)a[i];
}
g = squared * 1000 / (scale*scale);
return g;
}

int accel(int axis) {
if (axis < 0 || axis > 3) return 0;
return a[axis];
}

int roll() {
if (r != 0) return r;
r = (int)(atan2(a[0], a[2]) * 180. / M_PI);
return r;
}

int pitch() {
if (t != 0) return t;
t = (int)(acos(a[1] / (float)scale) * 180. / M_PI);
return t;
}

void toConsole() {
Serial.print("xV="); Serial.print(b[0]);
Serial.print("\tyV="); Serial.print(b[1]);
Serial.print("\tzV="); Serial.print(b[2]);
Serial.print("\tx="); Serial.print(a[0]);
Serial.print("\ty="); Serial.print(a[1]);
Serial.print("\tz="); Serial.print(a[2]);
Serial.print("\tmg="); Serial.print(milligee());
Serial.print("\tpitch="); Serial.print(pitch());
Serial.print("\troll="); Serial.print(roll());
Serial.println();
}

void toProcessing() {
Serial.print(0);
// XYX mV
Serial.print(";"); Serial.print(b[0]);
Serial.print(";"); Serial.print(b[1]);
Serial.print(";"); Serial.print(b[2]);
// XYX valores
Serial.print(";"); Serial.print(a[0]);
Serial.print(";"); Serial.print(a[1]);
Serial.print(";"); Serial.print(a[2]);
// mGe (1 gee = 9.8ms2)
Serial.print(";"); Serial.print(milligee());
// pitch
Serial.print(";"); Serial.print(pitch());
// roll
Serial.print(";"); Serial.print(roll());
Serial.println();
}

void loop() {
update();
}

};

const int sensorEjeX = 0;
const int sensorEjeY = 1;
const int sensorEjeZ = 2;
const int buttonPin = 8;
const int ledPinRed =11;
const int ledPinGreen =10;
const int ledPinBlue = 9;

int bucle = 0;
Accelerometer accel = Accelerometer(sensorEjeX, sensorEjeY, sensorEjeZ, 64, ledPinRed);

void establishContact() {
Serial.begin(38400);
Serial.println("Arduino conectado.");
}

void initButtons() {
pinMode(buttonPin, INPUT);
digitalWrite(buttonPin, LOW); // pulldown
}

void initLEDs() {
pinMode(ledPinRed, OUTPUT);
pinMode(ledPinGreen, OUTPUT);
pinMode(ledPinBlue, OUTPUT);
}

void setup(){
establishContact();
initButtons();
initLEDs();
}

void loop() {
delay(20);
accel.loop();

// Calibracion bajo peticion
if (HIGH == digitalRead(buttonPin)) { accel.calibrate(); }

// Enviamos solo 1 de cada 8 muestras
if (--bucle <= 0) { bucle = 8; accel.toProcessing(); }
}





Código fuente de Processing
 /**  
Proyecto : Horizonte Artificial
Autor....: Ricard Forner (RFKsolutions)
Version..: 0.0.1
Fecha....: 10/07/2011

Código fuente: Processing
*/

import processing.serial.*;

//
int appWidth = 400;
int appHeight = 480;
int appCenterX = appWidth/2;
int appCenterY = appHeight/2;
int diametro = 200;

// Puerto serie de comunicacion arduino
Serial port;

// valores procedentes de arduino
int sensorXmV;
int sensorYmV;
int sensorZmV;
int sensorX;
int sensorY;
int sensorZ;
int sensormg;
int sensorPitch;
int sensorRoll;

void setup() {
size(appWidth, appHeight);
smooth();
noStroke();
background(0);
PantallaTexto();
AbrirPuertoSerie();
}

void PantallaTexto() {
textMode(SCREEN);
text("Proyecto Horizonte Artificial", 30, 40);
text("by RFKsolutions", 30, 60);
text("v 0.0.1 - 10 julio de 2011", 30, 80);

text("Prueba: Visualización ROLL", appCenterX, appHeight-40);
text("Hardware: Arduino + ADXL335", appCenterX, appHeight-20);
}

void AbrirPuertoSerie() {
// Pintamos la lista de puertos disponibles
println(Serial.list());
// Puerto del arduino
port = new Serial(this, "COM17", 38400);
// no se leera la función serialEvent() hasta que aparezca un salto de linea
port.bufferUntil('\n');
}

void arduinoPuertoDisponible() {
while (port.available() > 0) {
serialEvent(port.read());
}
}

void serialEvent(int Serial) {
String inStr = port.readStringUntil('\n');

if (inStr!=null) {
inStr = trim(inStr);
// Se dividen los valores por el separador ";"
int[] sensores = int(split(inStr,";"));

if (sensores.length>=10) {
sensorXmV = sensores[1];
sensorYmV = sensores[2];
sensorZmV = sensores[3];
sensorX = sensores[4];
sensorY = sensores[5];
sensorZ = sensores[6];
sensormg = sensores[7];
sensorPitch = sensores[8];
sensorRoll = sensores[9];
}
print(sensorXmV); print("; ");
print(sensorYmV); print("; ");
print(sensorZmV); print("; ");
print(sensorX); print("; ");
print(sensorY); print("; ");
print(sensorZ); print("; ");
print(sensormg); print("; ");
print(sensorPitch); print("; ");
println(sensorRoll);
}
}

void draw() {
arduinoPuertoDisponible();

translate(appCenterX, appCenterY);

float lastAng = radians(sensorRoll);
int[] angs = {180, 180};
for (int i=0; i<angs.length; i++) {
// Horizonte: Zona
if (i==0) {
// Tierra
fill(208, 119, 0);
} else {
// Cielo
fill(108, 156, 255);
}
arc(0, 0, diametro, diametro, lastAng, lastAng+radians(angs[i]));
// Horizonte: Lineas
if (i==0) {
// Tierra lineas
fill(198, 198, 198);
for (int j=0; j<3; j++) {
rotate(lastAng);
rect(-((j%2==0)?40:20), +15+(j*30), (j%2==0)?80:40, 2);
rotate(-lastAng);
}
} else {
// Cielo lineas
fill(198, 198, 198);
for (int j=0; j<3; j++) {
rotate(PI+lastAng);
rect(-((j%2==0)?40:20), -15-(j*30), (j%2==0)?80:40, 2);
rotate(PI-lastAng);
}
}
lastAng += radians(angs[i]);
}

// Parrilla
fill(210,10,10);
rect((diametro/4), -2, (diametro/4), 4);
rect(-(diametro/2), -2, (diametro/4), 4);
}



Videos
Como resultado de lo publicado en el presente artículo, un par de videos de su funcionamiento.







1 comentario:

Ricardo vivanco dijo...

heEstimado su código me a sacado de mas de un apuro, pero tengo una duda con un valor de una variable "pScale" que le das un valor por default de 64, mi duda cual es la responsabilidad de ese valor y como afecta al resultado.
saludos!
muy buen blog