Rockerino

.accordion {
background-color: #eee;
color: #444;
cursor: pointer;
padding: 18px;
width: 100%;
border: none;
text-align: left;
outline: none;
font-size: 15px;
transition: 0.4s;
}

.active, .accordion:hover {
background-color: #ccc;
}

.accordion:after {
content: ‘02B’;
color: #777;
font-weight: bold;
float: right;
margin-left: 5px;
}

.active:after {
content: “2212”;
}

.panel {
padding: 0 18px;
background-color: white;
max-height: 0;
overflow: hidden;
transition: max-height 0.2s ease-out;
}


ROCKERINO:
ROBOT BALANCÍN

INTEGRANTES

  • RUBÉN CALVO MARTÍNEZ
  • MIGUEL ANGEL MONREAL VELASCO
  • RAÚL BERNARDINO IGLESIAS GONZÁLEZ

INTRODUCCIÓN

La elección de proyecto a
desarrollar utilizando la placa Arduino, ha sido la de montar un robot auto
balanceado para mantenerse en equilibrio. Tiene relación con el sistema que
implementan los Hoverboards o Seeways, ahora tan de moda y nos pareció una idea
bastante interesante, ya que puede estar enfocado a numerosas aplicaciones con
una necesidad de recursos mínima, consiguiendo resultados fascinantes como el
robot desarrollado por Google “Atlas”.

MATERIALES Y PRESUPUESTO

Los materiales requeridos
para montar tanto la infraestructura como los componentes necesarios para
desarrollar la funcionalidad son los siguientes:
·        
Infraestructura:
o  
1 x Tira de
velcro.
o  
2 x palillos
de madera.
o  
1 x tablón de
plástico.
o  
Hilo para
soldar FLUX SOLDER.
o  
Tuercas, varillas
y arandelas para sujeción.
o  
Cinta
aislante.
·        
Componentes:
o  
2 x Micro
motor DC con reductora. (9.46$)
o  
1 x Módulo driver
L298N para motor DC. (7.59$)
o  
1 x MPU650
Acelerómetro y giroscopio. (5.45$)
o  
Placa Arduino
MEGA.
o  
Porta pilas 6
pilas AA con cable. (1.50 $).     
o  
Pila de
petaca de 9v con conector de alimentación. (2.30$).         
o  
2 x
Interruptores simples.
 

IMPLEMENTACIÓN

1.- ¿CÓMO FUNCIONA EL EQUILIBRIO?


Hay dos factores importantes que hacen que el robot se mantenga en equilibrio.

  1. L298N: Es el
    módulo encargado de dar la potencia necesaria, en nuestro caso, a dos micro
    motores con reductora, para poder contrarrestar la caída del robot. Las
    directrices de cuanta potencia y para donde tiene que girar una rueda, se las
    da la placa arduino, que es el encargado de orquestar en armonía todos los
    chips del robot.
  2. MPU6050: Se
    encarga de dictar a la placa arduino la posición en el plano del mundo, para
    poder corregir de manera constante al robot. Va enviando información de la
    posición “x”, “y” y “z” para saber si el robot está levantado, tumbado o
    inclinado (puede caerse).
En cuanto a la colocación
de los componentes, el robot de equilibrio automático es esencialmente un
péndulo invertido. Puede equilibrarse mejor si el centro de masa es más alto en
relación con los ejes de las ruedas. Por esta razón casi todos los componentes
están colocados en el centro de los diferentes niveles, buscando la mejor
relación de peso en los diferentes lados tanto izquierdo como derecho, para que
sea más manejable y regulable los parámetros de equilibrio.
Antes de poder usar el
componente MPU6050, hay que calibrarlo, para poder tener las variables de
equilibrio correctas. Para ello hemos usado un código propio del fabricante
(anexo a esta descripción).

2.- MONTAJE

El proceso de montaje se
resume en el siguiente video:

3.- CÓDIGO

Para la implementación
del código hemos usado las siguientes librerias:

  1. PID
    (Proportional Integral Directive): Utilizada para calcular la posición del
    robot.
  2. LmotorController:
    Utilizada para el control de los motores con el módulo LN298N.
  3. I2Cdev y MPU6050_6Axis_MotionApps20:
    Utilizadas para leer los datos de la inclinación del robot con el módulo
    MPU6050.
Para el control del PID hemos
hecho uso de las variables Kp (Proportional), Ki (Integral) y Kd (Directive),
ajustadas a mano, además  de las
variables “originalSetpoint”, para configurar ángulo de partida; “setpoint”,
donde se almacena el ángulo actual del robot; “movingAngleOffset”, donde se
configura el ángulo de desplazamiento; y las variables input y outpot, todas
ellas de tipo double.
La variable Kp necesita
ser ajustada de forma manual para el correcto funcionamiento del robot, un
valor demasiado alto provocaría que se balancease hacia delante y hacia detrás
de forma violenta, un valor demasiado bajo acabaría con la caida del mismo, ya
que las correcciones de los motores no serían lo suficientemente grandes.
La variable Kd se refleja
en las oscilaciones que el robot da para alcanzar la estabilidad, si el valor
es el correcto el robot mantendrá el equilibrio a pesar de ser empujado.
A pesar de la correcta
configuración de las variables kp y kd el robot se balancea al iniciarse, para
que tarde lo menos posible es importante la correcta configuración de la
variable ki.
Es posible que el ángulo
y la velocidad de los motores necesiten ser re-configurados en función de como
se coloquen los componentes en el robot o que un motor sea algo más rápido que
el otro.
Hacemos uso de una variable
de control “dmpReady” para que, en caso de fallo, se pare la ejecución
del programa, además
de la variable “mpuInterrupt” que toma valor “true” cuando hay una interrupción
de MPU.

Calibración MPU6050

 // I2Cdev and MPU6050 must be installed as libraries  
   #include "I2Cdev.h"#  
 include "MPU6050.h"#  
 include "Wire.h"  
 ///////////////////////////////////  CONFIGURATION  /////////////////////////////  
 //Change this 3 variables if you want to fine tune the skecth to your needs.  
 int buffersize = 1000; //Amount of readings used to average, make it higher to get more precision but sketch will be slower (default:1000)  
 int acel_deadzone = 8; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge (default:8)  
 int giro_deadzone = 1; //Giro error allowed, make it lower to get more precision, but sketch may not converge (default:1)  
 // default I2C address is 0x68  
 // specific I2C addresses may be passed as a parameter here  
 // AD0 low = 0x68 (default for InvenSense evaluation board)  
 // AD0 high = 0x69  
 //MPU6050 accelgyro;  
 MPU6050 accelgyro(0x68); // <-- use for AD0 high  
 int16_t ax, ay, az, gx, gy, gz;  
 int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz, state = 0;  
 int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;  
 ///////////////////////////////////  SETUP  ////////////////////////////////////  
 void setup() {  
   // join I2C bus (I2Cdev library doesn't do this automatically)  
   Wire.begin();  
   // COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE  
   TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.  
   // initialize serial communication  
   Serial.begin(115200);  
   // initialize device  
   accelgyro.initialize();  
   // wait for ready  
   while (Serial.available() && Serial.read()); // empty buffer  
   while (!Serial.available()) {  
     Serial.println(F("Send any character to start sketch.n"));  
     delay(1500);  
   }  
   while (Serial.available() && Serial.read()); // empty buffer again  
   // start message  
   Serial.println("nMPU6050 Calibration Sketch");  
   delay(2000);  
   Serial.println("nYour MPU6050 should be placed in horizontal position, with package letters facing up. nDon't touch it until you see a finish message.n");  
   delay(3000);  
   // verify connection  
   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");  
   delay(1000);  
   // reset offsets  
   accelgyro.setXAccelOffset(0);  
   accelgyro.setYAccelOffset(0);  
   accelgyro.setZAccelOffset(0);  
   accelgyro.setXGyroOffset(0);  
   accelgyro.setYGyroOffset(0);  
   accelgyro.setZGyroOffset(0);  
 }  
 ///////////////////////////////////  LOOP  ////////////////////////////////////  
 void loop() {  
   if (state == 0) {  
     Serial.println("nReading sensors for first time...");  
     meansensors();  
     state++;  
     delay(1000);  
   }  
   if (state == 1) {  
     Serial.println("nCalculating offsets...");  
     calibration();  
     state++;  
     delay(1000);  
   }  
   if (state == 2) {  
     meansensors();  
     Serial.println("nFINISHED!");  
     Serial.print("nSensor readings with offsets:t");  
     Serial.print(mean_ax);  
     Serial.print("t");  
     Serial.print(mean_ay);  
     Serial.print("t");  
     Serial.print(mean_az);  
     Serial.print("t");  
     Serial.print(mean_gx);  
     Serial.print("t");  
     Serial.print(mean_gy);  
     Serial.print("t");  
     Serial.println(mean_gz);  
     Serial.print("Your offsets:t");  
     Serial.print(ax_offset);  
     Serial.print("t");  
     Serial.print(ay_offset);  
     Serial.print("t");  
     Serial.print(az_offset);  
     Serial.print("t");  
     Serial.print(gx_offset);  
     Serial.print("t");  
     Serial.print(gy_offset);  
     Serial.print("t");  
     Serial.println(gz_offset);  
     Serial.println("nData is printed as: acelX acelY acelZ giroX giroY giroZ");  
     Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");  
     Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");  
     while (1);  
   }  
 }  
 ///////////////////////////////////  FUNCTIONS  ////////////////////////////////////  
 void meansensors() {  
   long i = 0, buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0;  
   while (i < (buffersize + 101)) {  
     // read raw accel/gyro measurements from device  
     accelgyro.getMotion6( & ax, & ay, & az, & gx, & gy, & gz);  
     if (i > 100 && i <= (buffersize + 100)) { //First 100 measures are discarded  
       buff_ax = buff_ax + ax;  
       buff_ay = buff_ay + ay;  
       buff_az = buff_az + az;  
       buff_gx = buff_gx + gx;  
       buff_gy = buff_gy + gy;  
       buff_gz = buff_gz + gz;  
     }  
     if (i == (buffersize + 100)) {  
       mean_ax = buff_ax / buffersize;  
       mean_ay = buff_ay / buffersize;  
       mean_az = buff_az / buffersize;  
       mean_gx = buff_gx / buffersize;  
       mean_gy = buff_gy / buffersize;  
       mean_gz = buff_gz / buffersize;  
     }  
     i++;  
     delay(2); //Needed so we don't get repeated measures  
   }  
 }  
 void calibration() {  
   ax_offset = -mean_ax / 8;  
   ay_offset = -mean_ay / 8;  
   az_offset = (16384 - mean_az) / 8;  
   gx_offset = -mean_gx / 4;  
   gy_offset = -mean_gy / 4;  
   gz_offset = -mean_gz / 4;  
   while (1) {  
     int ready = 0;  
     accelgyro.setXAccelOffset(ax_offset);  
     accelgyro.setYAccelOffset(ay_offset);  
     accelgyro.setZAccelOffset(az_offset);  
     accelgyro.setXGyroOffset(gx_offset);  
     accelgyro.setYGyroOffset(gy_offset);  
     accelgyro.setZGyroOffset(gz_offset);  
     meansensors();  
     Serial.println("...");  
     if (abs(mean_ax) <= acel_deadzone) ready++;  
     else ax_offset = ax_offset - mean_ax / acel_deadzone;  
     if (abs(mean_ay) <= acel_deadzone) ready++;  
     else ay_offset = ay_offset - mean_ay / acel_deadzone;  
     if (abs(16384 - mean_az) <= acel_deadzone) ready++;  
     else az_offset = az_offset + (16384 - mean_az) / acel_deadzone;  
     if (abs(mean_gx) <= giro_deadzone) ready++;  
     else gx_offset = gx_offset - mean_gx / (giro_deadzone + 1);  
     if (abs(mean_gy) <= giro_deadzone) ready++;  
     else gy_offset = gy_offset - mean_gy / (giro_deadzone + 1);  
     if (abs(mean_gz) <= giro_deadzone) ready++;  
     else gz_offset = gz_offset - mean_gz / (giro_deadzone + 1);  
     if (ready == 6) break;  
   }  

Código Rockerino

 #include <PID_v1.h> // Libreria para calcular los valores de la posicion del robot: P (proportional), I (integral) y D (derivative).  
 #include <LMotorController.h> // Libreria para el uso de los dos motores con el modulo LN298N.  
 #include "I2Cdev.h" // Libreria para leer datos de la inclinación del robot con el MPU6050.  
 #include "MPU6050_6Axis_MotionApps20.h" // Libreria para leer datos de la inclinación del robot con el MPU6050.  
 #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE  
  #include "Wire.h"  
 #endif  
 #define MIN_ABS_SPEED 30 // Output a los motores.  
 MPU6050 mpu; // Variable del MPU.  
 // MPU6050 - variables de control y estado.  
 bool dmpReady = false; // Se inia a true si la inicializacion del DMP fue correcta.  
 uint8_t mpuIntStatus; // Byte de estado del MPU.  
 uint8_t devStatus; // Estado de retorno de cada operacion del robot (0 = correcto, !0 = error).  
 uint16_t packetSize; // Tamanio del paquete DMP (por defecto 42 bytes).  
 uint16_t fifoCount; // Conteo de los bytes actuales de la cola.  
 uint8_t fifoBuffer[64]; // Almacenamiento de buffer de cola.  
 // Variables de orientacion y movimiento.  
 Quaternion q; // [w, x, y, z] Contenedor cuaternio.  
 VectorFloat gravity; // [x, y, z] vector gravedad.  
 float ypr[3]; // [yaw, pitch, roll] Contenedor yaw/pitch/roll y vector de gravedad.  
 /* PID - Proporciona la correcion entre el valor deseado (o entrada) y el valor real (salida).  
  * La diferencia entre estas dos es el margen de error.  
  * El controlador del PID reduce el error a lo minimo posible, ajustando constantemente el valor de salida.  
  */  
 double originalSetpoint = 172.50; // Configuracion del punto de ajuste.  
 double setpoint = originalSetpoint;  
 double movingAngleOffset = 0.1; // Configuracion del angulo.  
 double input, output;  
 /* KP, KD y KI  
  * El input es la inclinacion deseada, en grados.  
  * El MPU6050 lee la inclinacion actual del robot y pasa esa informacion al PID, el cual  
  * hace calculos para controlar el motor y mantener al robot en equilibrio.  
 */  
 double Kp = 38; // < kp = poca correcion (el robot se caera). > kp = robot "baile" (hacia delante y hacia atras) .   
 double Kd = 1.95; // Un buen kd disminuye las oscilaciones hasta estabilizarlo.  
 double Ki = 270; // El valor ki correcto acortará el tiempo necesario para que el robot se estabilice.  
 PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);  
 // VELOCIDAD MOTOR - Valores de velocidad de los dos motores.  
 double motorSpeedFactorLeft = 0.6; //0.555  
 double motorSpeedFactorRight = 0.49; //0.45  
 // L298N - Control de pines.  
 int ENA = 5;  
 int IN1 = 6;  
 int IN2 = 7;  
 int IN3 = 9;  
 int IN4 = 8;  
 int ENB = 10;  
 LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);  
 /* Indica si el pin de interrupción de la MPU se ha elevado.  
  * Variable "volatile" para cambios en tiempo de ejecucion.  
 */  
 volatile bool mpuInterrupt = false;   
 void dmpDataReady(){ // Inicializacion en el setup de la variable mpuInterrupt a true.  
  mpuInterrupt = true;  
 }  
 void setup(){  
  // Union de I2C bus (la libreria I2Cdev no lo hace automaticamente).  
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE  
  Wire.begin();  
  TWBR = 24; // 400kHz I2C de reloj (200kHz si la CPU es de 8MHz).  
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE  
  Fastwire::setup(400, true);  
  #endif  
  mpu.initialize(); // Inicializacion del MPU6050.  
  devStatus = mpu.dmpInitialize(); // Inicializacion del DMP.  
  // OFFSETS - Mis offsets en el test de calibracion son: 1327 -940 1316 82 36 -25, pero no tienen por que ser esos.  
  mpu.setXGyroOffset(82);  
  mpu.setYGyroOffset(304);  
  mpu.setZGyroOffset(-25);  
  mpu.setZAccelOffset(1316);  
  // DMP - Devuleve 0 si esta listo, en caso contrario control de errores.  
  if (devStatus == 0){  
   mpu.setDMPEnabled(true); // Habilitamos el DMP.  
   attachInterrupt(0, dmpDataReady, RISING); // Habilitar la detecion de interrupciones del arduino.  
   mpuIntStatus = mpu.getIntStatus(); // Estado del MPU.  
   dmpReady = true; // Control de errores mediante booleano en la funcion loop.  
   packetSize = mpu.dmpGetFIFOPacketSize(); // Tamanio de paquete para una comparacion posterior en el loop.  
   // Inicializacion de PID.  
   pid.SetMode(AUTOMATIC);  
   pid.SetSampleTime(10);  
   pid.SetOutputLimits(-255, 255);   
  } else {  
   /* ERROR!   
   * 1 = Carga de memoria inicial fallo.  
   * 2 = Las actualizaciones de configuración de DMP fallaron.  
   */  
   Serial.print(F("DMP Inicializacion fallida (codigo "));  
   Serial.print(devStatus);  
   Serial.println(F(")"));  
   }  
 }  
 void loop()  
 {  
  // Si el codigo falla, no hace nada.  
  if (!dmpReady) return;  
  // A la espera de interrupcion de MPU o paquete extra disponible.  
  while (!mpuInterrupt && fifoCount < packetSize){ // Si hay interrupcion o si la cola es mayor, no entro al bucle.  
  pid.Compute(); // Calculos del algoritmo del PID.  
  motorController.move(output, MIN_ABS_SPEED); // Output a los motores.  
  }  
  mpuInterrupt = false; // Reseteo de la varibale de interrupcion.  
  mpuIntStatus = mpu.getIntStatus(); // Seteo del estado del MPU.  
  fifoCount = mpu.getFIFOCount(); // Contado de la cola.  
  /* Interrupcion de datos DMP -> Lectura de paquete de la cola -> Seteo de las variables de orientacion y movimiento.  
  * En caso contrario, control de errores. Checkeo del desbordamiento (esto nunca debería ocurrir a menos   
  * que nuestro código sea demasiado ineficiente).  
  */  
  if ((mpuIntStatus & 0x10) || fifoCount == 1024){ // Seleccionamos el bit de interes y enmascaramos el resto.  
  mpu.resetFIFO(); // Reseteamos la cola.  
  Serial.println(F("FIFO overflow!")); // Retroalimentacion.  
  } else if (mpuIntStatus & 0x02){ // Por el contrario, checkeamos la interrupción de datos DMP.  
  while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // Esperamos a un tamanio correcto de cola.  
  mpu.getFIFOBytes(fifoBuffer, packetSize); // Leemos paquete de la cola.  
  fifoCount -= packetSize; // Seguimiento del conteo de la cola si > 1 paquete disponible.  
  // Seteamos las variables de orientacion y movimiento globales pasadas por referencia.  
  mpu.dmpGetQuaternion(&q, fifoBuffer);  
  mpu.dmpGetGravity(&gravity, &q);  
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);  
  input = ypr[1] * 180/M_PI + 180;  
  }  
 }  

PROBLEMAS Y
SOLUCIONES ENCONTRADAS

Al comienzo del
desarrollo del proyecto, no sabíaamos que Arduino Mega tiene los pines SDA y SCL
propios y exclusivos para esa función, cosa que por ejemplo Arduino Uno no los
tiene de ese modo. Esto nos llevó a equivocación, hasta que los encontramos y
conseguimos solucionarlo.
Por otro lado, uno de los
problemas más importantes fue que uno de los motores de una de las ruedas iba más
rápido que el otro. Esto hacia que Rockerino se moviese dando vueltas, aunque
también se mantenía en equilibrio, pero acababa cayéndose. Al principio
pensamos que era problema del MPU6050 (acelerómetro y giroscopio) que no nos
estaba dando los datos correctos. Pero después de muchas pruebas nos dimos
cuenta que era problema del L298N (driver DC motores) que no le estábamos dando
unas variables de configuración iniciales correctas. Y también era problema de
los dos motores que giran las ruedas y de las propias ruedas, que eran
distintas.
Estas dos variables son
las que tenemos que facilitar a la función del constructor del módulo junto con
los pines que hemos usado en nuestro Arduino, para que sepa a qué velocidades
tiene que ir cada motor independientemente.
double
motorSpeedFactorLeft = 0.6;
double
motorSpeedFactorRight = 0.49;
LMotorController
motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft,
motorSpeedFactorRight);
Como puedes observar los
valores son distintos, esto es debido a que los motores y las ruedas, por
alguna razón, ha sido fabricados por distintos fabricantes, y, por lo tanto, no
se movían de la misma forma y velocidad. Cambiar estos valores fue nuestra
solución.

POSIBLES MEJORAS

Una de las mejoras que
nos hubiese gustado realizar, pero por falta de tiempo no la hemos contemplado,
es la de incorporar un control remoto del Rockerino. De este modo, no solo se
mantendría levantado y no se caería, sino que también podríamos manejarlo, como
si fuera un coche teledirigido. La idea sería que se mantuviese de pie,
mientras podemos manejarlo.
Otra mejora que también
pensamos fue incorporar en el código una forma de que Rockerino estando tumbado
se levantase solo, parece algo fácil, quizá para la próxima.

CONCLUSIONES

Gracias a esta práctica
hemos profundizado más en el mundo de la robótica, ya que algunos miembros de
nuestro equipo ya sabíamos hacer algunas cosas, pero con un proyecto un poco
más extenso, como es este, nos ayuda a mejorar y aprender cosas nuevas.
Además, estamos agradecidos
de tener una asignatura como esta en nuestro grado, para poner en práctica
nuestros conocimientos en Hw y Sw.

var acc = document.getElementsByClassName(“accordion”);
var i;

for (i = 0; i < acc.length; i++) { acc[i].addEventListener("click", function() { this.classList.toggle("active"); var panel = this.nextElementSibling; if (panel.style.maxHeight){ panel.style.maxHeight = null; } else { panel.style.maxHeight = panel.scrollHeight + "px"; } }); }

También te podría gustar...

Deja un comentario

Tu dirección de correo electrónico no será publicada. Los campos obligatorios están marcados con *