Dron Pepito. Mi primer arduino quadcopter.

Dron Pepito es mi primer quadricoptero. Es totalmente casero y el codigo tambien es mio aqui podeis verlo https://github.com/elchals/Dron-Pepito-arduino-quadcopter./tree/master . Tiene una longitud de 65cm de diametro, de eje motor a eje motor y pesa unos 1200 gramos.

DSC_0496
Dron Pepito.

Los brazos del dron estan echos con tubo cuadrado de aluminio de 16x16mm. Lo que hace que sean rigidos y ligeros. El resto esta impreso con mi impresora 3D en PLA. Aqui se puede ver un poco el proceso.

Y aqui teneis las piezas que diseñe, listas para imprimir http://www.thingiverse.com/thing:1013564.

Los motores que se usan para los drones un poquito grandes como este son los llamados brushless , o sea sin escobillas. Al no tener escobillas hay menos rozamiento y permite obtener velocidad muy altas con motores mas pequeños. Es imporante elegir bien los motores segun el tipo y peso del dron,de las helices… . Yo no soy ningun experto en esto , pero en internet hay mucha informacion y calculadoras. Yo elegi unos de 1000Kv. no se si acertadamente o no , simplemente porque encontre un kit barato y vi que mucha gente los usaba. 1000Kv. quiere decir que por cada voltio que le apliquemos al motor girara 1000 RPM. Si los alimento a 12voltios girarian a 12000RPMs, conque ya podeis ver lo rapidos que son este tipo de motores.

Estos motores no se pueden alimentar con una bateria directamente , ya que son trifasicos. Hacen falta los ESCs (electronic speed controler) para controlarlos. Estos ESCs reciven un pulso de entre 1 y 2ms. y segun la longitud de ese pulso los hace girar mas o menos rapido.

Este es uno de los ESCs que yo uso.

Los tres cables azules serian las tres fases que irian al motor , el rojo y negro gordos ,serian la alimentacion del esc , el blanco seria por donde enviarimos los pulsos al ESC y por ultimo el negro y rojo pequeños nos pueden servir para alimentar el arduino , sensores… ya que el ESC tiene un regulador de tension a 5V., pero si usamos esto , la sacariamos solo de uno de los ESCs ,no de los cuatro. Podemos cortar los otros tres o simplemente no conectarlos.

Para alimentar los motores empleo una bateria LIPO de 11,1 voltios y 2200A. ,esta la conecto a una placa que me hice con la fresadora, a la que llaman power distribution board , que simplemente es la encargada de distribuir el voltage a todo.

DSC_0487

DSC_0488

Antes de usar un ESC habria de calibrarse. Como no todos los mandos RC emiten la misma longitud de pulso , hay que hacer saber al ESC, el pulso minimo del Throttle de nuestro mando (seria como el acelerador) y el pulso maximo .En mi caso cuando el throttle esta al minimo da 1ms. y al maximo 2ms.

Normamente para calibrar un ESC se le ha de mandar el pulso maximo teniendo el throttle al maximo mientras la bateria esta desconectada , entonces conectar la bateria y el ESC hara una serie de pitidos, bajar el throttle al minimo para enviar el pulso minimo y tras oir otra serie de pitidos ya tenemos el ESC calibrado para los pulsos de nuestra emisora. Hay que repetir el proceso con cada uno de los motores. Es mucho mas facil de hacer que de explicar , asi que os recomiendo que mireis uno de los miles de videos que hay por internet para hacerlo.

Como sensor inercial he usado un MPU6050 , que ya tenia. Este sensor consta de un acelerometro y un giroscopio de 3 ejes ,suficiente para controlar el dron ,ya que este tiene tres ejes , que son yaw , pitch y roll.

PitchRollYaw

Una cosa muy importante es que como el acelerometro es muy sensible a las vibraciones de los motores, si no lo aislamos de la estructura del dron ,sus lecturas seran inservibles. Yo lo he echo con un trozo de espuma , aunque hay otras opciones.

Hay muchos tipos de drones , de 3 , 4 , 6 , 8… motores. El mio tiene cuatro ,lo que seria un quadricoptero. Dentro del grupo de los quadricopteros hay principalmente dos , en cruz y en equis , yo lo he echo en equis , porque como quiero añadirle una camara , asi las patas de delante no tapan la vision de la camara.

Para que dron Pepito pueda mantener el vuelo de una forma estable es necesario que dos motores giren en un sentido y dos en el otro y por lo tanto dos llevaran helices de sentido horario y los otros dos de sentido antihorario. Esto es asi porque si los cuatro motores giraran en el mismo sentido , el dron giraria todo el rato sobre su eje Yaw y nunca podria estar quieto. Al haber dos de un sentido y dos del otro , cuando los cuatro giran a la misma velocidad se anulan y el dron no gira. De ahi se deduce que si la suma de los dos de un sentido es mayor que la del otro , el dron girara sobre su eje yaw en un sentido o en el otro segun sea mayor el sentido horario o antihorario y a mas o menos velocidad dependiendo de la diferencia de velocidad. Aqui en este grafico que he sacado de internet se explica muy bien como mover el dron sobre sus tres ejes controlando la velocidad de los cuatro motores.

2

Hay varios tipos de vuelo de los drones y cada uno necesita de mas o menos sensores. Como yo solo hago uso del MPU6050 puedo hacer dos , que son el acrovatico y el estable , que serian los vuelos basicos. El modo acrovatico solo se hace uso del giroscopio ,siendo el mas facil de programar , pero por contra es muy dificil controlar el dron en este modo si no eres un gran experto. En este modo lo que le decimos con los joysticks del mando es a que velocidad y en que sentido queremos que gire cada eje del dron. Entonces si dejamos todos los mandos a cero ,quiere decir que todos los ejes mantendran la posicion ,pero no se estavilizaran solos , siendo esto trabajo del piloto. En cambio en el modo estable si hacemos uso del acelerometro para saber la posicion de cada eje del dron y con el mando controlamos el angulo deseado en cada eje. Esto quiere decir que si dejamos los mandos a cero , todos los ejes quedaran a cero grados manteniendose autoestable , solo teniendo que controlar la altura del dron.

El primer paso seria programar el modo acrovatico. Su funcionamiento es tan sencillo como que si queremos que el dron gire en su eje roll a 5 º/s ,hacemos la lectura del giroscopio del eje roll y corregimos la velocidad de los motores implicados en el movimiento roll mediante un PID. Esto lo hariamos con cada uno de los tres ejes. Esto quiere decir que hay tambien tres PIDs , uno por eje. Si no sabeis de que va eso de los controles PID , os recomiendo que leais mis entradas sobre Manolo self balancig robot , donde los explico.

El control en modo estable lo hago mediante otro PID en cascada. Por ejemplo si quiero que el dron este a 45º en pitch , mido el angulo atraves del acelerometro (esto tambien lo explico mejor en el post de Manolo robot) , la diferencia entre angulo deseado y angulo actual es el error que introduzco el PID del eje del modo estable y la salida sera la velocidad a la que quiero que gire el eje para corregir ese error, siendo esta la entrada del PID del eje del modo acrovatico. Siendo otra vez otros tres PIDs. En total seis en el modo estable.

Tras equilibrar helices y motores para evitar vibraciones innecesarias ,el  paso mas importante para un buen vuelo es ajustar bien los parametros de los PIDs. Primero se empieza ajustando los del modo acro. Yo para hacerlo lo hice con un “banco de pruebas” que me monte como pude , aunque con la experiencia , ahora mismo lo hago sin nada. Los parametros los ajusto atraves de bluetooth con mi telefono. Aqui explico como lo hice.

Con los PIDs del modo acro ajustados , ya se puede pasar a ajustar los PIDs del estable. Este lo hice mas con el metodo de prueba error, aunque hay bastante informacion en la red de como hacerlo.

Ya con todo ajustado ya se puede hacer volar al dron , aunque por experiencia recomiendo empezar en algun simulador o comprar muchas helices ya que es mas dificil de manejar de lo que me pensaba ajjajaja.

No voy a explicar el codigo ya que creo que si lo veis se entiende bastante bien y es muy parecido a lo que explico en el robot Manolo pero echo para tres ejes. De todas formas si teneis dudas no tengo problemas en intentarlas resolver. Tambien decir que aqui explico como leer el mando Rc de forma eficiente https://elchals.wordpress.com/category/tutoriales/leyendo-mando-rc/ .

Al final Dron Pepito volo bastante bien , pero tuve problemas para poner la camara debido a su forma y sus patas que molestaban y cambie un poco la forma pasando a ser Dron Simon, al que le construi un gimbal para estavilizar la camara, una copia de la GoPro china. Aqui un video de su primer vuelo.

 

 

 

Como leer un mando RC con arduino y interrupciones de hardware.

Hoy vamos a ver como leer los datos de un mando RC con arduino de una forma eficiente. Los mandos RC mandan pulsos a una fecuencia de 50Hz. , es decir cada 20ms. manda un nuevo pulso. La duracion de estos pulsos nos indica la posicion de la palanca. Tipicamente van de 1ms. que indicaria la palanca al minimo , a 2ms. que indicaria que esta al maximo. Un pulso por cada canal del mando. Es el mismo tipo de pulso que usan los servos.

Hay mandos de mas o menos canales. Por ejemplo el que yo uso ,que es uno barato , tiene cuatro canales. En mi caso puedo controlar por ejemplo el throttle , el pitch, el roll y el yaw de un dron.

Entonces lo que hemos de hacer es leer la duracion de los pulsos en cada canal del receptor y segun esta duracion sabremos la posicion de la palanca del mando.

Este es mi mando RC de cuatro canales.

 

Y este es el receptor. Tiene 6 canales , pero solo aprovecho cuatro.

Lo primero que hay que hacer si la emisora es nueva , es enlazar la emisora con el receptor. En este caso simplemente se ha de conectar un puente que nos sale con la emisora en el puerto bind, en este caso canal3 ,darle voltage en alguno de los pines positivo y negativo (5voltios) y con la emisora apagada dejar pulsado un boton que pone bind , encender la emisora y aguantar hasta que la luz se quede fija. Ahora ya estaria enlazadas y listas para usarse. Una vez enlazadas no hace falta hacerlo nunca mas.

Para hacer las pruevas he creado un circuito simple en una protoboard.

DSC_0506

La conexion es muy secilla.

  • Vin – a algun pin + del receptor.
  • Grn – a algun pin – del receptor.
  • A0 – pin señal Ch1.
  • A1 – pin señal Ch2.
  • A2 – pin señal Ch3.
  • A3 – pin señal Ch4.

La forma mas sencilla de leer los pulsos del receptor seria mediante un programa similar a este.


int throttle;
int pitch;
int roll;
int yaw;

void setup() {
Serial.begin(9600);
}

void loop() {
roll = pulseIn(A0,HIGH);
pitch = pulseIn(A1,HIGH);
throttle = pulseIn(A2,HIGH);
yaw = pulseIn(A3,HIGH);

Serial.print("Throttle:");
Serial.print(throttle);
Serial.print(" Pitch:");
Serial.print(pitch);
Serial.print(" Roll:");
Serial.print(roll);
Serial.print(" Yaw:");
Serial.println(yaw);
}

Ahora si lo subimos al arduino y abrimos el puerto serie veriamos los valores de cada canal. Y estos variarian segun movamos las palancas del mando asi.

Untitled

El codigo es muy simple de entender. La lectura la hace la funcion pulseIn. Por ejemplo la linea:


roll = pulseIn(A0,HIGH);

Esto lo que hace es que se queda esperando hasta que en el pin A0 haya un pulso HIGH ,cuenta la duracion de este y lo carga en la variable roll. Esto lo hago con los cuatro canales y el valor que nos devuelve esta en microsegundos. 1000 seria con la palanca del canal al minimo y 2000 al maximo.

Este es el tipico codigo ejemplo que esta en todos los sitios. Parece que con esto ya valga , pero la realidad es que este codigo no vale para nada. No es util. La razon es que arduino pierde mucho tiempo leyendo los pulsos y haria que el ciclo del programa de arduino fuese demasiado largo. En el peor de los casos estaria 20 milisegundos en cada canal antes de continuar con el resto de programa. Lo que en este caso serian 80milisegundos. Esto dependiendo para que es una eternidad, teniendo en cuenta que por ejemplo para estavilizar un dron se suelen emplear ciclos por debajo de los 5ms.

Ahora os voy a explicar como lo hago yo. Seguro que hay otras formas , pero esta , aunque es mas dificil de entender , es una forma muy eficiente de hacerlo , consumiendo muy poco tiempo por ciclo en las lecturas.

Lo hago mediante interrupciones por hardware. La forma de funcionar de estas interrupciones es facil de entender. Arduino esta a lo suyo hasta que en alguno de los pines que tenga asociada la interrupcion reciva un pulso. Cuando recive este pulso , arduino deja de hacer lo que este haciendo y pasa a ejecutar la rutina de la interrupcion y despues vuelve donde lo habia dejado.

Un simil seria , cuando estamos en el sofa tiraos viendo una pelicula con la cerveza y llaman al timbre. Esta seria la interrupcion. En ese momento paramos la pelicula , atendemos la llamada y cuando finalizamos con esta ,volvemos al sofa y continuamos con la pelicula donde lo habiamos dejado.

En arduino nano hay dos interrupciones INT0 y INT1. Yo voy a hacer uso de INT1 , pero igualmente podria haber usado la otra. Primero veamos el codigo y despues lo explico.


int throttle;
int pitch;
int roll;
int yaw;

long tiempo_pasado1;
long tiempo_pasado2;
long tiempo_pasado3;
long tiempo_pasado4;

int canal_1;
int canal_2;
int canal_3;
int canal_4;

void setup() {
Serial.begin(9600);
PCICR |= (1 << PCIE1); //Activo interrupcion por hardware 1.
PCMSK1 |= (1 << PCINT8); //Pin A0. ROLL CH1
PCMSK1 |= (1 << PCINT9); //Pin A1. PITCH CH2
PCMSK1 |= (1 << PCINT10); //Pin A2. THROTTLE CH3
PCMSK1 |= (1 << PCINT11);
}

void loop() {
roll = pulseIn(A0,HIGH);
pitch = pulseIn(A1,HIGH);
throttle = pulseIn(A2,HIGH);
yaw = pulseIn(A3,HIGH);

Serial.print("Throttle:");
Serial.print(throttle);
Serial.print(" Pitch:");
Serial.print(pitch);
Serial.print(" Roll:");
Serial.print(roll);
Serial.print(" Yaw:");
Serial.println(yaw);
}
ISR(PCINT1_vect){
long tiempo_actual = micros();
//ROLL=============================================
if(PINC & B00000001){
if(canal_1 == 0){
canal_1 = 1;
tiempo_pasado1 = tiempo_actual;
}
}
else if(canal_1 == 1){
canal_1 = 0;
roll = tiempo_actual - tiempo_pasado1;
}
//PITCH============================================
if(PINC & B00000010){
if(canal_2 == 0){
canal_2 = 1;
tiempo_pasado2 = tiempo_actual;
}
}
else if(canal_2 == 1){
canal_2 = 0;
pitch = tiempo_actual - tiempo_pasado2;
}
//THROTTLE=========================================
if(PINC & B00000100){
if(canal_3 == 0){
canal_3 = 1;
tiempo_pasado3 = tiempo_actual;
}
}
else if(canal_3 == 1){
canal_3 = 0;
}
//YAW=============================================
if(PINC & B00001000){
if(canal_4 == 0){
canal_4 = 1;
tiempo_pasado4 = tiempo_actual;
}
}
else if(canal_4 == 1){
canal_4 = 0;
yaw = tiempo_actual - tiempo_pasado4;
}
}

Demomento no voy a entrar muy a fondo en la configuracion de las interrupciones , ya que seria muy largo y no es facil de entender. Quedemonos con que la linea:

  • PCICR |= (1 << PCIE1);

Activa la interrupcion 1. Y en las siguientes.

  • PCMSK1 |= (1 << PCINT8); 
    PCMSK1 |= (1 << PCINT9); 
    PCMSK1 |= (1 << PCINT10); 
    PCMSK1 |= (1 << PCINT11);

Asocian las pines A0,A1,A2 y A3 a esa interrupcion.

He de decir que este codigo solo funcionaria con arduino nano. Porque por ejemplo PCINT8 correspone al pin A0 o PC0 en arduino nano. Pero no tiene porque ser asi en otros arduinos. Creo que con arduino Uno son iguales , pero ahora mismo o estoy seguro , habria que provarlo. Es posible que tambien funcionara con arduino uno. Si no tocaria adaptarlo. Estos son los pines de arduino nano.

Esto lo que  hace es que cuando haya un cambio de estado en alguno de los cuatro pines A0-A3 tanto sea de LOW a HIGH o de HIGH a LOW arduino saltara a la rutina de la interrupcion. Que en este caso sera :

ISR(PCINT1_vect){}

Ejecutara lo que haya entre las llaves de la funcion y volvera al loop principal donde lo dejo.

Voy a explicar un poco esta funcion.


long tiempo_actual = micros();
//ROLL=============================================
if(PINC &amp; B00000001){
if(canal_1 == 0){
canal_1 = 1;
tiempo_pasado1 = tiempo_actual;
}
}
else if(canal_1 == 1){
canal_1 = 0;
roll = tiempo_actual - tiempo_pasado1;
}

Primero compruebo si el pin A0 (PC0 o primer bit de PORTC) esta en HIGH , si lo esta y el flag canal_1 esta a cero , quiere decir que ha habido un cambio de estado en este pin de LOW a HIGH. Entonces activo el flag canal_1 y pongo en marcha un contador y continua con el programa. en otra llamada a esta funcion , cuando A0 este en LOW y el flag este a 1, querra decir que ha habido un cambio en ese pin de HIGH a LOW. Entonces con el conador puesto en marcha anteriormente sabemos la duracion del pulso recivido. Pongo otra vez el flag a cero y vuelta a empezar.

Esto lo hago con los cuatro pines A0-A3. Como no estamos esperando hasta que se produzca el cambio de estado del pin, sino que simplemente ponemos en marcha un contador y seguimos con el programa,no se pierde nada de tiempo ,resultando un codigo muy eficiente a costa de ser mas complicado de entender. El resultado de este codigo es el mismo que el del de antes , la duracion de los pulsos en microsegundos , pero infinitamente mas rapido.

Algo parecido y mas facil de entender lo podriamos haber echo mediante la libreria PinChangeInt ,que crea interrupciones por hardware en cualquier pin de una manera mas facil. Pero yo si se, intento evitar librerias de terceros cuando quiero un programa rapido.

 

Manolo self balancing robot part.4-Moviendo los motores.

Bueno ahora ya sabemos el angulo al que esta el robot y con ese angulo y los PIDs hemos calculado la velocidad a la que habriamos de girar los motores. Ahora solo queda que los motores giren segun la salida de los PIDs.

Yo he usado dos motores paso a paso NEMA17. Ahora lo que yo hago es, si el robot esta dentro de una inclinacion razonable para estavilizarlo, activo los motores poniendo a cero el pin ENABLE del driver y si ya no hay nada que hacer, pongo ENABLE a uno para parar los motores por seguridad. Si esta dentro de la inclinacion de seguridad definida por mi , mediante el pin DIR hago que giren los motores en el sentido correcto , segun sea la salida de los PIDs positiva o negativa.Y por ultimo voy enviando pulsos al pin STEP de cada driver a la velocidad que nos indica el PID.

¿Pero como enviar los pasos a la velocidad adecuada sin detener demasiado el ciclo del programa?

Pues en mi caso ha sido mediante el uso de las interrupciones de arduino.  Hay varios tipos de interropciones posibles en arduino y es una cosa muy interesante y util , pero no son precisamente faciles de utilizar ni de entender. Por eso de momento no voy a explicarlas muy a fondo , esto daria para varios posts y provablemente lo haga en otra ocasion. Demomento solo decir que he usado interrupciones por timer. Para simplificarlo digamos que arduino tiene un contador (en verdad tiene varios y segun que arduino tendra mas o menos) que salta cada cierto tiempo definido por nosotros y entonces ejecuta una rutina. En esa rutina lo que hago es mandar un pulso al pin STEP. Entonces para variar la velocidad , simplemente hago que salte el contador mas o menos rapido. Contra mas rapido se ejecute la rutina ,mas pulsos por segundo recivira el driver y por lo tanto mas rapido giraran los motores,

Para este proyecto he usado arduino UNO porque era lo que tenia a mano. Arduino UNO tiene tres timers. El TIMER0 , el TIMER1 y el TIMER 2. El TIMER0 es usado por funciones como pueden ser millis() o delay() por ejemplo. Entonces como yo quiero seguir haciendo uso de esas funciones , no puedo usar el TIMER0 para los motores. Me quedan el TIMER1 que es de 16bits y el TIMER2 que es de 8bits.

Si solo hubiera querido que Manolo aguantara el equilibrio, con usar el TIMER1 que es el mas preciso me hubiera valido. Pero como yo queria que pudiera girar. Necesito los dos , uno por rueda. Como TIMER2 es de 8bits , aunque el TIMER1 sea de 16bits , tengo que usarlo como si fuera de 8 , ya que los dos tienen que ser iguales. Lo bueno hubiera sido usar otro arduino , como por ejemplo el Leonardo que tiene otro timer de 16bits extra. Asi podria haber usado los dos de 16 , teniendo un control sobre la velocidad mucho mas preciso., aunque no se hasta que punto se hubiera notado.

Como he dicho anteriormente , los timers activan una rutina que lo unico que hace es mandar un pulso al driver que controlan. Esto lo podria haber echo mediante digitalWrite , activo el pin STEP y seguidamente lo desactivo. Esto seria un paso de motor. El problema es que digitalWrite aunque nos soluciona mucho la vida ,porque es muy sencilla de usar , es “lenta”. En la inmensa mayoria de los casos esto no es grabe , pero en este que hemos de mandar tantos pasos por segundo hace perder mucho tiempo valioso al programa principal. La solucion a esto es cambiar el estado de los pines haciendo uso de  instrucciones de bajo nivel. Esto es muy rapido , pero otra vez ,al igual que con las interrupciones no es tan facil como hacer un digitalWrite. Pero vale la pena aprenderlo para situaciones como esta.

Por ultimo explicar como mover al robot. Si suponemos que el robot es totalmente simetrico. Si lo mantenemos a cero grados de inclinacion estara parado. En cambio si le decimos que se mantenga en por ejemplo 10º el robot se movera hacia adelante tratando de mantener esa inclinacion , si aumentamos la inclinadion deseada se movera mas rapido y si le damos una inclinacion negativa , se movera hacia atras. Yo no lo he echo directamente con los angulos. En mi caso tengo un PID de control de la velocidad visto en el anterior post ,que hace precisamente esto , varia el angulo deseado del robot segun la velocidad deseada.

Para girarlo , tendriamos que quitar velocidad de una rueda y esa misma velocidad añadirsela a la otra. Entonces el robot girara hacia el lado de la rueda mas lenta , mas rapido contra mayor sea la diferencia de velocidad entre las dos ruedas, pero siempre repetando que la suma de la velocidad de las dos ruedas sea igual a la velocidad que nos calculan los PIDs para mantenerlo equilibrado o a la velocidad deseada por nosotros.

El codigo de mi programa no tiene mucho mas. Lo que queda son funciones para el control bluetooth , la eprom para guardar las constantes de los PID…Pero nada importante para mantenerlo estavilizado. He de decir que me costo menos de lo esperado que Manolo mantuviera el equilibrio , pero me costo mucho mas que mantuviera el equilibrio sin que estuviera todo el rato desplazandose de un lado al otro. Hay que entender bien lo que hace cada paramentro de los PIDs y con paciencia ir provando hasta encontrar los mas adecuados , que variaran de un robot a otro a no ser que sean esactamente iguales.

Con esto, doy por terminado este proyecto y con lo aprendido creo que me voy a atrever a hacer a Dron Pepito , un quadcopter DIY.Los robots balancin y los drones tienen mucho mas en comun de lo que parece a primera vista. Con que en el siguiente post os presentare a Dron Pepito.

 

Manolo self balancing robot Part.3- El control PID

Ahora que ya sabemos a que angulo esta Manolo , lo siguente es equilibrarlo si este esta cayendo. Como ya explique , si Manolo cae hacia algun lado , los motores han de girar hacia ese mismo lado. ¿Pero a que velocidad? . Eso es lo dificil. Si no es suficiente Manolo caera porque no le dara tiempo de estavilizarse , si es muy alta Manolo sobre oscilara hasta que caera tambien.

En una primera aproximacion lo que podriamos hacer es simplemente si la lectura del angulo es positiva , darle a los motores una velocidad positiva y si la lectura es negativa , la velocidad seria negativa. Podriamos ir ajustando la velocidad hasta encontrar la mas adecuada , pero creerme que es imposible equilibrar asi a Manolo. El problema del cambio de angulo cuando cae no es lineal y por lo tanto no existe una velocidad concreta que pueda mantenerlo en pie. Como he dicho antes o caera por que no llega o por sobre  oscilacion.

Una forma un poquito mejor seria leer el angulo actual , compararlo con el deseado (cero grados para mantenerlo en equilibrio) y aplicar una fuerza proporcional a ese error. A esto lo llamariamos un control proporcional y seria algo asi.

  • Error = AnguloDeseado – AnguloActual
  • Velocidad = Kp * Error

Donde velocidad seria la velocidad que aplicariamos a los motores y Kp seria la constante proporcional que tendriamos que ajustar para encontrar el valor mas correcto.

Esta aproximacion es mucho mejor que la otra , pero tampoco es valida. Quiza podriamos llegar a equilibrar a Manolo durante un momento , pero al final siempre terminaria cayendo porque seguiria siendo muy inestable.

Lo correcto seria usar un control PID.El control PID se dice que es un mecanismo de control por realimentación de bucle cerrado. Esto es asi porque lo que hace es comparar el valor obtenido en la salida del PID con el deseado y este error es usado en la entrada para intentar minimizarlo en el siguiente ciclo.

Un PID consta de tres controles realmente , proporcional , integral y derivativo.Y tiene una formula tan bonita como esta.

d655dc7d5c02fbb8641e9a898cb406e1

El control proporcional ya lo vimos anteriormente y es la base para los otros dos. De echo aveces se puede prescindir del derivativo  o del integral , teniendo asi un control PI o un control PD , pero no podemos prescindir de este. Kp seria la ganancia de este control y contra mas alta sea esta mas rapido reaccionara , pero si lo sube mucho habra sobre oscilaciones.

El control integral es proporcional a la integral del error , haciendolo facil seria proporcional a la suma de los errores.Con este se intenta eliminar el error que sigue produciendose aun con el control proporcional. Se puede aplicar de varias formas , pero yo lo he usado asi.

  • ErrorActual = AnguloDeseado – AnguloActual
  • AcumulacionError = AcumulacionError + ErrorActual*TiempoCiclo
  • Integral = Ki *AcumulacionError*TiempoCiclo

Por ultimo quedaria el derivativo. El derivativo es proporcional a la derivada del error en el tiempo.Esto quiere decir que actua proporcionalmente a lo lo que varie el error. Si el error permanece estacionario , no hace nada. El derivativo ayuda a evitar las sobreoscilaciones. Normalmente se escribe asi.

  • ErrorActual = AnguloDeseado – AnguloActual
  • Derivativo = Kd*(ErrorActual-ErrorPasado)/TiempoCiclo
  • ErrorPasado = ErrorActual

Pero yo para la variacion del error es decir (ErrorActual-ErrorPasado) , he preferido utilizar solo el valor del giroscopio , anulando asi el ruido producido por el acelerometro. Conque quedaria asi.

  • Derivativo = Kd*VelocidadGiroscopio/TiempoCiclo

Con lo cual mi control PID sobre el angulo seria algo asi.

  • ErrorActual = AnguloDeseado – AnguloActual
  • Proporcional = Kp * ErrorActual
  • AcumulacionError = AcumulacionError + ErrorActual*TiempoCiclo
  • Integral = Ki *AcumulacionError*TiempoCiclo
  • Derivativo = Kd*(ErrorActual-ErrorPasado)/TiempoCiclo
  • ErrorPasado = ErrorActual
  • SalidaPID = Proporcional + Integral + Derivativo

Aqui “solo” quedaria encontrar los valores de las tres constantes Kp,Ki y Kd. Yo para poder hacerlo mas facil enviaba los valores por bluetooth y asi veia como afectaba al comportamiento de Manolo en tiempo real. La forma de encontrarlos fue el metodo ampliamene conocido y usado de prueba y error. Primero ajusto el control proporcional ,dejando los demas a cero ,hasta que empezaba a sobre oscilar y entonces correjia esas oscilaciones con el control derivativo. Por ultimo con el control integral terminaba de estavilizarlo. Aqui hay que perder mucho tiempo para encontrar los valores correctos.

He de decir que no existen solo unos valores correctos. Esto depende de la respuesta que busquemos. Conque no queda otra mas que ir provando y ver como responde.

Ajustando AnguloDeseado (si el robot fuera totalmente simetrico seria cero, si no habria que buscarlo ,pero deberia ser cercano a cero) y los parametros del PID ya podemos hacer que manolo este completamente estable en terreno llano.

AnguloDeseado variara segun la inclinacion del terreno y de si desplazamos su centro de grabedad con algo , por ejemplo colocandole un peso encima. Una forma de que Manolo encuentre su AnguloDeseado automaticamente ,independientemente del terreno o de si cambia su centro de grabedad, es poniendo otro control PID antes del PID que controla la estavilidad.

Este seria un control PID igual al anterior pero en vez de buscar el error en el angulo , buscaria el error sobre la velocidad de Manolo. Como entrada tendria la velocidad de Manolo y la salida seria el AnguloDeseado. Por ejemplo si nuestro AnguloDeseado es de 1º y Manolo se mueve hacia adelante ,el control PID sobre la velocidad leera el valor de la velocidad a la que se mueve y aumentaria el AnguloDeseado hasta que la velocidad fuera igual a cero y este seria el AnguloDeseado que aplicariamos a la entrada del siguiente PID. Esto tambien va bien para poder mover a Manolo despues. Si en vez de decirle al PID que buscamos una velocidad cero , le decimos que queremos una de velocidad de 10 , este ajustara el AnguloDeseado para que Manolo se mueva a esa velocidad.

Esto serian dos controles PID en cascada. Esto quiza sea la parte mas complicada del robot y ahora solo nos quedaria traducir la salida del PID en velocidad en los motores , pero eso sera en el siguiente post.

 

Manolo self balancing robot part.2- Obteniendo el angulo

El otro dia vimos el hardware de Manolo , pero sin el software Manolo no se levantaria del suelo y eso es lo que vamos a ver hoy.Aqui podeis ver y descargar mi codigo https://github.com/elchals/Manolo-self-balancig-robot-by-ElChals/tree/master. Decir que ni soy programador ni he estudiado nada que se le parezca , con que provablemente pueda hacerse mejor y mas claro , pero lo unico que puedo decir es que funciona.

¿Como consigue Manolo mantenerse en pie por el mismo incluso empujandolo?

Pues la teoria es muy facil , se le llama control del pendulo invertido. Pensar en cuando una persona intenta mantener una escoba equilibrada en nuestra mano. Suponiendo que la escoba fuera perfectamente simetrica tenemos que intentar mantenerla a cero grados de inclinacion y si por ejemplo se cae hacia adelante ,nosotros iremos hacia adelante tambien, hasta volver a conseguir esos cero grados , en cambio si cae hacia atras, iriamos hacia atras buscando otra vez mantener esos cero grados.

Pues Manolo funciona igual. Primero mide su inclinacion y si es cero no se mueve , si es positiva hace girar las ruedas hacia adelante y si es negativa hacia atras. ¿Que facil no?

Pues en verdad no lo es tanto , hacer esto tiene varios problemas importantes de por medio.El primero es:

¿Como sabemos la inclinacion de Manolo?

Manolo tiene una IMU (unidad de medicion inercial) , concretamente la MPU6050 y esta consta de un acelerometro y de un giroscopio ,ambos de tres ejes.

mpu-6050Un acelerometro como su nombre indica sirve para medir aceleraciones. Y direis , ya y esto a mi que… pues bueno ,haciendo un poco de memoria de cuando ibamos al  colegio recordareis que la tierra tenia una gravedad de 9.8m/s^2 ,o un 1G. Esto es una aceleracion y como el acelerometro puede medir la aceleracion en cada uno de sus ejes (es decir X , Y y Z) ,haciendo bastante mas memoria y haciendo uso de la trigonometria podemos obtener el angulo que buscamos. Ahora veremos un poco mejor como.

MPU6050_轴向定位En el caso de la imagen , estando la IMU totalmente paralela al suelo tendriamos una lectura de 1G en el eje Z y 0Gs en X e Y. Si movieramos la IMU los valores de estos vectores cambiarian segun la inclinacion y con trigonometria hayariamos el angulo.

No voy a dar una clase de matematicas aqui , si a alguien le interesa solo tiene que dar un pequeño repaso a la trigonometria como me toco a mi o simplemente fiaros de las siguentes formulas que voy a daros. Supongamos que leemos de la IMU las tres aceleraciones de los tres ejes. (La forma de obtenerlas varia dependiendo de la IMU que useis y habria que mirar el Datasheet para ver como hacerlo. Ahora no voy a explicar como lo hago yo con esta porque seria muy largo. Pero otro dia hago un tutorial. Mientras si estais interesados podeis mirar mi codigo). Las aceleraciones serian por ejemplo Acx , Acy y Acz. Apartir de ahi la forma de obtener el angulo seria usando estas formulas.

  • AnguloX = atan(Acy / sqrt(square(Acx) + square(Acz))
  • AnguloY = atan(Acx / sqrt(square(Acy) + square(Acz))

Normalmente las IMUs dan la aceleracion en radianes/segundo , conque si queremos el angulo en grados habriamos de multiplicar el angulo por (180/3.141592).¿Ycon esto ya tendriamos el angulo?.Pues si pero no , porque una cosa que no os he contado ,es que las medidas de los acelerometros casi siempre son erroneas. Esto es por dos razones.

Una es que los acelerometros tienen unas lecturas muy ruidosas , haciendo que estas sean poco utiles por si mismas. Y la otra es que si el robot cae hacia adelante por ejemplo , eso tambien es una aceleracion y esta nos provoca una lectura erronea. Conque demomento podemos decir que del acelerometro solo nos podemos fiar cuando el robot esta quieto.

Para calcular el angulo tambien podemos hacer uso del giroscopio. El giroscopio mide velocidad angular y tiene la ventaja sobre el acelerometro de que sus medidas son muy precisas. La forma de calcular el angulo apartir de la velocidad angular seria integrando las sucesivas lecturas. Es decir si sabemos que el robot estaba a 10º en el ultimo ciclo y estaba cayendo a 15º/segundo , sabemos que un segundo despues estara a 25º.

  • Angulo = AnguloAnterior + VelocidadAngular*TiempoPasado

Pero el giroscopio tambien tiene sus problemas. El principal es que en cada nuevo calculo siempre se acumula un pequeño error, que segun van pasando los ciclos se va aunmentando (esto se llama DRIFT) llegando a inutilizar la lectura. El otro es que no podemos saber de que angulo partimos para ir integrandole las proximas lecturas.

Como podemos ver cada dispositivo tiene sus pros y sus contras. Lo ideal seria fiarnos normalmente del giroscopio y ir “refrescando” las lecturas con el acelerometro sobre todo cuando este no varie mucho.

Fusionando acelerometro y giroscopio.

Hay varias formas de fusionar las lecturas del acelerometro y del giroscopio para obtener unas lecturas lo mas estables y reales posibles.

Las dos mas usadas serian el filtro de Khalman y el filtro complementario. El MPU6050 tambien dispone de un algoritmo interno llamado DMP que lo hace por nosotros y nos ahorra tenerlo que hacer nosotros por software. Yo no lo he querido usar por hacerlo de una forma mas “casera” , pero seguramente mejoraria aun mas los resultados obtenidos.

El filtro Khalman es un filtro muy importante usado incluso en los satelites , tiene la ventaja de ser muy bueno bien usado , pero la desventaja es que tiene un mayor coste computacional comparado con el filtro complementario y que si queres entenderlo has de ser un muy buen matematico y cuando digo muy es MUUUUUUUYYYYY.  Hay librerias para arduino que te permiten usarlo sin llegar a entender realmente lo que hace , eso que no me gusta y junto a que no obtuve mucho mejor resultado que con el filtro complementario ,hizo que al final me decantara por el filtro complementario.

El filtro complementario es muy facil de entender y su coste computacional es mas que escaso. Y la forma de usarlo seria esta:

  • Angulo = (1-K) * AnguloGiroscopio + K * AnguloAcelerometro

Donde K seria una constante que nos idicaria la importancia que le queremos dar al acelerometro y al giroscopio. Como nos interesa hacer mas caso al giroscopio que al acelerometro , K suele ser muy baja , en mi caso 0,01. y quedaria asi.

  • Angulo = 0,99 * AnguloGiroscopio + 0,01 * AnguloAcelerometro

Con esto porfin tenemos un angulo relativamente libre de ruidos y usable.Una cosa que introduce mucho ruido en las lecturas son las vibraciones de los motores. Estas las detecta el acelerometro y son fatales. Yo reduje mucho este problema poniendo espuma entre la base del robot y el acelerometro , reduciendo asi mucho las vibraciones y mejorando la lectura del angulo. Otra cosa que yo no use y tendria que haberlo hecho , porque hubiera sido muy buena para reducir el ruido de los motores ,es que el MPU6050 tiene un filtro paso bajos configurable internamente , usando este y configurandolo bien se puede filtrar mucho el ruido provocado por las vibraciones.

Con esto ya tendriamos el primer gran problema resuelto , la obtencion del angulo de Manolo respecto del suelo. El siguiente problema es saber cuanto y a que velocidad hay que mover los motores para corregir la caida del robot, pero eso ya sera en el siguiente post , de momento ya he soltado suficiente por hoy.

 

Manolo el robot equilibrista. Part.1

Pues esto es con lo que he estado entretenido esta ultima temporada. Se llama Manolo y es un robot equilibrista. Basicamente lo que hace es mantener el equilibrio sobre dos ruedas. Viene a ser como un pequeño Segway , esos vehiculos de dos ruedas tan chulos que casi siempre llevan a encima a un policia. Aqui va un video de Manolo para que entendais de lo que hablo.

Manolo esta echo de plastico , parte impreso con la impresora 3D y parte cortado de una tabla de cortar carne de los chinos  con la CNC. La estructura esta inspirada en el B-Robot de JJulio. Para moverlo he usado dos motores paso a paso NEMA17 porque era lo que tenia , pero tambien se podria haber echo con unos motores DC normales lo suficientemente potentes y igual hubiera sido mas facil el codigo. Eso si , para ir bien estos tendrian que tener encoders para saber si el robot se esta moviendo y a que velocidad , aunque se puede mantener el equilibrio sin estos tambien pero con algo mas de movimiento.

En cuanto a la electronica ,el cerebro es un Arduino Uno , aunque si lo hubiera tenido a mano hubiera sido mejor un Arduino leonardo porque este tiene dos timers de 16 bits y el arduino solo uno y asi hubiera podido usar uno por rueda. Con el arduino Uno me he visto obligado a usar los de 8 bits perdiendo algo de precision en su control. Aunque no se hasta que punto se hubiera notado.

Despues hay una placa que me hice que contiene los dos drivers de los motores Pap ,unos Pololu drv8825 ,junto a sus condensadores de desacoplo y sus jumperes para configurar los micropasos. Yo los he usado a 1/16 , obteniendo 3200 pasos por vuelta y asi hacer que su movimiento sea muy suave.

Conexion del driver.

En mi caso el microcontrolador es el arduino y por el pin Step se le mandan los pulsos de los pasos. En Dir segun este en HIGH o en LOW el motor girara en un sentido o en otro. Enable conecta o desconecta el motor y mediante los pines M0,M1 y M2 se configuran los micropasos.

En esta placa tambien hay un divisor de tension con el que mido la tension de la bateria en todo momento y cuando esta baja del umbral que yo le marco ,me avise mediante un led parpadeando y desconecte los motores.  Esto es necesario porque uso una bateria LiPo y estas baterias si se descargan mucho nos las cargamos. Como la bateria es de 11,1 voltios no se puede conecar directamente a una entrada de arduino y por eso es necesario el divisor de tension ,con este divisor reducimos el voltaje a 1/3 y asi me aseguro que siempre este por debajo de 5 voltios.Despues en el arduino multiplico la entrada por tres y ese es el voltaje real de la bateria.La conexion es muy facil y seria asi.

 

Voltimetro con arduino.
Voltimetro con arduino.

Y la funcion que lee el voltaje es esta:

void compVoltaje(){
float voltaje = analogRead(VOLTAJE);
voltaje = voltaje * (5.0 / 1023.0)*3;
if (voltaje<VOL_MIN)
{
while(1)
{
delay(1000);
digitalWrite(LED,HIGH);
delay(100);
digitalWrite(LED,LOW);
delay(100);
digitalWrite(LED,HIGH);
delay(100);
digitalWrite(LED,LOW);
}
}
}

 

Si el voltaje esta por encima del voltaje indicado en VOL__MIN sigue el programa normalmente , si esta por debajo parpadea el led y se queda ahi.

Despues tengo un modulo bluetooth para poder configurar el robot desde el telefono y para poder dirgirlo. Es un HC-05 y es muy facil de conectar tambien. Se puede alimentar a 5 voltios , pero la señal que reciva por el puerto serie RX tiene que ser de 3,3 voltios. Como el arduino funciona a 5V hay que colocar ahi otro divisor de tension. Yo no lo conecto a los pines RX y TX del arduino para poder seguir usando ese puerto serie para depurar el programa. Lo conecto a los pines 10 y 11 , pero para leerlo tengo que crear un puerto serie virtual mediante software serial. Para usar el modulo por primera vez hay que configurarlo mandandole unos comandos especiales atraves de un programa. No voy a explicarlo de momento que si no esto se va a hacer muy largo. Si a alguien le interesa es facil de encontrar en la red.

HC-05 en arduino

 

Y por ultimo lleva un MPU6050 que contiene un acelerometro y un giroscopio. Se comunican mediante i2c , es decir los pines A4 y A5 del arduino. Su conexion es muy sencilla ,pero tambien funciona a 3,3V, cuidado con esto.

mpu-6050

Hasta aqui los componentes de Manolo , en el siguiente post veremos como funciona todo esto.

 

Mi nueva impresora 3D lista para imprimir.

Pues nada , aqui va una nueva entrada. Esta vez solo queria enseñaros mi nueva impresora 3D terminada. Ya hacia tiempo que solo me faltaban cuatro tonterias para tenerla lista y porfin he tenido ganas de terminarla.

DSC_0478

Una impresora 3D auque parezca una cosa espectacular , en el fondo no es mas que una CNC como podria ser la Makertrastos que hice anteriormente , pero en vez de tener acoplada una fresadora que quita material , lleva un cabezal que funde plastico y capa a capa va imprimiendo el diseño que nosotros le indiquemos.

Esta vez no voy a explicar nada sobre su construccion , ya que no he inventado nada nuevo. Hay tanta informacion en internet de diferentes modelos de impresoras que yo creo que no vale la pena complicarse la vida inventando una nueva.

Mi impresora esta basada en una Prusa i3 , concretamente en una prusa rework con alguna modificacion para adaptarla a materiales que tenia mas a mano. Si a alguien le interesa aqui se explica perfectamente como montar una http://reprap.org/wiki/Prusa_i3_Rework_Introduction. No hace falta tener grandes conocimientos de nada para montarse una , aunque ser un poco manitas no va mal y saber usar Arduino tambien biene bien para configurarla por primera vez.

Mi marco no es de aluminio como el de la original , yo lo hice de metacrilato que corte con la Makertrastos y le hice unos refuerzos para que ganara rigidez. La base de la cama caliente tambien la corte con la CNC en DM. La piezas de plastico verdes son piezas impresas con otra impresora y las compre en Ebay (ahora las podria imprimir yo). Los motores son unos paso a paso pequeños , unos NEMA17 concretamente. Y el cerebro de todo esto es un arduino Mega al que se le carga un firmware que encuentras facilmente en la red , junto con Ramps.

Ramps , es una shield para arduino , pensada especialmente para las impresoras 3D. Pero basicamente no tiene mas que los componentes necesarios para manejar hasta 6 drivers de motores Pap y poco mas. Con lo que no seria muy dificil hacer una placa igual , pero se encuentra por precios tan reducidos que no vale la pena ni pensar en hacerse una.

La forma de usarla es tambien parecida a la de una fresadora Cnc. Primero se hace el diseño con cualquier programa tipo autocad , en mi caso Rhinoceros y se exporta como un archivo .stl . Despues se carga en un programa que nos cree las trayectorias de cada capa y donde tambien se configuran las caraceristicas de impresion segun lo que vayamos a imprimir y del tipo de plastico que vamos a usar. En mi caso de momento uso el Cura. Un software gratuito muy facil de entender.

Por ultimo solo decir que una impresora 3D no es como una impresora de papel de enchufar y imprimir. La impresora necesita de continuos ajustes segun lo que se imprima. No es lo mismo imprimir con PLA que no necesita que la base llamada cama caliente funcione y tiene una temperatura de impresion de unos 200º que imprimir en ABS que si necesita de una temperatura concreta en la cama caliente para una buena impresion y una temperatura de impresion mas elevada. La impresora aveces tambien se puede obstruir y mil problemas mas. Con lo que quiero decir que hay que saber un poco lo que se hace. Cada plastico tiene sus caracteristicas y hay que aprender como configurar la impresora en cada ocasion. Las primeras impresiones no suelen salir muy bien y poco a poco se le coje el truco y van saliendo mejor.

Aqui os dejo un pequeño video imprimiendo un futuro proyecto.