Proyecto balancing joaquín berrocal piris abril 2016 53 pag

PROYECTO BALANCING EN ARDUINO Por Joaquín Berrocal Piris (Abril del 2016) FOTOS – PROGRAMA - E INFORMACIÓN VARIADA EN LA QUE ME BASO Dirección donde compre mi kit v3 y lo monté sobre abril de 2016 http://www.sainsmart.com/robotics/instabots.html Desde esta dirección puedes descargar los programas que te da el que te lo vende. Pero por mucho que probé con mi kit ninguno hacia nada y tuve que finalmente adaptar la Versión 1. Y hacerle bastantes cambios tanto en la activación de los motores como en el setup poniéndole los OFFSET que tuve que averiguar con otro programa llamado: "MPU6050_calibration.ino" Y entonces sí conseguí que funcionara. Además la placa driver para el control de los motores tenía una de sus salidas inoperativa por lo que tuve que utilizar una placa externa con L298. Dejo fotos y video.

TODOS LOS COMPONENTES DEL KIT V3

La placa que va montada sobre la ARDUINO MEGA para el control de los motores (L298P) y para soporte del gisoscopio-acelerometro MPU6050 + del sensor Wireless NRF24L01 a 2,4GHZ trabaja de 1,9 a 3,3 V. 100m alcance

NOTA: COMO EL DRIVER L298 DE LOS MOTORES DE ESTA PLACA QUE A MÍ ME LLEGÓ NO ACTUABA UNA DE SUS SALIDAS, TUVE QUE SUSTITUIRLO POR LA PLACA EXTERNA QUE MÁS ABAJO EXPLICO SU CONEXIÓN Y FUNCIONAMIENTO.

PLACA PARA EL CONTROL DE LOS MOTORES PONGO QUE TUVE QUE UTILIZAR POR ENCONTRARSE MAL LA QUE VENÍA EN EL KIT.

TUTORIAL USO DRIVER DUAL L298 PARA MOTORES DC y paso a paso ESTA ES LA MISMA PLACA QUE COMPRE EN ALIEXPRESSS. http://electronilab.co/tutoriales/tutorial-de-uso-driver-dual-l298n-para-motores-dc-y-paso-a-paso-con-arduino/

Tutorial: Uso de Driver L298N para motores DC y paso a paso con Arduino El siguiente tutorial está basado en el Driver dual para motores (Full-Bridge) – L298N, ofrecido por ELECTRONILAB.CO. Puedes adquirir este módulo en nuestra tienda.

Este módulo basado en el chip L298N te permite controlar dos motores de corriente continua o un motor paso a paso bipolar de hasta 2 amperios.

El módulo cuenta con todos los componentes necesarios para funcionar sin necesidad de elementos adicionales, entre ellos diodos de protección y un regulador LM7805 que suministra 5V a la parte lógica del integrado L298N. Cuenta con jumpers de selección para habilitar cada una de las salidas del módulo (A y B).

La salida A esta conformada por OUT1y OUT2 La salida B por OUT3 y OUT4.

Los pines de habilitación son ENA y ENB respectivamente.deben estar a nivel alto para estar operativo Echemos un vistazo a cómo controlar sólo uno de los motores, Motor1. Con el fin de activar el motor, la línea ENABLE1 debe ser alta. A continuación, controlar el motor y su dirección mediante la aplicación de una señal LOW o HIGH a las líneas Input1 y INPUT2, como se muestra en esta tabla.

Input1 Input2 Acción 0 0 Roturas de motor y paradas * 1 0 El motor gira hacia adelante 0 1 El motor gira hacia atrás 1 1 Roturas de motor y paradas * * Para costa un motor a una parada más lento, aplique una señal de baja a la línea ENABLE1.

En la parte inferior se encuentran los pines de control del módulo, marcados como IN1, IN2, IN3 e IN4.

Conexión de alimentación Este módulo se puede alimentar de 2 maneras gracias al regulador integrado LM7805.

√ Cuando el jumper de selección de 5V se encuentra activo, el módulo permite una alimentación de entre 6V a 12V DC. Como el regulador se encuentra activo, el pin marcado como +5V tendrá un voltaje de 5V DC. Este voltaje se puede usar para alimentar la parte de control del módulo ya sea un microcontrolador o un Arduino, pero recomendamos que el consumo no sea mayor a 500 mA. √ Cuando el jumper de selección de 5V se encuentra inactivo, el módulo permite una alimentación de entre 12V a 35V DC. Como el regulador no está funcionando, tendremos que conectar el pin de +5V a una tensión de 5V para alimentar la parte lógica del L298N. Usualmente esta tensión es la misma de la parte de control, ya sea un microcontrolador o Arduino.

-------------------------------------------------- 0 -----------------------------------------------http://electronilab.co/tienda/driver-dual-para-motores-full-bridge-l298n/

Descripción del Producto Este módulo es el complemento ideal para proyectos de robótica y Router CNC.Permite controlar hasta 2 motores de corriente continua o un motor paso a paso bipolar. También permite controlar un motor paso a paso unipolar configurado como bipolar de forma muy sencilla y eficaz. .

Características Voltaje de alimentación, mínimo de 5 V. Posee dos entradas, una de 5V para controlar la parte lógica y otra para alimentar las salidas al motor, que pueden ser de 5V o más.

La tarjeta tiene la opción de habilitar un regulador LM7805 integrado en ella para alimentar la parte lógica con lo que se puede alimentar la tarjeta con 12V por ejemplo.

Corriente máxima 2 Amperios.

Posee 6 entradas de control (ver tabla de control) Admite entradas de señal PWM para el control de velocidad.

Dimensiones: 43 mm x 23,9 mm x 43 mm.

Salidas: para 2 motores de DC o para un motor bipolar paso a paso.

Partes

Esquema

Mรกs informaciรณn: Hoja de datos (datasheet) Cรณdigo de ejemplo

--------------------------------------------------------------- 0 ----------------------------------------------------------------

Los motores llevan encoders magnéticos tipo HALL pero no los utilizo en esta versión.

Los colores no coinciden con los de mi motor pero las posiciones numéricas sí. Es el mismo motor JGA25-370-12v201rpm http://www.sainsmart.com/sainsmart-instabots-upright-rover-kit-pro-motor.html

MONTAJE DEL MANDO A DISTANCIA

¡¡Nota en esta versión v1 el LCD no es utilizado!!!

MANTENIENDO LA VERTICALIDAD DEL BALANCING

PROGRAMA UTILIZADO PARA EL BALANCING Abril 2016 “balancing_robot.ino” /* +++++balancing_robot.ino Ultimo hecho en Abril 2016++++ ES LA VERSIÓN 1 DE sainsmart.com http://www.sainsmart.com/sainsmart-instabots-upright-rover-kit-pro-updated-2-wheel-self-balancin Diferencias sustanciales entre las distintas versiones: Version 1 : utiliza arduino UNO. EL MANDO no lleva LCD. Y los motores no llevan encoders. Driver L298N Versión 2: Utiliza la UNO. El mando SI lleva LCD los motores no llevan encoders. Driver L298N Versión 3. Utiliza la MEGA. El mando sí lleva LCD y los motores SÍ llevan encoders tipo HALL. (Driver L298P Insertada en la misma placa montada sobre la MEGA)

ESTE ES EL PROGRAMA FINAL PARA MI BALANCÍNG basado en la Versión1 Está modificado por mí pues por ejemplo no tenía los offset en el setup configurado para tener un valor creíble de los ejes.

La versión 1 no lleva lcd ni encoders La V1 esta para la arduino UNO y le tengo adaptada para la arduino MEGA 2560 le he añadido lo siguiente: uso la librería Mirf.h y he de configurar los pines Mirf.cePin = 53; Mirf.csnPin = 48; en la uno no hace falta configurarlo por venir por defecto con los pines 8 y 7 respectivamente Programas auxiliares que necesito para ajustar los offset ""MPU6050_calibration.ino" se obtienen los valores offset o bien este otro. que lo hace automáticamente "Tutorial_MPU6050_equilibrado" */ #include "Wire.h" #include "SPI.h" #include "Mirf.h" #include "nRF24L01.h" #include "MirfHardwareSpiDriver.h" #include "I2Cdev.h" #include "MPU6050.h" MPU6050accelgyro; //NOMBRE que le asigna al acelerometro giroscopio int16_t ax, ay, az; int16_t gx, gy, gz; #define Gry_offset 0 // este valor sale de la cte del fabte para obtener valores creíbles //dividir el raw entre 131 para el giroscopio y entre 16387 para el acelerómetro // el Giróscopo el valor raw/131 ---> 1/131 = 0.00763358 #defineGyr_Gain0.00763358 #define Angle_offset 0 #defineRMotor_offset20 #defineLMotor_offset20 #define pi 3.14159 long data; int x, y;

float kp, ki, kd; floatr_angle, f_angle, omega; float Turn_Speed = 0, Turn_Speed_K = 0; float Run_Speed = 0, Run_Speed_K = 0, Run_Speed_T = 0; floatLOutput,ROutput;

¡¡ continua en la siguiente hoja!!!

unsigned long preTime = 0; float SampleTime = 0.08; unsigned longlastTime; floatInput, Output; floaterrSum, dErr, error, lastErr; inttimeChange; // -----------Pines ADAPDATOS POR MÍ PARA LA MEGA ----int TN1=23; // adapatado por mí a la MEGA int TN2=22; int ENA=5; int TN3=24; int TN4=25; int ENB=4; //-------------------------------------------------------void setup() { Wire.begin(); accelgyro.initialize(); //se le he asignado al #define MPU6050 accelegyro pinMode(TN1,OUTPUT); pinMode(TN2,OUTPUT); pinMode(TN3,OUTPUT); pinMode(TN4,OUTPUT); pinMode(ENA,OUTPUT); pinMode(ENB,OUTPUT); // -------------ADAPDATO POR MÍ PARA LA MEGA Y SACADO DE LA V3----//Arduino pin para libreria MIRF ce --> 53 csn --> 48 para la uno ce --> 8 csn --> 7 Mirf.cePin = 53; Mirf.csnPin = 48; // Mirf.sckPin = 52; //Mirf.mosiPin = 51; // Mirf.misoPin = 50; //----------------------------------------------------------------Mirf.spi = &MirfHardwareSpi; Mirf.init(); Mirf.setRADDR((byte *)"serv1"); Mirf.payload = sizeof(long); Mirf.config(); Serial.begin(115200);

//+++++++++Incluido por MÍ AÑADIR LOS OFFSET DE MI MPU6050 ++++++++++++ /* con ayuda de ""MPU6050_calibration.ino" se obtienen los valores offset //también existe este otro programa que directamente lo que hace es grabarselo al pic sin tener que hacer ya nada de lo que aquí mas abajo hacemos sellama"Tutorial_MPU6050_equilibrado" //a mi me sale en posicion horizontal: Puesto sobre protoboard: withoffsets: Youroffsets:

6 862 AcX

-6 6525 AcY

16387 1506 AcZ

0 0 -112 59 GyX GyY

0 -8 GyZ

Estos otros eran sobre la placa extensiva QUE ES COMO LO HE DEJADO A 26 ABRIL 2016 with offsets: 4 -5 16376 -1 1 0 Your offsets: 880 6653 1473 -98 32 -13 AcX AcY AcZ GyX GyY GyZ una vez conocidos los valores offset con el programa MPU6050_calibration.ino pegarselo a estas instrucciones para partir de unos valores próximos a valor 0 */ // MPU6050 accelgyro;// esta definido al comienzo del programa

Serial.println("InitializingI2Cdevices..."); accelgyro.initialize(); // verify connection Serial.println("Testingdeviceconnections..."); //***** la ? indica una u otra cosa Serial.println(accelgyro.testConnection() "?MPU6050connectionsuccessful" : "MPU6050connectionfail // +++++calibración de los offset del MPU6050 previamente calculados con +++ // +++ ayuda de ""MPU6050_calibration.ino" se obtienen los valores offset +++ // a fecha 16 abril 2016 sobre la placa que venia cuando lo compré accelgyro.setXAccelOffset(880); accelgyro.setYAccelOffset(6653); accelgyro.setZAccelOffset(1473); accelgyro.setXGyroOffset(-98); accelgyro.setYGyroOffset(32); accelgyro.setZGyroOffset(-13); //++++++++++++++++++++++++++++++++++++++++++++++

} void loop() { Serial.print(" Valor A0 : ");Serial.print(analogRead(A0));Serial.print("\t"); Serial.println(" Valor A2 : ");Serial.print(analogRead(A2));Serial.print("\t");

//+++UTILIZO EL MPU 6050 colocado sobre la PLACA EXTENSIVA DE LA MEGA 2560+++ //ES DECIR LA DIRECCIÓN "Y" ES TRASVERSAL AL EJE DE RUEDAS. mientras que en la V1 del proyecto //se encuentra al contrario por ir dispuesta de forma contraria.

Recive();

//Para el mando con los joystick.

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //original es atn2(ay,az).... todo lo demas no c r_angle = (atan2(ay, az) * 180 / pi + Angle_offset); //con el MPU ORIENTADO DE LA COMPRA EJE X LONDIGU /* ----lo dejo como información del programa V1 del que me baso-// cambio ay por ax al tener la MPU a lo largo del eje ruedas X perpendicular //r_angle = (-1*(atan2(ax, az) * 180 / pi + Angle_offset)); ////Serial.println(r_angle); //Serial.print(" r_angle "); Serial.print(r_angle); //"omega" es el resultado de multiplicar el valor del giroscopio eje X gx valor bruto por la constante //dada por el fabte para obtener un valor fino o depurado 1/131.0 que es = a 0.00763358 //y ese es el valor del #define Gyr_Gain 0.00763358 osea omega = (1/131.0)*(gx +Gry_offset); //nota en "Gry_offset" no es necesario ya poner nada pues está equilibrado a cero en la posición horizo //pero si no hubiese utilizado las instrucciones puestas en el SETUP tendría que haber aquí puesto el //offset adecuado que ya probe y en mi caso estaba para el eje X entorno a "65" omega = Gyr_Gain * (gx + Gry_offset); ////con el MPU ORIENTADO DE LA COMPRA EJE X LONDIGUTIDNAL A EJ // de lo contrario sustituir gx por gy poner arriba r_angle = (at //////Serial.print(r_angle);Serial.print("\t");Serial.println(omega);//Serial.print("omega=");Seri */ if (abs(r_angle)<45){ myPID(); PWMControl(); } //si r_angle no es menor de 45 parar el motor PWM los pone a low else{ analogWrite(ENA, 0); //En la MEGA pin 5 PWM = 0 analogWrite(ENB, 0); //En la MEGA pin 4 PWM = 0 } }

// ++ para el control desde el mando+++++

// *******CONTINUA EN LA SIGUIENTE HOJA.***

voidRecive(){ if(!Mirf.isSending()&&Mirf.dataReady()){ Mirf.getData((byte*)&data); Mirf.rxFifoEmpty(); x = data >> 16; y = data & 0x0000ffff; //Estaba en 520, lo pongo en 530 porque mi joystick en posiciรณn neutra da un valor de 522 // y por mayor seguridad le pongo 530 if(x >= 530){ Run_Speed_K = map(x, 520, 1023, 0, 100); Run_Speed_K = Run_Speed_K / 50; Run_Speed = Run_Speed + Run_Speed_K; Serial.print(" Run_Speed x >= 530 valor >>> ");Serial.println(Run_Speed); } else if(x <= 480){ Run_Speed_K = map(x, 480, 0, 0, -100); Run_Speed_K = Run_Speed_K / 50; Run_Speed = Run_Speed + Run_Speed_K; Serial.print(" Run_Speed x <= 480 valor >>> ");Serial.println(Run_Speed); } else{ Run_Speed_K = 0; } if(y >= 530){ Turn_Speed = map(y, 520, 1023, 0, 20); Serial.print("Turn_Speed y >= 530 valor >>> ");Serial.println(Turn_Speed); } else if(y <= 480){ Turn_Speed = map(y,480,0,0,-20); Serial.print("Turn_Speed y <= 480 valor >>> ");Serial.println(Turn_Speed); } else{ Turn_Speed = 0; } } else{ x = y = 500; } }

//FIN DEL void Recive()

voidmyPID(){ //he probado que en lugar de 0.1 poner 0.4 รณ 0.5 va quizas algo mejor. kp = analogRead(A0)*0.1; Serial.print(" kp= ");Serial.print(kp); kd = analogRead(A2)*1.0; Serial.print(" kd= ");Serial.print(kd); //ki= analogRead(A3)*0.001; Serial.print(" ki=");Serial.print(ki); //Serial.print(kp);Serial.print("\t");Serial.print(kd);

/// kp = 0; Serial.print(" kp=");Serial.print(kp); //si cambio por esto por lo de arriba no funciona /// kd = 0; Serial.print(" kd=");Serial.print(kd); ki = 0; Serial.print(" ki=");Serial.println(ki); unsigned long now = millis(); float dt = (now - preTime) / 1000.0; preTime = now; float K = 0.8; float A = K / (K + dt); f_angle = A * (f_angle + omega * dt) + (1 - A) * r_angle; timeChange = (now - lastTime);

Serial.print("

f_angle= ");Serial.println(

if(timeChange>=SampleTi me){ Input = f_angle; error = Input; errSum += error * timeChange; dErr = (error - lastErr) / timeChange; Output = kp * error + ki * errSum + kd * dErr; LOutput = Output + Run_Speed + Turn_Speed; Serial.print(" LOutput= ");Serial.println(LOutput); ROutput = Output + Run_Speed - Turn_Speed; Serial.print(" ROutput= ");Serial.println(ROutput); lastErr = error; lastTime = now; //para ver los valores de f_angle ;Output; LOutput ; ROutput y /S/e/rial.print(f_angleS )e;rial.print("\t");Serial.print(Output);Serial.print("\t"); //Serial.print(LOutput);Serial.print("\t");;Serial.println(ROutput); } } //esta función tuve que modificarla por completo de la original //que no valía en absoluto. voidPWMControl(){ if(LOutput > 0){ digitalWrite (TN3, LOW); //en MEGA pin 24 GIRAR RUEDAS POR A DCHA SENTIDO C digitalWrite (TN4, HIGH); //en MEGA pin 25

HIGH+ LOW

} else if(ROutput < 0){ digitalWrite (TN3, HIGH); //en MEGA pin 24 GIRAR RUEDAS al contrario a IZQDA digitalWrite (TN4, LOW); //en MEGA pin 25

LOW

HIGH } else{ digitalWrite (TN3, HIGH); //en MEGA pin 23 SI ROutput = 0 ""PARAR MOTOR"" digitalWrite (TN4, HIGH); //en MEGA pin 25 } if(ROutput > 0){ digitalWrite (TN1, LOW); //en MEGA pin 23 GIRAR RUEDAS POR A IZQDA SE digitalWrite (TN2, HIGH); //en MEGA pin 22

HIGH MISMO

} else if(LOutput < 0){ digitalWrite (TN1, HIGH); //en MEGA pin 23 GIRAR RUEDAS al contrario a DCHA digitalWrite (TN2, LOW); //en MEGA pin 22

LOW

LOW

HIGH } else{ digitalWrite (TN1, HIGH); //en MEGA pin 23 SI LOutput = 0 ""PARAR MOTOR" digitalWrite (TN2, HIGH); //en MEGA pin 22 } // Y desde aquí se envian los pulsos a los driver para por PWM //en MEGA ENA pin 5 del control driver 1 TN1-2 y ENB pin 4 del control driver TN3-4

analogWrite(ENA,min(255,abs(LOutput)+LMotor_offset)); analogWrite(ENB,min(255,abs(ROutput)+RMotor_offset)); } //FIN DEL PROGRAMA. A CONTINUACIÓN EN HORIZONTAL PARA PODER VER TODO LO ESCRITO. 24

/* +++++balancing_robot.ino Ultimo hecho en Abril 2016++++ ES LA VERSIÓN 1 DE sainsmart.com http://www.sainsmart.com/sainsmart-instabots-upright-rover-kit-pro-updated-2-wheel-self-balancing-robot-kit.html Diferencias sustanciales entre las distintas versiones: Version 1 : utiliza arduino UNO. EL MANDO no lleva LCD. Y los motores no llevan encoders. Driver L298N Versión 2: Utiliza la UNO. El mando SI lleva LCD los motores no llevan encoders. Driver L298N Versión 3. Utiliza la MEGA. El mando sí lleva LCD y los motores SÍ llevan encoders tipo HALL. (Driver L298P Insertada en la misma placa montada sobre la MEGA)

ESTE ES EL PROGRAMA FINAL PARA MI BALANCÍNG basado en la Versión1 Está modificado por mí pues por ejemplo no tenía los offset en el setup configurado para tener un valor creíble de los ejes.

La versión 1 no lleva lcd ni encoders La V1 está para la arduino UNO y le tengo adaptada para la arduino MEGA 2560 le he añadido lo siguiente: uso la libreria Mirf.h y he de configurar los pines Mirf.cePin = 53; Mirf.csnPin = 48; en la uno no hace falta configurarlo por venir por defecto con los pines 8 y 7 respectivamente Programas auxiliares que necesito para ajustar los offset ""MPU6050_calibration.ino" se obtienen los valores offset o bien este otro. que lo hace automaticamente "Tutorial_MPU6050_equilibrado" */

#include "Wire.h" #include "SPI.h"

25

#include "Mirf.h" #include "nRF24L01.h" #include "MirfHardwareSpiDriver.h" #include "I2Cdev.h" #include "MPU6050.h"

MPU6050accelgyro; //NOMBRE que le asigna al acelerometro giroscopio int16_t ax, ay, az; int16_t gx, gy, gz; #define Gry_offset 0 // este valor sale de la cte del fabte para obtener valores creibles //dividir el raw entre 131 para el giroscopio y entre 16387 para el acelerometro // el Giroscopo el valor raw/131 ---> 1/131 = 0.00763358 #defineGyr_Gain0.00763358 #define Angle_offset 0 #defineRMotor_offset20 #defineLMotor_offset20 #define pi 3.14159 long data; int x, y; float kp, ki, kd; floatr_angle, f_angle, omega; float Turn_Speed = 0, Turn_Speed_K = 0; float Run_Speed = 0, Run_Speed_K = 0, Run_Speed_T = 0; floatLOutput,ROutput; unsigned long preTime = 0; float SampleTime = 0.08; unsigned longlastTime; floatInput, Output; floaterrSum, dErr, error, lastErr; inttimeChange; // -----------Pines ADAPDATOS POR MĂ? PARA LA MEGA ----int TN1=23; // adapatado por mi a la MEGA int TN2=22; int ENA=5; int TN3=24; int TN4=25; int ENB=4; //--------------------------------------------------------

26

void setup() { Wire.begin(); accelgyro.initialize(); //se le he asignado al #define MPU6050 accelegyro pinMode(TN1,OUTPUT); pinMode(TN2,OUTPUT); pinMode(TN3,OUTPUT); pinMode(TN4,OUTPUT); pinMode(ENA,OUTPUT); pinMode(ENB,OUTPUT); // -------------ADAPDATO POR MÍ PARA LA MEGA Y SACADO DE LA V3----//Arduino pin para libreria MIRF ce --> 53 csn --> 48 para la uno ce --> 8 csn --> 7 Mirf.cePin = 53; Mirf.csnPin = 48; // Mirf.sckPin = 52; //Mirf.mosiPin = 51; // Mirf.misoPin = 50; //----------------------------------------------------------------Mirf.spi = &MirfHardwareSpi; Mirf.init(); Mirf.setRADDR((byte *)"serv1"); Mirf.payload = sizeof(long); Mirf.config(); Serial.begin(115200);

//+++++++++Incluido por MÍ AÑADIR LOS OFFSET DE MI MPU6050 ++++++++++++ /* con ayuda de ""MPU6050_calibration.ino" se obtienen los valores offset //también existe este otro programa que directamente lo que hace es grabarselo al pic sin tener que hacer ya nada de lo que aquí mas abajo hacemos sellama"Tutorial_MPU6050_equilibrado" //a mi me sale en posicion horizontal: Puesto sobre protoboard: withoffsets: Youroffsets:

6 862 AcX

-6 6525 AcY

16387 1506 AcZ

0 0 -112 59 GyX GyY

0 -8 GyZ

Estos otros eran sobre la placa extensiva QUE ES COMO LO HE DEJADO A 26 ABRIL 2016 with offsets: 4 -5 16376 -1 1 0 Your offsets: 880 6653 1473 -98 32 -13 AcX AcY AcZ GyX GyY GyZ

27

una vez conocidos los valores offset con el programa MPU6050_calibration.ino pegarselo a estas instrucciones para partir de unos valores próximos a valor 0 */ // MPU6050 accelgyro;// esta definido al comienzo del programa Serial.println("InitializingI2Cdevices..."); accelgyro.initialize(); // verify connection Serial.println("Testingdeviceconnections..."); //***** la ? indica una u otra cosa Serial.println(accelgyro.testConnection() "?MPU6050connectionsuccessful" : "MPU6050connectionfailed"); // +++++calibración de los offset del MPU6050 previamente calculados con +++ // +++ ayuda de ""MPU6050_calibration.ino" se obtienen los valores offset +++ // a fecha 16 abril 2016 sobre la placa que venia cuando lo compré accelgyro.setXAccelOffset(880); accelgyro.setYAccelOffset(6653); accelgyro.setZAccelOffset(1473); accelgyro.setXGyroOffset(-98); accelgyro.setYGyroOffset(32); accelgyro.setZGyroOffset(-13); //++++++++++++++++++++++++++++++++++++++++++++++

} void loop() { Serial.print(" Valor A0 : ");Serial.print(analogRead(A0));Serial.print("\t"); Serial.println(" Valor A2 : ");Serial.print(analogRead(A2));Serial.print("\t");

//+++UTILIZO EL MPU 6050 colocado sobre la PLACA EXTENSIVA DE LA MEGA 2560+++ //ES DECIR LA DIRECCIÓN "Y" ES TRASVERSAL AL EJE DE RUEDAS. mientras que en la V1 del proyecto //se encuentra al contrario por ir dispuesta de forma contraria. Recive(); //Para el mando con los joystick. accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //original es atn2(ay,az).... todo lo demas no cambia en absoluto r_angle = (atan2(ay, az) * 180 / pi + Angle_offset); //con el MPU ORIENTADO DE LA COMPRA EJE X LONDIGUTIDNAL A EJE MOTORES

28

/* ----lo dejo como información del programa V1 del que me baso-// cambio ay por ax al tener la MPU a lo largo del eje ruedas X perpendicular //r_angle = (-1*(atan2(ax, az) * 180 / pi + Angle_offset)); ////Serial.println(r_angle); //Serial.print(" r_angle "); Serial.print(r_angle); //"omega" es el resultado de multiplicar el valor del giroscopio eje X gx valor bruto por la constante //dada por el fabte para obtener un valor fino o depurado 1/131.0 que es = a 0.00763358 //y ese es el valor del #define Gyr_Gain 0.00763358 osea omega = (1/131.0)*(gx +Gry_offset); //nota en "Gry_offset" no es necesario ya poner nada pues está equilibrado a cero en la posición horizontal //pero si no hubiese utilizado las instrucciones puestas en el SETUP tendría que haber aquí puesto el //offset adecuado que ya probe y en mi caso estaba para el eje X entorno a "65" omega = Gyr_Gain * (gx + Gry_offset); ////con el MPU ORIENTADO DE LA COMPRA EJE X LONDIGUTIDNAL A EJE MOTORES // de lo contrario sustituir gx por gy poner arriba r_angle = (atan2(ay, az) * 180 ..... //////Serial.print(r_angle);Serial.print("\t");Serial.println(omega);//Serial.print("omega=");Serial.println(omega); */ if (abs(r_angle)<45){ myPID(); PWMControl(); } //si r_angle no es menor de 45 parar el motor PWM los pone a low else{

analogWrite(ENA, 0); //En la MEGA pin 5 PWM = 0 analogWrite(ENB, 0); //En la MEGA pin 4 PWM = 0 } } // ++ para el control desde el mando+++++ voidRecive(){ if(!Mirf.isSending()&&Mirf.dataReady()){ Mirf.getData((byte*)&data); Mirf.rxFifoEmpty(); x = data >> 16; y = data & 0x0000ffff; //Estaba en 520, lo pongo en 530 porque mi joystick en posición neutra da un valor de 522 // y por mayor seguridad le pongo 530 if(x >= 530){ Run_Speed_K = map(x, 520, 1023, 0, 100); Run_Speed_K =

29

Run_Speed_K / 50; Run_Speed = Run_Speed + Run_Speed_K; Serial.print(" Run_Speed x >= 530 valor >>> ");Serial.println(Run_Speed); } else if(x <= 480){ Run_Speed_K = map(x, 480, 0, 0, -100); Run_Speed_K = Run_Speed_K / 50; Run_Speed = Run_Speed + Run_Speed_K; Serial.print(" Run_Speed x <= 480 valor >>> ");Serial.println(Run_Speed); } else{ Run_Speed_K = 0; } if(y >= 530){ Turn_Speed = map(y, 520, 1023, 0, 20); Serial.print("Turn_Speed y >= 530 valor >>> ");Serial.println(Turn_Speed); } else if(y <= 480){ Turn_Speed = map(y,480,0,0,-20); Serial.print("Turn_Speed y <= 480 valor >>> ");Serial.println(Turn_Speed); } else{ Turn_Speed = 0; } } else{ x = y = 500;

} }

//FIN DEL void Recive()

voidmyPID(){ //he probado que en lugar de 0.1 poner 0.4 รณ 0.5 va quizas algo mejor. kp = analogRead(A0)*0.1; Serial.print(" kp= ");Serial.print(kp); kd = analogRead(A2)*1.0; Serial.print(" kd= ");Serial.print(kd); //ki= analogRead(A3)*0.001; Serial.print(" ki=");Serial.print(ki); //Serial.print(kp);Serial.print("\t");Serial.print(kd);

/// kp = 0; Serial.print(" kp=");Serial.print(kp); //si cambio por esto por lo de arriba no funciona. /// kd = 0; Serial.print(" kd=");Serial.print(kd); ki = 0;

30

Serial.print(" ki=");Serial.println(ki); unsigned long now = millis(); float dt = (now - preTime) / 1000.0; preTime = now; float K = 0.8; float A = K / (K + dt); f_angle = A * (f_angle + omega * dt) + (1 - A) * r_angle;

Serial.print("

f_angle= ");Serial.println(f_angle);

timeChange = (now - lastTime); if(timeChange>=SampleTime){ Input = f_angle; error = Input; errSum += error * timeChange; dErr = (error - lastErr) / timeChange; Output = kp * error + ki * errSum + kd * dErr; LOutput = Output + Run_Speed + Turn_Speed; Serial.print(" LOutput= ");Serial.println(LOutput); ROutput = Output + Run_Speed - Turn_Speed; Serial.print(" ROutput= ");Serial.println(ROutput); lastErr = error; lastTime = now; //para ver los valores de f_angle ;Output; LOutput ; ROutput y /S/e/rial.print(f_angleS )e;rial.print("\t");Serial.print(Output);Serial.print("\t"); //Serial.print(LOutput);Serial.print("\t");;Serial.println(ROutput); } } //esta función tuve que modificarla por completo de la original //que no valía en absoluto. voidPWMControl(){

if(LOutput > 0){ digitalWrite (TN3, LOW); //en MEGA pin 24 GIRAR RUEDAS POR A DCHA digitalWrite (TN4, HIGH); //en MEGA pin 25 } else if(ROutput < 0){ digitalWrite (TN3, HIGH); //en MEGA pin 24 GIRAR RUEDAS al contrario a IZQDA digitalWrite (TN4, LOW); //en MEGA pin 25 } else{

HIGH+ SENTIDO CONTRARIO GIRO LOW

LOW HIGH

31

digitalWrite (TN3, HIGH); digitalWrite (TN4, HIGH);

//en MEGA pin 23 SI ROutput = 0 ""PARAR MOTOR"" //en MEGA pin 25

} if(ROutput > 0){ digitalWrite (TN1, LOW); //en MEGA pin 23 GIRAR RUEDAS POR A IZQDA digitalWrite (TN2, HIGH); //en MEGA pin 22

HIGH MISMO SENTIDO CAIDA LOW

} else if(LOutput < 0){ digitalWrite (TN1, HIGH); //en MEGA pin 23 GIRAR RUEDAS al contrario a DCHA LOW digitalWrite (TN2, LOW); //en MEGA pin 22 HIGH } else{ digitalWrite (TN1, HIGH); //en MEGA pin 23 SI LOutput = 0 ""PARAR MOTOR"" digitalWrite (TN2, HIGH); //en MEGA pin 22 } // Y desde aquí se envian los pulsos a los driver para por PWM //en MEGA ENA pin 5 del control driver 1 TN1-2 y ENB pin 4 del control driver TN3-4 analogWrite(ENA, min(255, abs(LOutput)+LMotor_offset)); analogWrite(ENB, min(255, abs(ROutput)+RMotor_offset)); }

//FIN DEL PROGRAMA EN HORIZONTAL PARA PODER VER TODO LO ESCRITO.

32

INFORMACIÓN SOBRE LOS ELEMENTOS UTILIZADOS Con ejemplos de prueba y uso En primer lugar de la página donde compré el kit. Saintsmart.com pero después de muchas pruebas no había forma que funcionara en absoluto. Ello es debido a que el programa que te puedes bajar de ahí no debe estar bien y tuve que modificarlo bastante. Además en la V1 que es en la que me baso utiliza una placa Arduino UNO. Y en mi proyecto utilizo la Arduino MEGA 2560. +El control está basado en un giroscopio-acelerometro MPU6050 comunicado con el Arduino por bus I2C. Información sobre él.

Módulo Acelerómetro y giroscopio MPU6050

EL módulo Acelerómetro MPU tiene un giroscopio de tres ejes con el que podemos medir velocidad angular y un acelerómetro también de 3 ejes con el que medimos los componentes X, Y y Z de la aceleración.

La dirección de los ejes está indicado en el módulo el cual hay que tener en cuenta para no equivocarnos en el signo de las aceleraciones.

33

La comunicación del módulo es por I2C, esto le permite trabajar con la mayoría de microcontroladores.

Los pines SCL y SDA tienen una resistencia pull-up en placa para una conexión directa al microcontrolador o Arduino.

Tenemos dos direcciones I2C para poder trabajar: Pin AD0 AD0=HIGH (5V) AD0=LOW (GND o NC)

Dirección I2C 0x69 0x68

El pin ADDR internamente en el módulo tiene una resistencia a GND, por lo que si no se conecta, la dirección por defecto será 0x68.

El módulo tiene un regulador de voltaje en placa de 3.3V, el cual se puede alimentar con los 5V del Arduino.

Librería Para el PMU6050 En este tutorial trabajaremos con la librería desarrollada por Jeff Rowberg, la librería se descargar en: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 Esta librería trabaja con una librería adicional para la comunicación I2C: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdev Para trabajar los siguientes ejercicios es necesario instalar las librerías en el IDE Arduino.

Conexiones entre Arduino y el MPU6050 Las conexiones son del modo I2C estándar: MPU6050 Arduino Uno, Nano, Mini Arduino Mega, DUE Arduino Leonardo VCC 5V 5V 5V GND GND GND GND SCL A5 3 21 SDA A4 2 20

34

Ej.1: Realizar mediciones del MPU6050: En este ejemplo realizaremos lecturas tanto del acelerรณmetro como del giroscopio.

El sketch para este ejercicio: // Librerias I2C para controlar el mpu6050 // la libreria MPU6050.h necesita I2Cdev.h, I2Cdev.h necesita Wire.h #include "I2Cdev.h" #include "MPU6050.h" #include "Wire.h" // La direcciรณn del MPU6050 puede ser 0x68 o 0x69, dependiendo // del estado de AD0. Si no se especifica, 0x68 estarรก implicito MPU6050 sensor; // Valores RAW (sin procesar) del acelerometro y giroscopio en los eje s x,y,z int ax, ay, az; int gx, gy, gz; void setup() { Serial.begin(57600); Wire.begin(); sensor.initialize();

//Iniciando puerto serial //Iniciando I2C //Iniciando el sensor

if (sensor.testConnection()) Serial.println("Sensor iniciado correctamente"); else Serial.println("Error al iniciar el sensor"); } void loop() { // Leer las aceleraciones y velocidades angulares sensor.getAcceleration(&ax, &ay, &az); sensor.getRotation(&gx, &gy, &gz); 35

//Mostrar las lecturas separadas por un [tab] Serial.print("a[x y z] g[x y z]:t"); Serial.print(ax); Serial.print("t"); Serial.print(ay); Serial.print("t"); Serial.print(az); Serial.print("t"); Serial.print(gx); Serial.print("t"); Serial.print(gy); Serial.print("t"); Serial.println(gz); delay(100); } Repasemos las funciones de la librería utilizada en este ejercicio.

Inicialmente como se indico es necesario incluir las siguientes 3 librerías #include "I2Cdev.h" #include "MPU6050.h" #include "Wire.h"

Posteriormente crear la variable u objeto para el MPU6050, que en nuestro caso tiene el nombre de: “sensor” MPU6050 sensor;

En este caso la dirección I2c es 0x68, pero si deseamos trabajar con otra dirección debemos modificar la linea anterior: MPU6050 sensor(0x69);

Posteriormente en el void loop() inicializamos tanto la comunicación I2C como el MPU6050 y en nuestro caso la comunicación serial puesto que la usaremos más adelante: Serial.begin(57600); Wire.begin(); sensor.initialize();

//Iniciando puerto serial //Iniciando I2C //Iniciando el sensor

Al inicializar el sensor, los rangos por defecto serán: - acelerómetro -2g a +2g - giroscopio: -250°/sec a +250°/sec Teniendo en cuenta que la resolución de las lecturas es de 16 bits por lo que el rango de lectura es de -32768 a 32767.

En el void loop() realizamos las lecturas y las guardamos en sus variables respectivas, esto se hace con las siguientes funciones: sensor.getAcceleration(&ax, &ay, &az); 36

sensor.getRotation(&gx, &gy, &gz);

La primera función lee la aceleración de los componentes x-y-z como parámetro, es necesario dar la dirección de las variables como argumento, para lo que se usa: &variable.

La segunda función realiza la lectura de la velocidad angular y guarda las lecturas en sus respectivas variables.

Finalmente enviamos por el puerto serial los datos leídos.

Si ubicamos el sensor en posición horizontal obtendremos medidas similares a los que mostramos a continuación.

Conforme movamos el sensor los componentes de aceleración irán cambiando en función del ángulo del sensor, puesto que la gravedad siempre es paralela al eje "Z" y se descompondrá en las componentes x-yz del sensor.

Ej.2: Calibrando nuestro MPU6050 En este ejemplo realizaremos la calibración del MPU6050, esto es necesario ya que el sensor MPU6050 probablemente no se encuentre 100% en una posición horizontal, esto debido a que el sensor al ser soldado en el módulo puede estar desnivelado agregando un error en cada componente. De igual forma cuando instalemos el módulo en nuestro proyecto, puede estar desnivelado a pesar que a simple vista lo notemos correctamente nivelado.

Para solucionar este problema, se puede configurar en el módulo MPU6050 OFFSETS y de esta forma compensar los errores que podamos tener.

El sketch para realizar la calibración es el siguiente: 37

// Librerias I2C para controlar el mpu6050 // la libreria MPU6050.h necesita I2Cdev.h, I2Cdev.h necesita Wire.h #include "I2Cdev.h" #include "MPU6050.h" #include "Wire.h" // La direcciรณn del MPU6050 puede ser 0x68 o 0x69, dependiendo // del estado de AD0. Si no se especifica, 0x68 estarรก implicito #define MPU6050 sensor; // Valores RAW (sin procesar) del acelerometro y giroscopio en los eje s x,y,z int ax, ay, az; int gx, gy, gz; //Variables usadas por el filtro pasa bajos long f_ax,f_ay, f_az; int p_ax, p_ay, p_az; long f_gx,f_gy, f_gz; int p_gx, p_gy, p_gz; int counter=0; //Valor de los offsets int ax_o,ay_o,az_o; int gx_o,gy_o,gz_o; void setup() { Serial.begin(57600); Wire.begin(); sensor.initialize();

//Iniciando puerto serial //Iniciando I2C //Iniciando el sensor

if (sensor.testConnection()) Serial.println("Sensor iniciado correctamente"); // Leer los offset los offsets anteriores ax_o=sensor.getXAccelOffset(); ay_o=sensor.getYAccelOffset(); az_o=sensor.getZAccelOffset(); gx_o=sensor.getXGyroOffset(); gy_o=sensor.getYGyroOffset(); gz_o=sensor.getZGyroOffset(); Serial.println("Offsets:"); Serial.print(ax_o); Serial.print("\t"); Serial.print(ay_o); Serial.print("\t"); Serial.print(az_o); Serial.print("\t"); Serial.print(gx_o); Serial.print("\t"); Serial.print(gy_o); Serial.print("\t"); Serial.print(gz_o); Serial.print("\t"); Serial.println("nnEnvie cualquier caracter para empezar la calibracion"); // Espera un carรกcter para empezar a calibrar while (true){if (Serial.available()) break;} Serial.println("Calibrando, no mover IMU"); 38

} void loop() { // Leer las aceleraciones y velocidades angulares sensor.getAcceleration(&ax, &ay, &az); sensor.getRotation(&gx, &gy, &gz); // Filtrar las lecturas f_ax = f_ax-(f_ax>>5)+ax; p_ax = f_ax>>5; f_ay = f_ay-(f_ay>>5)+ay; p_ay = f_ay>>5; f_az = f_az-(f_az>>5)+az; p_az = f_az>>5; f_gx = f_gx-(f_gx>>3)+gx; p_gx = f_gx>>3; f_gy = f_gy-(f_gy>>3)+gy; p_gy = f_gy>>3; f_gz = f_gz-(f_gz>>3)+gz; p_gz = f_gz>>3; //Cada 100 lecturas corregir el offset if (counter==100){ //Mostrar las lecturas separadas por un [tab] Serial.print("promedio:"); Serial.print("t"); Serial.print(p_ax); Serial.print("t"); Serial.print(p_ay); Serial.print("t"); Serial.print(p_az); Serial.print("t"); Serial.print(p_gx); Serial.print("t"); Serial.print(p_gy); Serial.print("t"); Serial.println(p_gz); //Calibrar el acelerometro a 1g en el eje z (ajustar el offset) if (p_ax>0) ax_o--; else {ax_o++;} if (p_ay>0) ay_o--; else {ay_o++;} if (p_az-16384>0) az_o--; else {az_o++;} sensor.setXAccelOffset(ax_o); sensor.setYAccelOffset(ay_o); sensor.setZAccelOffset(az_o); //Calibrar el giroscopio a 0ยบ/s en todos los ejes (ajustar el offset) if (p_gx>0) gx_o--; 39

else {gx_o++;} if (p_gy>0) gy_o--; else {gy_o++;} if (p_gz>0) gz_o--; else {gz_o++;} sensor.setXGyroOffset(gx_o); sensor.setYGyroOffset(gy_o); sensor.setZGyroOffset(gz_o); counter=0; } counter++; }

El programa básicamente está modificando constantemente los offset intentando eliminar el error con la medida real que deseamos, en esta caso ax=0, ay=0, az=1g y gx= 0, gy=0, gz=0.

Inicialmente leemos los offsets actuales y esperamos que el usuario envía un carácter por el puerto serie.

Antes de enviar el carácter es necesario ubicar el sensor en posición horizontal y evitar moverlo durante la calibración, dicha posición será nuestro nivel para futuras mediciones.

Después de enviar el carácter el programa realiza las lecturas tanto del acelerómetro como del giroscopio, usando un filtro estabilizamos un poco las lecturas y cada 100 lecturas comprobamos si los valores son cercanos a los valores que deseamos leer, dependiendo de esto se aumenta o disminuye los offsets. Esto hará que las lecturas filtradas converjan a: -aceleración: p_ax = 0 , p_ay = 0 , p_az = +16384 -Velocidad angular: p_gx =0 , p_gy = 0 , p_gz = 0 Cuando en el monitor serial se observen valores cercanos a los anteriores debemos desconectar o reiniciar nuestro Arduino. Con esto el MPU6050 quedará configurado con el último offset calculado en el programa de calibración.

A continuación mostramos la salida del monitor serial después de enviar el carácter y esperar que los valores se acerquen a: 0 0 +16384 0 0 0

40

Cuando tengamos estos valores debemos reiniciar el Arduino o simplemente cerrar y abrir el monitor serial. Posteriormente podemos volver a cargar el primer ejemplo para probar las lecturas con los nuevos offsets.

Ej.3: Escalado de lecturas En este ejemplo vamos a escalar las lecturas a valores con las unidades de aceleración y velocidad angular.

Primero tenemos que saber los rangos con los que está configurado nuestro MPU6050, dichos rangos pueden ser 2g/4g/8g/16g para el acelerómetro y 250/500/1000/2000(°/s) para el giroscopio.

Para este ejemplo trabajaremos con los rangos por defecto (2g y 250°/s): Variable valor mínimo valor central valor máximo Lectura MPU6050 -32768 0 +32767 Aceleración -2g 0g +2g Velocidad angular -250°/s 0°/s +250°/s Para escalar simplemente debemos usar una ecuación para convertir el valor leído en un valor de aceleración o velocidad angular.

A continuación se muestra el sketch con la ecuación correspondiente para escalar los valores: // Librerias I2C para controlar el mpu6050 // la libreria MPU6050.h necesita I2Cdev.h, I2Cdev.h necesita Wire.h #include "I2Cdev.h" #include "MPU6050.h" #include "Wire.h" // La dirección del MPU6050 puede ser 0x68 o 0x69, dependiendo // del estado de AD0. Si no se especifica, 0x68 estará implícito 41

MPU6050 sensor; // Valores RAW (sin procesar) del acelerometro y giroscopio en los eje s x,y,z int ax, ay, az; int gx, gy, gz; void setup() { Serial.begin(57600); Wire.begin(); sensor.initialize();

//Iniciando puerto serial //Iniciando I2C //Iniciando el sensor

if (sensor.testConnection()) Serial.println("Sensor iniciado correctamente"); else Serial.println("Error al iniciar el sensor"); } void loop() { // Leer las aceleraciones y velocidades angulares sensor.getAcceleration(&ax, &ay, &az); sensor.getRotation(&gx, &gy, &gz); float ax_m_s2 = ax * (9.81/16384.0); float ay_m_s2 = ay * (9.81/16384.0); float az_m_s2 = az * (9.81/16384.0); float gx_deg_s = gx * (250.0/32768.0); float gy_deg_s = gy * (250.0/32768.0); float gz_deg_s = gz * (250.0/32768.0); //Mostrar las lecturas separadas por un [tab] Serial.print("a[x y z](m/s2) g[x y z](deg/s):t"); Serial.print(ax_m_s2); Serial.print("\t"); Serial.print(ay_m_s2); Serial.print("\t"); Serial.print(az_m_s2); Serial.print("\t"); Serial.print(gx_deg_s); Serial.print("\t"); Serial.print(gy_deg_s); Serial.print("\t"); Serial.println(gz_deg_s); delay(100); } Los valores que tenemos ahora ya están escalados a unidades de aceleración y velocidad angular. En nuestro caso hemos convertido la aceleración a valores en m/s^2 por lo que se reemplazó el valor de g = 9.81, si se desea trabajar en unidades "g" no es necesario este último paso.

Si el sensor está en posición horizontal se debe obtener mediciones cercanas a 9.8 m/s^2 (aceleración de la gravedad terrestre) en la componente z de la aceleración.

42

Ej.4: Calculando el ángulo de inclinación con el acelerómetro del MPU6050 Si tenemos en cuenta que la única fuerza que actúa sobre el sensor es la fuerza de la gravedad. Entonces los valores que obtenemos en las componentes del acelerómetro corresponden a la gravedad y los ángulos de la resultante serán la inclinación del plano del sensor, puesto que la gravedad siempre es vertical.

Para entenderlo mejor, asumiremos que estamos en un plano X-Z e inclinamos el MPU6050 un ángulo θ, dicho ángulo se calcula de la siguiente forma:

Lo anterior nos sirve para calcular el ángulo en un plano 2D, pero para calcular los ángulos de inclinación en un espacio 3D tanto en X como en Y usamos las siguientes formulas:

43

Tener en cuenta que estamos calculando el ángulo de inclinación, si deseáramos el ángulo de rotación es decir por ejemplo el ángulo que rota el eje x en su mismo eje, entonces en las formulas necesitamos cambiar el ay por el ax y viceversa.

El sketch para calcular los ángulos de inclinación es el siguiente: // Librerias I2C para controlar el mpu6050 // la libreria MPU6050.h necesita I2Cdev.h, I2Cdev.h necesita Wire.h #include "I2Cdev.h" #include "MPU6050.h" #include "Wire.h" // La dirección del MPU6050 puede ser 0x68 o 0x69, dependiendo // del estado de AD0. Si no se especifica, 0x68 estará implícito MPU6050 sensor; // Valores RAW (sin procesar) del acelerometro

en los ejes x,y,z

int ax, ay, az; void setup() { Serial.begin(57600); Wire.begin(); sensor.initialize();

//Iniciando puerto serial //Iniciando I2C //Iniciando el sensor

if (sensor.testConnection()) Serial.println("Sensor iniciado correctamente"); else Serial.println("Error al iniciar el sensor"); } void loop() { // Leer las aceleraciones sensor.getAcceleration(&ax, &ay, &az); //Calcular los angulos de inclinacion: float accel_ang_x=atan(ax/sqrt(pow(ay,2) + pow(az,2)))*(180.0/3.14); float accel_ang_y=atan(ay/sqrt(pow(ax,2) + pow(az,2)))*(180.0/3.14); //Mostrar los angulos separadas por un [tab] Serial.print("Inclinacion en X: "); Serial.print(accel_ang_x); Serial.print("tInclinacion en Y:"); Serial.println(accel_ang_y); delay(10); } 44

A continuación mostramos los resultados al tener inclinado el MPU6050 aproximadamente 45° con respecto a X:

Esto funciona solo si la única aceleración presente es la gravedad, pero si movemos rápidamente el MPU y sin realizar ninguna inclinación el ángulo que obtenemos con el programa anterior varía, generándonos errores para estos casos.

Ej.5: Calculando el ángulo de rotación usando el giroscopio del MPU5060 Como se explicó al inicio el giroscopio nos entrega la velocidad angular, y para calcular el ángulo actual necesitamos integrar la velocidad y conocer el ángulo incial. Esto lo hacemos usando la siguiente formula:

Tener en cuenta que cuando nos referimos a θx nos referimos al ángulo que gira el eje X sobre su propio eje. En la siguiente imagen se observa que la velocidad angular es perpendicular al plano de rotación.

Para calcular los ángulos de rotación tanto en el eje X como en Y usamos el siguiente sketch: 45

// Librerias I2C para controlar el mpu6050 // la libreria MPU6050.h necesita I2Cdev.h, I2Cdev.h necesita Wire.h #include "I2Cdev.h" #include "MPU6050.h" #include "Wire.h" // La direcciĂłn del MPU6050 puede ser 0x68 o 0x69, dependiendo // del estado de AD0. Si no se especifica, 0x68 estarĂĄ implĂ­cito MPU6050 sensor; // Valores RAW (sin procesar) del acelerometro y giroscopio en los eje s x,y,z int gx, gy, gz; long tiempo_prev, dt; float girosc_ang_x, girosc_ang_y; float girosc_ang_x_prev, girosc_ang_y_prev; void setup() { Serial.begin(57600); Wire.begin(); sensor.initialize();

//Iniciando puerto serial //Iniciando I2C //Iniciando el sensor

if (sensor.testConnection()) Serial.println("Sensor iniciado correctamente"); else Serial.println("Error al iniciar el sensor"); tiempo_prev=millis(); }

void loop() { // Leer las velocidades angulares sensor.getRotation(&gx, &gy, &gz); //Calcular los angulos rotacion: dt = millis()-tiempo_prev; tiempo_prev=millis(); girosc_ang_x = (gx/131)*dt/1000.0 + girosc_ang_x_prev; girosc_ang_y = (gy/131)*dt/1000.0 + girosc_ang_y_prev; girosc_ang_x_prev=girosc_ang_x; girosc_ang_y_prev=girosc_ang_y; //Mostrar los angulos separadas por un [tab] Serial.print("Rotacion en X: "); Serial.print(girosc_ang_x); Serial.print("tRotacion en Y: "); Serial.println(girosc_ang_y); 46

delay(100); }

Con este programa al girar aumentará o disminuirá el ángulo en función del sentido de giro del MPU

Tomar nota que la medida no es exacta incluso cuando no se mueve, el ángulo varía, o si se gira cierto ángulo y luego se regresa a la posición original el ángulo que medimos no es el inicial, esto se debe a que al integrar la velocidad angular y sumar el ángulo inicial hay un error producto de la mala medición del tiempo o del ruido en la lectura del MPU, el error por más pequeño que sea, se va acumulando en cada iteración y creciendo, este error es conocido como DRIFT. Para disminuir el drift existen varios métodos, la mayoria aplica filtros para eliminar el ruido de las lecturas del sensor. También se pueden usar otros sensores como magnetómetros o acelerómetros y con los ángulos calculados con estos mejorar el cálculo del giroscopio.

Uno de los mejores filtros para eliminar el drift es el filtro Kalman, pero se necesita una buena capacidad de procesamiento computacional, haciéndolo difícil implementar en Arduino.

Otro filtro muy usado es el filtro de complemento, que mostramos a continuación:

Ej.6: Implementando un filtro de Complemento: acelerómetro + giroscopio El filtro de complemento o en inglés "Complementary Filter" es uno de los más usados por su fácil implementación, combina el ángulo calculado por el giroscopio y el ángulo calculado por el acelerómetro.

La necesidad de combinar ambas lecturas es que si solo trabajámos con el acelerómetro, este es susceptible a las aceleraciones producto del movimiento del MPU o a fuerzas externas, pero en tiempos 47

largos el ángulo no acumula errores. A diferencia que si trabajamos solo con el giroscopio si bien este no es susceptible a fuerzas externas, con el tiempo el drift es muy grande y nos sirve solo para mediciones de tiempos cortos.

La ecuación para calcular el ángulo usando el filtro de complemento es:

De esta forma el ángulo del acelerómetro está pasando por un filtro pasa bajos, amortiguando las variaciones bruscas de aceleración; y el ángulo calculado por el giroscopio tiene un filtro pasa altos teniendo gran influencia cuando hay rotaciones rápidas. Podemos probar también con otros valores diferentes a 0.98 y 0.02 pero siempre deben de sumar 1. a continuación mostramos el sketch para realizar esta tarea: // Librerias I2C para controlar el mpu6050 // la libreria MPU6050.h necesita I2Cdev.h, I2Cdev.h necesita Wire.h #include "I2Cdev.h" #include "MPU6050.h" #include "Wire.h" // La dirección del MPU6050 puede ser 0x68 o 0x69, dependiendo // del estado de AD0. Si no se especifica, 0x68 estará implícito MPU6050 sensor; // Valores RAW (sin procesar) del acelerometro y giroscopio en los eje s x,y,z int ax, ay, az; int gx, gy, gz; long tiempo_prev; float dt; float ang_x, ang_y; float ang_x_prev, ang_y_prev; void setup() { Serial.begin(57600); Wire.begin(); sensor.initialize();

//Iniciando puerto serial //Iniciando I2C //Iniciando el sensor

if (sensor.testConnection()) Serial.println("Sensor iniciado correctamente"); else Serial.println("Error al iniciar el sensor"); } void loop() { // Leer las aceleraciones y velocidades angulares sensor.getAcceleration(&ax, &ay, &az); sensor.getRotation(&gx, &gy, &gz); 48

dt = (millis()-tiempo_prev)/1000.0; tiempo_prev=millis(); //Calcular los ángulos con acelerómetro float accel_ang_x=atan(ay/sqrt(pow(ax,2) + pow(az,2)))*(180.0/3.14); float accel_ang_y=atan(-ax/sqrt(pow(ay,2) + pow(az,2)))*(180.0/3.14); //Calcular ángulo de rotación con giroscopio y filtro complemento ang_x = 0.98*(ang_x_prev+(gx/131)*dt) + 0.02*accel_ang_x; ang_y = 0.98*(ang_y_prev+(gy/131)*dt) + 0.02*accel_ang_y;

ang_x_prev=ang_x; ang_y_prev=ang_y; //Mostrar los ángulos separadas por un [tab] Serial.print("Rotacion en X: "); Serial.print(ang_x); Serial.print("tRotacion en Y: "); Serial.println(ang_y); delay(10); } Ahora si movemos el MPU6050 rápidamente sin rotar, la variación del ángulo será mínima, además el drift se elimina y solo se nota en tiempos cortos.

------------------------------------------------------------------- 0 ----------------------------------------------------------49

EN ESTA DIRECCIÓN HAY EJEMPLOS CON EL USO DEL Emisor/receptor NRF24L01 Con ejemplos muy fáciles e interesantes: http://www.prometec.net/nrf2401/

Estos módulos usan el bus SPI para acelerar la conexión con el micro controlador por lo que vamos a ver la descripción de los pines necesarios para su conexión, en vista superior:

Hay dos librerías básicas para el manejo de los NRF2401s en Arduino, la NRF24 que es la que vamos a usar aquí (Porque me parece más sencilla) y la librería MIRF.

PIN

NRF2401

ARDUINO UNO

MEGA

GND

1

GND

GND

VCC

2

3.3

3.3

CE

3

9

9

CSN

4

10

53

SCK

5

13

52

MOSI

6

11

51

MISO

7

12

50

IRQ

8

2

–

•

Es importante recalcar que estos módulos funcionan a 3.3V; EL fabricante previene contra conectarles a 5V so pena de achicharrarlos.

•

La librería MIRF espera que hagas unas conexiones de pines diferentes, así que no te conviene usar este diagrama de conexión si pretendes utilizarla.

•

Tenéis que comprender que los modelos UNO y MEGA usan diferentes pines para el control del bus SPI y luego para rematar la faena, según la librería cambian de nuevo.

50

Vamos con el esquema de conexiĂłn de protoboard:

Para poder emitir y recibir con estos mĂłdulos vamos a necesitar dos Arduinos configurados con estas conexiones a los que llamaremos emisor y receptor, porque inicialmente los programas van a ser distintos.

Empezaremos haciendo la prueba mĂĄs sencilla posible. Uno de ellos, el emisor, radia una serie de nĂşmeros y el otro, el receptor, los recibe y muestra en la consola serie.

51

Package Contents

SainSmart InstaBots 2-Wheel Self-Balancing Upright Rover Car Robot Kit Pro

Instruction Manual

1 x UNO R3

Attention

1 x MEGA2560 R3

2 x Sensor Shield

1 x IIC/I2C 1602 LCD

2 x 24L01

1. Robot: 11.1V Li-Po battery is the best power source for the robot, but other types of batteries can be used.Ensure that the voltage of the battery is between 10V to 13V; 2. Remote control: Use a 9V battery for the remote controller; 3. Connect/ make sure that the NRF24l01wireless module and MPU6050 module has been inserted into the shield board.

4. Programme Remote_Controller_V3 and Upright_Rover_V3(provided) into arduino UNO and arduino mega2560 via USB cable. Since USB cable doesn’t have enough power supply to run the robot, so please connect to 11.1V Li-Po battery when testing.

5. You can wireless control the robot before it balances.

2 x USB Cable

2 x Joystick

Parts and Components 2 x Gear Motor

1 x 9V Battery Snap

2 x Wheel

1 x MPU6050

1 x Wireless Remote Controller Platform

2 x Motor bracket

V3 Shield Schematics

2 x Coupling

1 x Balancing Robot Platform

wires

* Batteries are not included

Wiring Battery: Positive electrode -> +11.1V Negative electrode -> GND Board: MOTOR+(red) -> 12V+ MOTOR -(black) -> 12VGND(green) -> GND Vcc(blue) -> 5V A Vout(yellow) -> PWM A B Vout(white) -> PWM B Download the code from product page

Remote control(left): GND -> G +5V -> V VRx -> A3 VRy -> A2 SW -> D3 Remote control(right): GND -> G +5V -> V VRx -> A1 VRy -> A0 SW -> D2

1

2

Motor MODEL: JGA25-370-12V-201rpm 1. Standard Operating Conditions Rated Voltage: Direction of Rotation: Operating Temperature and Humidity: Storage Temperature:

12V DC constant between motor terminals CW when viewed from output shaft side Temperature range of -10 ℃ ~+50 ℃ Humidity range of Temperature range of -20℃~+60℃

2. Measuring Conditions Motor Position: To be place t horizontally when measuring Power Supply: Regulated DC power supply Environmental Temperature and Humidity: Temperature range of 15℃ to 30℃ Relative humidity 30%and 70%

WWW.SAINSMART.COM

Encoder Wiring:

3. Electrical Characteristics (at initial stage after 30 seconds run-in) No Load Current: 50±5% ma No Load Speed: 01±5% rpm Starting Voltage: 1.5 V Rated Load: 0.53Kg.cm Rated Load Current: 250±5% ma Rated Load Speed: 160±10% rpm Stall Current: 900±5% ma Maximum torque: 2.8Kg.cm Power: 1.25 w External Appearance: Attached Outline Drawing Shaft End Play: 0.5~0.3 mm Weights: Approx: 100g LOCKED ROTO R: OK Life: >500H

1. MOTOR+ 2. MOTOR3. HALL SENSOR GND 4. HALL SENSOR Vcc 5. HALL SENSOR A Vout 6. HALL SENSOR B Vout

Motor

Dmm A,B

12 3

16 3

Output Circuit

3

20 12

24 12

28 16

36 16

42 18

Output Waveform

4

Connection

A.

B.

C.

D.

E.

F.

G.

H.

I.

J.

K.

L.

M.

N.

O.

Remote controller

A.

B.

E.

F.

C.

D.

G.