Proyecto balancing joaquin 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-12v-
201rpm http://www.sainsmart.com/sainsmart-instabots-upright-rover-kit-pro-motor.html

¡¡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
Diferenciassustancialesentrelasdistintasversiones:
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"seobtienenlosvaloresoffset
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;
#defineGry_offset0
// 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
#defineAngle_offset0
#defineRMotor_offset20
#defineLMotor_offset20
#define pi 3.14159
longdata;
int x, y;

float kp, ki, kd;
floatr_angle, f_angle, omega;
floatTurn_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 -----
intTN1=23; // adapatado por mí a la MEGA
intTN2=22;
intENA=5;
int TN3=24;
int TN4=25;
intENB=4;
//--------------------------------------------------------
void setup() {
Wire.begin();
accelgyro.initialize(); //se le he asignado al #define MPU6050 accelegyro
pinMode(TN1,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:
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
withoffsets: 6 -6 16387 0 0 0
Youroffsets: 862 6525 1506 -112 59 -8

Serial.println("InitializingI2Cdevices...");
accelgyro.initialize();
//verifyconnection
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("ValorA0:");Serial.print(analogRead(A0));Serial.print("t");
Serial.println("ValorA2:");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; Serial.print(" f_angle= ");Serial.println(
timeChange = (now - lastTime);

24
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 HIGH+
SENTIDO C
digitalWrite(TN4, HIGH); //en MEGA pin 25 LOW
}
else if(ROutput < 0){
digitalWrite(TN3, HIGH); //en MEGA pin 24 GIRAR RUEDAS al contrario a IZQDA LOW
digitalWrite(TN4, LOW); //en MEGA pin 25
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 HIGH MISMO
SE
}
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. A CONTINUACIÓN EN HORIZONTAL PARA PODER VER TODO LO ESCRITO.

25
/*
+++++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"seobtienenlosvaloresoffset o bien este
otro. que lo hace automaticamente "Tutorial_MPU6050_equilibrado"
*/
#include "Wire.h"
#include "SPI.h"

26
#include "Mirf.h"
#include "nRF24L01.h"
#include "MirfHardwareSpiDriver.h"
#include "I2Cdev.h"
MPU6050accelgyro; //NOMBRE que le asigna al acelerometro giroscopio int16_t ax, ay,
az;
int16_t gx, gy, gz;
#defineGry_offset0
// 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
#defineAngle_offset0
#defineRMotor_offset20
#defineLMotor_offset20
#define pi 3.14159
longdata; int x, y;
float kp, ki, kd; floatr_angle, f_angle,
omega;
floatTurn_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 -----
intTN1=23; // adapatado por mi a la MEGA intTN2=22;
intENA=5;
intTN3=24; intTN4=25; int
ENB=4;
//--------------------------------------------------------

27
void setup() { Wire.begin();
accelgyro.initialize(); //se le he asignado al #define MPU6050 accelegyro
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:
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
withoffsets: 6 -6 16387 0 0 0
Youroffsets: 862 6525 1506 -112 59 -8

28
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();
//verifyconnection
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("ValorA0:");Serial.print(analogRead(A0));Serial.print("t");
Serial.println("ValorA2:");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

29
/* ----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 =

30
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;

31
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 HIGH+ SENTIDO CONTRARIO GIRO
}
else if(ROutput < 0){
digitalWrite(TN3, HIGH); //en MEGA pin 24 GIRAR RUEDAS al contrario a IZQDA LOW
digitalWrite(TN4, LOW); //en MEGA pin 25 HIGH
}
else{

32
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 HIGH MISMO SENTIDO CAIDA
}
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.

33
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.

34
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 Dirección I2C
AD0=HIGH (5V) 0x69
AD0=LOW (GND o NC) 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 21 3
SDA A4 20 2

35
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 "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); //Iniciando puerto serial
Wire.begin(); //Iniciando I2C
sensor.initialize(); //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);

36
//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 "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:
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:

37
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-y-
z 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:

38
#include "I2Cdev.h"
#include "Wire.h"
// del estado de AD0. Si no se especifica, 0x68 estará implicito
#define MPU6050 sensor;
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() {
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");

39
}
void loop() {
// 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){
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--;

40
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

41
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:
#include "I2Cdev.h"
#include "Wire.h"
// del estado de AD0. Si no se especifica, 0x68 estará implícito

42
MPU6050 sensor;
s x,y,z
int ax, ay, az;
int gx, gy, gz;
void setup() {
correctamente");
}
void loop() {
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);
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.

43
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:

44
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:
#include "I2Cdev.h"
#include "Wire.h"
MPU6050 sensor;
// Valores RAW (sin procesar) del acelerometro en los ejes x,y,z
int ax, ay, az;
void setup() {
correctamente");
}
void loop() {
// Leer las aceleraciones
//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);
}

45
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:

46
#include "I2Cdev.h"
#include "Wire.h"
MPU6050 sensor;
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() {
correctamente");
tiempo_prev=millis();
}
void loop() {
// Leer las velocidades angulares
//Calcular los angulos rotacion:
dt = millis()-tiempo_prev;
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);

47
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

48
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:
#include "I2Cdev.h"
#include "Wire.h"
MPU6050 sensor;
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() {
correctamente");
}
void loop() {

49
dt = (millis()-tiempo_prev)/1000.0;
//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 -----------------------------------------------------------

50
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.

51
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.

Instruction Manual
SainSmart InstaBots
2-Wheel Self-Balancing
Upright Rover Car Robot Kit Pro
Motor
Parts and Components
V3 Shield Schematics
MODEL: JGA25-370-12V-201rpm
1. Standard Operating Conditions
Rated Voltage: 12V DC constant between motor terminals
Direction of Rotation: CW when viewed from output shaft side
Operating Temperature and Humidity: Temperature range of -10 ℃ ~+50 ℃
Humidity range of
Storage Temperature: 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%
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
Attention
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.
Wiring
Battery:
Positive electrode -> +11.1V
Negative electrode -> GND
Board:
MOTOR+(red) -> 12V+
MOTOR -(black) -> 12V-
GND(green) -> GND
Vcc(blue) -> 5V
A Vout(yellow) -> PWM A
B Vout(white) -> PWM B
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
Package Contents
1 x UNO R3 1 x MEGA2560 R3 1 x IIC/I2C 1602 LCD 2 x USB Cable
2 x Sensor Shield 2 x 24L01 2 x Joystick
2 x Gear Motor 2 x Wheel 2 x Motor bracket 2 x Coupling
1 x 9V Battery Snap 1 x MPU6050 1 x Balancing Robot Platform
1 x Wireless Remote Controller Platform wires
* Batteries are not included
Motor
Encoder Wiring:
1. MOTOR+
2. MOTOR-
3. HALL SENSOR GND
4. HALL SENSOR Vcc
5. HALL SENSOR A Vout
6. HALL SENSOR B Vout
Dmm 12 16 20 24 28 36 42
A , B 3 3 12 12 16 16 18
Output Circuit Output Waveform
1 2
3 4
Download the code
from product page
WWW.SAINSMART.COM

Remote controller
Connection
A. B. C. D.
A. B.
C.
E. F. G.
E. F. G.
H.
I. J. K.
M. N. O.
L.
D.

Proyecto balancing joaquin berrocal piris abril 2016 - 53 pag

Empfohlen

Empfohlen

Weitere ähnliche Inhalte

Was ist angesagt?

Was ist angesagt? (18)

Andere mochten auch

Andere mochten auch (9)

Ähnlich wie Proyecto balancing joaquin berrocal piris abril 2016 - 53 pag

Ähnlich wie Proyecto balancing joaquin berrocal piris abril 2016 - 53 pag (20)

Mehr von joaquinin1

Mehr von joaquinin1 (18)

Kürzlich hochgeladen

Kürzlich hochgeladen (20)

Proyecto balancing joaquin berrocal piris abril 2016 - 53 pag