Ya vimos en un post anterior cómo poder obtener la información de un Joystick Arduino sin errores de lectura.
Ahora vamos a seguir creando una aplicación de navegación para utilizar la información obtenida de un Joystick como parámetro de control de dirección.
En este principio vamos a convertir la información obtenida de los ejes X e Y para obtener concretamente un parámetro que es el ángulo de giro creado por la composición de estas dos direcciones.
Para ello, requeriremos del concepto trigonométrico de el arco tangente que forman estos dos parámetros.
IMPORTANTE: Siempre que vayamos a utilizar una función matemática para obtener el arco tangente, para evitar fallos usaremos la función atan2( double y, double x) que es muy distinta a la del arco tangente atan (double value).
Para simplificar la diferencia reside en que disponermos de cuatro cuadrantes en los que la división entre los catetos nos da positivo para el primer y tercer cuadrante y negativo para el segundo y el cuarto, el programa no sabrá cuál se asocia a cuál. Y para saber diferenciar si estamos en un cuadrante o en otro hemos de introducir información de signo en los catetos.
Sabiendo esto ya podemos crear nuestro programa para obtener información de orientación con un Joystick.
Este ejemplo está realizado con una placa Arduino Esplora, pero en esencia, solo hay que sustituir los valores de lectura recopilados por un joystick externo con los de la placa Esplora.
#include <Esplora.h>
int X, Y;
int X_minOffset, X_maxOffset, Y_minOffset, Y_maxOffset;
int X_limit, Y_limit;
void setup() {
Serial.begin(9600);
X_minOffset = -40;
X_maxOffset = 40;
Y_minOffset = -40;
Y_maxOffset = 40;
X_limit = 512;
Y_limit = 512;
}
void loop() {
X = Esplora.readJoystickX();
Y = Esplora.readJoystickY();
int Xvalue = 0;
int Yvalue = 0;
if(X > X_maxOffset || X < X_minOffset ){
Xvalue = map(X,-512,512,-X_limit,X_limit);
}
if(Y > Y_maxOffset || Y < Y_minOffset ){
Yvalue = map(Y,-512,512,-Y_limit,Y_limit);
}
double angle = atan2 (Yvalue, Xvalue);
if ( Xvalue != 0 || Yvalue != 0 ){
Serial.print(" Value X: ");
Serial.print(Xvalue);
Serial.print(" \t Value Y: ");
Serial.print(Yvalue);
Serial.print(" \t Orientation: ");
Serial.println(360/2/M_PI*angle);
}
}
Antes de nada primero vamos a filtrar un poco la señal para evitar valores erroneos cuando está en reposo. Tal y como vimos en el post de calibración del Joystick.
Cómo podemos apreciar, la operación matemática que se aplica es la siguiente:
dir = atan2 (Yvalue, Xvalue);
Y este función nos proporciona el valor del ángulo tangente en radianes.
Para pasarlo a grados, solamente hay que hacer una conversión cambiando la escala multiplicando por 360º y dividiendo por 2π.
360/2/M_PI*dir
Una vez hecha esta parte podremos apreciar dos cosas.
- El valor de 0 grados no está orientado en la orientación norte de nuestro dispositivo. En el caso de la placa Arduino Esplora, el valor positivo de los ejes es hacia la derecha para X y hacia abajo para Y. Por lo que el ángulo de 0 grados como el Norte se encuentra girando el joystick hacia la izquierda.
- Cuando nos desplazamos respecto al cero hacia la izquierda o hacia la derecha observaremos que este valor crece y decrece, pero como estamos haciendo una circunferencia, al llegar a 180º observaremos un salto hacia -180º en negativo.
En función de lo que requiramos será mejor o peor, teniendo en cuenta que siempre va a haber un salto de 360º de diferencia cuando se da una vuelta completa.
Vamos a pensar distintas soluciones para estos supuestos.
Para el primer caso, lo único que hay que hacer es realizar un cambio de eje para las lecturas y un cambio de signo para establecer la dirección positiva de los dos ejes.
#include <Esplora.h>
int X, Y;
int X_minOffset, X_maxOffset, Y_minOffset, Y_maxOffset;
int X_limit, Y_limit;
void setup() {
Serial.begin(9600);
X_minOffset = -40;
X_maxOffset = 40;
Y_minOffset = -40;
Y_maxOffset = 40;
X_limit = 512;
Y_limit = 512;
}
void loop() {
X = -Esplora.readJoystickY();
Y = -Esplora.readJoystickX();
int Xvalue = 0;
int Yvalue = 0;
if(X > X_maxOffset || X < X_minOffset ){
Xvalue = map(X,-512,512,-X_limit,X_limit);
} if(Y > Y_maxOffset || Y < Y_minOffset ){
Yvalue = map(Y,-512,512,-Y_limit,Y_limit);
}
double angle = atan2 (Yvalue, Xvalue);
if ( Xvalue != 0 || Yvalue != 0 ){
Serial.print(" Value X: ");
Serial.print(Xvalue);
Serial.print(" \t Value Y: ");
Serial.print(Yvalue);
Serial.print(" \t Orientation: ");
Serial.println(360/2/M_PI*angle);
}
}
Como se puede observar, para definir los ejes positivos hacia arriba y a la derecha, solo nos ha hecho falta multiplicar por -1 e invertir la lectura de X con la de Y.
Ahora mismo ya tendremos las lecturas de 0 hacia el Norte y a medida que giremos obtendremos el ángulo de giro deseado.
Para finalizar, imaginemos que queremos asociar el Norte, el Sur, el Este y el Oeste en un intervalo definido de ángulos.
Para ello, vamos a empezar conociendo los ángulos obtenidos que van desde 0 a 180 hacia la derecha y de 0 a -180 hacia la izquierda. Esto hace que en el mismo punto de 180 y -180 haya un salto de 360 grados.
Así que vamos a realizar una operación en valor absoluto para que se configure el Norte y el Sur como un eje, cuyos valores aumentan a medida que modifica el giro y se de una manera diferente que el Este y el Oeste cuyos valores crecen y decrecen.
#include <Esplora.h>
int X, Y;
int X_minOffset, X_maxOffset, Y_minOffset, Y_maxOffset;
int X_limit, Y_limit;
int angle_threshold = 30;
void setup() {
Serial.begin(9600);
X_minOffset = -40;
X_maxOffset = 40;
Y_minOffset = -40;
Y_maxOffset = 40;
X_limit = 512;
Y_limit = 512;
}
void loop() {
X = -Esplora.readJoystickY();
Y = -Esplora.readJoystickX();
int Xvalue = 0;
int Yvalue = 0;
int angle;
if(X > X_maxOffset || X < X_minOffset ){
Xvalue = map(X,-512,512,-X_limit,X_limit);
}
if(Y > Y_maxOffset || Y < Y_minOffset ){
Yvalue = map(Y,-512,512,-Y_limit,Y_limit);
}
double dir = atan2 (Yvalue, Xvalue);
angle = 360/2/M_PI*dir;
if ( Xvalue != 0 || Yvalue != 0 ){
Serial.print(" Value X: ");
Serial.print(Xvalue);
Serial.print(" \t Value Y: ");
Serial.print(Yvalue);
Serial.print(" \t Orientation: ");
Serial.print(angle);
Serial.print(" \t Absolute Angle: ");
int abs_angle = abs(angle % 180);
Serial.println(abs_angle);
if( abs_angle < angle_threshold){
Serial.println("North");
}else if ( abs_angle > 180 - angle_threshold){
Serial.println("South");
}else if ( abs_angle > 90 - angle_threshold && abs_angle < 90 + angle_threshold){
if ( angle <0 ){
Serial.println("West");
}else{
Serial.println("East");
}
}
}
}
Como último añadido podremos observar que el ángulo 0 puede confundirse con Norte y Sur; así que lo que habría que hacer sería recoger el ángulo 180 como excepción e incluirlo dentro del condicional. Pero mucho más fácil es si aumentamos el módulo de 180 a 181. Un grado más que va a hacer que la diferencia en el comportamiento sea ínfima y con este único cambio podremos definir exactamente las 4 direcciones en su rango sin cambiar nada más.
#include <Esplora.h>
int X, Y;
int X_minOffset, X_maxOffset, Y_minOffset, Y_maxOffset;
int X_limit, Y_limit;
int angle_threshold = 30;
void setup() {
Serial.begin(9600);
X_minOffset = -40;
X_maxOffset = 40;
Y_minOffset = -40;
Y_maxOffset = 40;
X_limit = 512;
Y_limit = 512;
}
void loop() {
X = -Esplora.readJoystickY();
Y = -Esplora.readJoystickX();
int Xvalue = 0;
int Yvalue = 0;
int angle;
if(X > X_maxOffset || X < X_minOffset ){
Xvalue = map(X,-512,512,-X_limit,X_limit);
}
if(Y > Y_maxOffset || Y < Y_minOffset ){
Yvalue = map(Y,-512,512,-Y_limit,Y_limit);
}
double dir = atan2 (Yvalue, Xvalue);
angle = 360/2/M_PI*dir;
if ( Xvalue != 0 || Yvalue != 0 ){
Serial.print(" Value X: ");
Serial.print(Xvalue);
Serial.print(" \t Value Y: ");
Serial.print(Yvalue);
Serial.print(" \t Orientation: ");
Serial.print(angle);
Serial.print(" \t Absolute Angle: ");
int abs_angle = abs(angle % 181);
Serial.println(abs_angle));
if( abs_angle < angle_threshold){
Serial.println("North");
}else if ( abs_angle > 180 - angle_threshold){
Serial.println("South");
}else if ( abs_angle > 90 - angle_threshold && abs_angle < 90 + angle_threshold){
if (angle <0){
Serial.println("West");
}else{
Serial.println("East");
}
}
}
}
Cuando ya lo tengamos, el siguiente paso será controlar el giro de un robot con este Joystick y hacer funcionar un robot a distancia.
One comment