Robot Hexapodo

[willynovi]

Sin embargo, me surge una duda: Como sincronizar el movimiento de las patas? O sea, para generar una caminata debemos mover las patas sincronizadamente. Cada pata describe un movimiento que esta desfasado respecto al de las demas (esa fase depende de cuantas patas movamos a la vez). Como se les ocurre que podemos implementar esto?

Estuve viendo que para mantener el robot estable, sin contar con sensores, es necesario tener al menos 3 patas en el suelo.
Se puede hacer entonces tres tipos de movimientos de patas dependiendo de la velocidad/suabidad del movimiento.

Mover de a una sola patas, menor velocidad -> mas suabidad y armonioso el movimiento
Mover de a dos patas
Mover de a tres patas, mayor velocidad -> menos suabe el moviemiento

La cinemática inversa ya la tengo en papel, luego la reviso y la adjunto, aunque no es muy dificil.

Lo que estoy pensando es que controlemos la posición del centro de gravedad del robot y luego el programa resuelve solo la posición o movimiento de cada pata.

Un contador sirve como referencia de sincronismo, un "clock universal" para las patas. Cada pata recibe como parametro el movimiento que tiene q describir (mediante una funcion matematica, probablemente curvas bezier) y una fase. Entonces la posicion de cada pata en un instante dado se calcula como la funcion correspondiente al valor del contador mas el desfasaje.

Si, yo creo que es la forma, se necesita la variable tiempo, tambien si fuera necesario implementar un PID.

[willynovi]

Cada pata recibe como parametro el movimiento que tiene q describir (mediante una funcion matematica, probablemente curvas bezier)

Como no estaba al tanto de las curvas Bezier, es decir, las conozco si, pero no como se generaban  , me puse un poco a ver como es el tema y encontré un código que puede resultar, al menos yo voy a empezar con eso como para introducir algún movimiento.

Código: [Seleccionar]

/*Código para generar una curva cúbica de Bézier*/
 
typedef struct
{
    float x;
    float y;
}
Point2D;
 
/*
cp es una matriz de 4 elementos donde:
cp[0] es el primer punto, o P0 en el diagrama de abajo
cp[1] es el primer punto de control, o P1 en el diagrama de abajo
cp[2] es el segundo punto de control, o P2 en el diagrama de abajo
cp[3] es el punto final, o P3 en el diagrama de abajo
t es el valor del parámetro, 0 <= t <= 1
*/
 
Point2D PointOnCubicBezier( Point2D* cp, float t )
{
    float   ax, bx, cx;
    float   ay, by, cy;
    float   tSquared, tCubed;
    Point2D result;
 
    /* cálculo de los coeficientes polinomiales */
 
    cx = 3.0 * (cp[1].x - cp[0].x);
    bx = 3.0 * (cp[2].x - cp[1].x) - cx;
    ax = cp[3].x - cp[0].x - cx - bx;
 
    cy = 3.0 * (cp[1].y - cp[0].y);
    by = 3.0 * (cp[2].y - cp[1].y) - cy;
    ay = cp[3].y - cp[0].y - cy - by;
 
    /* cálculo del punto de la curva en el valor del parámetro t */
 
    tSquared = t * t;
    tCubed = tSquared * t;
 
    result.x = (ax * tCubed) + (bx * tSquared) + (cx * t) + cp[0].x;
    result.y = (ay * tCubed) + (by * tSquared) + (cy * t) + cp[0].y;
 
    return result;
}
 
/*
 ComputeBezier llena un array de Point2D con los puntos de la curva
 generados de los puntos de control cp.
 Esta llamada debe ubicar suficiente lugar de memoria para el resultado
 la cual es del tamaño <sizeof(Point2D) numberOfPoints>
*/
 
void ComputeBezier( Point2D* cp, int numberOfPoints, Point2D* curve ) {
    float   dt;
    int   i;
 
    dt = 1.0 / ( numberOfPoints - 1 );
 
    for( i = 0; i < numberOfPoints; i++)
        curve[i] = PointOnCubicBezier( cp, i*dt );
}


Entonces lo que tendríamos que hacer es definir el punto de la nueva posición del pié y los dos puntos de control para generar la curva de Bezier.
Los puntos de control por el momento pienso que sean la simple elevación de posición actual y posición nueva. Mas adelante veo si se puede ajustar para lograr algún otro tipo de movimiento. De todas formas los puntos de control tienen que ser calculados a partir de alguna simple ecuación de los puntos actual y nuevo.

Este "algoritmo" es simple, quizás no es el mas rápido o que consuma menos memoria, pero es un inicio. Aunque tengo que probar como funciona 

Fuente: Curva de Bézier - Wikipedia

[gera]

Te paso una funcion mas simple

Código: C

1. float bezier (float p0, float p1, float p2, float t) {
2.   return sq(1-t)*p0 + 2*t*(1-t)*p1 + sq(t)*p2;
3. }

Las variables p0, p1 y p2 representan las componente sobre el eje "y" de cada punto. La componente sobre el eje "x" son 0, 0.5 y 1 respectivamente. Entonces la funcion:

Código: [Seleccionar]

bezier(0, 2, 0,t);Corresponde a una curva bezier cuadratica compuesta por los puntos (0,0)  (0.5,2) (1,0)

Yo creo que con eso es suficiente para generar los pasos del robot, ya que te permite levantar la pata cuanto quieras, y si queres hacer un paso mas largo (o corto), basta con tomar intervalos mas largos entre punto y punto.
Espero que te sirva, saludos!!

[willynovi]

Claro, yo habia pensado de entrada en usar una cuadrática, no tiene mucho sentido una cúbica, pero fue un código que lo vi lindo 

La función que presentas vos es la básica y aplicada a un paso unitario, así entiendo 

Habría que, según la suavidad del movimiento, iterar según cuantos puntos intermedios y calcular cada vez, por cada punto intermedio, la función Bezier con la entrada t.

Voy a intentar adaptar la que tengo a una cuadrática y tambien probar la que pusiste, luego sacaré conclusiones.

Lo que estoy notando es que me siento un poco oxidado con los razonamientos matemáticos 

[gera]

Willy, yo creo que va a ser mejor usar una tabla, asi no desperdiciamos tiempo haciendo siempre los mismos calculos. Y si queremos variar la altura o longitud del paso, multiplicamos por un escalar y listo.

Bueno, ya tengo algo de codigo hecho. Funciona asi:
Recibe 20 caracteres por el puerto serie, los almacena en el array servo_duty[], y utiliza esos valores para calcular el duty de las 20 señales PWM.
Como ven en el archivo hexapodo.h, hay un array con los pines de cada uno de los servos. Entonces si hago:

Código: [Seleccionar]

output_high(servo_pin[3]);

estoy poniendo el alto el pin del cuarto servo (porque comienza de 0).
Para generar las 20 señales PWM, pongo todos los pines en alto, luego incremento un contador cada 50us con el timer1, y cuando ese contador se iguala al duty del servo, lo pongo en bajo. Cuando el contador llega a 400 (20ms), vuelve a empezar.
Esto me da una resolucion de (2ms-1ms)/50us=20 posibles valores. Los servos q compre pueden recorrer 60º. Entonces la resolucion es 60º/20=3º. Creo que es aceptable no? Aunque puede mejorarse, ya que hay 18ms que estamos desperdiciando, puesto que siempre estaran en nivel bajo.

Bueno, espero sus sugerencias y opiniones  

Hexapodo.c

Código: C

1. #include <18F4550.h>
2.  
3. #fuses HSPLL,NOWDT,NOPROTECT,NOLVP,NODEBUG,PLL5,CPUDIV1,NOMCLR
4. #use delay(clock=48000000)
5. #use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7)
6.  
7. #include "hexapodo.h"
8.  
9. int8 servo_duty[20]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
10. int16 counter=0;
11.  
12. #INT_TIMER1
13. void timer1_isr(){
14.    int i=0;
15.    
16.    for(i=0;i<20;i++){
17.       if(counter==(int16)servo_duty[i])
18.          output_low(servo_pin[i]);
19.    }
20.    
21.    if(++counter>=400)    //400*50us = 20ms (wave periodo required for drive a servo)
22.       counter=0;
23.  
24.    set_timer1(PRELOAD_TMR1);
25. }
26.  
27.  
28. #INT_RDA
29. void serial_isr(){
30.    char data;
31.    int i=0;
32.    
33.    if(kbhit()){
34.       while(data=getc() != '\r' && i<20){
35.          servo_duty[i]=data;
36.          i++;
37.       }
38.    }
39. }
40.  
41.  
42. void init(){
43.    // Digital IO
44.    set_tris_a(0b110011);   //V_SENS,NC,LED_G,LED_R,NC,NC
45.    set_tris_b(0b11110000); //PGD,PGC,NC,NC,S,S,S,S
46.    set_tris_c(0b10001000); //RX,TX,S,S,VUSB,S,S,S
47.    set_tris_d(0b00000000); //S,S,S,S,S,S,S,S
48.    set_tris_e(0b000);      //S,S,S
49.    
50.    // Timer1
51.    setup_timer_1 ( T1_INTERNAL | T1_DIV_BY_1 );
52.    set_timer1(PRELOAD_TMR1);
53.    
54.    // Interrupts
55.    enable_interrupts(INT_TIMER1);
56.    enable_interrupts(INT_RDA);
57.    enable_interrupts(GLOBAL);
58. }
59.  
60.  
61.  
62.  
63.  
64.  
65. void main(){
66.    delay_ms(10);
67.    init();
68.    
69.    while(TRUE){
70.       output_low(LED_R);
71.       delay_ms(250);
72.       output_high(LED_R);
73.       delay_ms(250);
74.    }
75. }

Hexapodo.h

Código: C

1. #ifndef _HEXAPODO_H_
2. #define _HEXAPODO_H_ 1
3.  
4.  
5. #define LED_R PIN_A3
6. #define LED_G PIN_A2
7.  
8. #define SERVO1 PIN_B3
9. #define SERVO2 PIN_B2
10. #define SERVO3 PIN_B1
11. #define SERVO4 PIN_B0
12. #define SERVO5 PIN_D7
13. #define SERVO6 PIN_D6
14. #define SERVO7 PIN_D5
15. #define SERVO8 PIN_D4
16. #define SERVO9 PIN_C5
17. #define SERVO10 PIN_C4
18. #define SERVO11 PIN_D3
19. #define SERVO12 PIN_D2
20. #define SERVO13 PIN_D1
21. #define SERVO14 PIN_D0
22. #define SERVO15 PIN_C2
23. #define SERVO16 PIN_C1
24. #define SERVO17 PIN_C0
25. #define SERVO18 PIN_E2
26. #define SERVO19 PIN_E1
27. #define SERVO20 PIN_E0
28.  
29.  
30.  
31. #define PRELOAD_TMR1 64943
32.  
33.  
34. int16 servo_pin[] = {SERVO1,SERVO2,SERVO3,SERVO4,SERVO5,SERVO6,SERVO7,SERVO8,SERVO9,SERVO10,SERVO11,SERVO12,SERVO13,SERVO14,SERVO15,SERVO16,SERVO17,SERVO18,SERVO19,SERVO20};
35.  
36. void init();
37.  
38. #endif

[willynovi]

Gera, entiendo lo de la matriz, incluso el código que proponia tambien llena una matriz que puede hacerse tambien unitario y luego al momento de aplicarlo lo multiplicamos por un coeficiente. Aunque tu solución lo que calcula es la altura y los puntos actual y nuevo tienen que tener la misma altura.

De todas formas, por el momento son cálculos y especulaciones en el aire, voy a ver si estos días puedo fabricarme una pata y llegar a mover los servos en alguna dirección 

Con respecto a tu programa, creo que puedes mejorar incluso un poquito el tiempo de interrupción, yo lo probé con 40us y no me dió problemas, no probé con menos todavia.

Creo que tienes un error en los cálculos de la resolución, porque el pulso alto del servo es 2ms, entonces -> 2ms/50us=40, por lo que si tienes servos de 60° te da una resolución de 1,5°. Mucho mas aceptable de lo que pensabas 
Creo que ese valor 20 que calculas lo usas en el programa, quizas debas cambiarlo.
Lo que no veo son los 0,5ms iniciales, o eso ya lo tienes incorporado en el valor del duty de cada servo?

El otro punto que le veo a diferencia de mi programa es que tu siempre mantienes el tiempo de la interrupción en 50us incluso posterior a que terminó el pulso de 2,5ms. Es decir, de 2,5ms hasta 20ms tienes 17,5ms que no es necesario tener interrupciones por el timer.
Lo que yo hago es modificar el valor del timer para que desborde a 17,5ms y así cae la interrupción cuando deben ponerse nuevamente los pines de los servos en alto.
Similar con el 0,5ms iniciales donde no es necesario modificar el estado de los pines.

De todas formas, si así te funciona bien pues no toques nada.

Siempre me ha quedado la duda de que si tener interrupciones afecta en algo al funcionamiento del microcontrolador, mientras que no afecte al programa obvio, me refiero solo al microcontrolador sin tener en cuenta el desarrollo del programa.

Veo que tu comunicacion la atiendes por interrupción, lo has probado ya? tener la interrupcion cada 50us afecta en algo?

[Miquel_S]

¡ Y queriais que yo aportara algo ! Ya ni se donde me perdi. 
El que sabe sabe y el que no ...........

Saludos!

[gera]

Creo que tienes un error en los cálculos de la resolución, porque el pulso alto del servo es 2ms, entonces -> 2ms/50us=40, por lo que si tienes servos de 60° te da una resolución de 1,5°. Mucho mas aceptable de lo que pensabas 
Creo que ese valor 20 que calculas lo usas en el programa, quizas debas cambiarlo.

Si no me equivoco, es 1ms, porque el pulso minimo es de 1ms y el maximo de 2ms, o sea que la informacion util se encuentra en ese intervalo de 1ms.

Lo que no veo son los 0,5ms iniciales, o eso ya lo tienes incorporado en el valor del duty de cada servo?

Tenes razon! Inicializo todos los servos en 0, cuando debería ser 30 (30 interrupciones por 50us cada una = 1.5ms)

El otro punto que le veo a diferencia de mi programa es que tu siempre mantienes el tiempo de la interrupción en 50us incluso posterior a que terminó el pulso de 2,5ms. Es decir, de 2,5ms hasta 20ms tienes 17,5ms que no es necesario tener interrupciones por el timer.
Lo que yo hago es modificar el valor del timer para que desborde a 17,5ms y así cae la interrupción cuando deben ponerse nuevamente los pines de los servos en alto.
Similar con el 0,5ms iniciales donde no es necesario modificar el estado de los pines.

Es una buena idea, asi el timer no esta interrumpiendo a lo tonto Es mas, tambien hay 1ms inicial que siempre se mantiene en alto. O sea que podemos hacer lo siguiente:
-Poner todos los pines en alto
-Configurar el timer para q interrumpa dentro de 1ms.
-Luego que interrumpa cada 10us unas 100 veces, y alli modulamos nuestra señal (total 1ms)
-Por ultimo que vuelva a interrumpir a los 18ms (mas los 2ms anteriores tenemos 20ms)

Veo que tu comunicacion la atiendes por interrupción, lo has probado ya? tener la interrupcion cada 50us afecta en algo?

Todavia no lo pruebo, cuando escribi estas lineas no tenia la placa a mano. En caso de haber problemas, podria implementar un buffer para recibir los datos por el puerto serie, y luego vaciarlo en el bucle principal del programa. O bien, levantar un flag cuando caiga la interrupcion del timer, y cambiar el estado de los pines en el bucle principal.
Ya voy a hacer pruebas y vemos que sale
Saludos!!

[willynovi]

Creo que todos los servos, al menos los analógicos, trabajan con un pulso de ancho mínimo 0,5ms y máximo 2,5ms y la frecuencia de período 20ms.

Aunque puede variar muy poco según el fabricante en mi caso.
Los que tengo yo según el fabricante Hextronic el pulso debe ser entre 0,45ms y 2,45ms.

Supongo que la mejor forma es ensayarlos y ver como reaccionan.

Con esto en mente lo que hice fue poner los tiempos como #define y así resulta mas facil ajustar el programa.

[willynovi]

Averigue un poco, de todas formas no he llegado a conclusiones finales 
Aparentemente los servos, al menos la mayoria, trabajan con un pulso centrado en 1,5ms y el periodo de 20ms.

Ahora estoy teniendo otro problema y recurro a la ayuda de todos aquellos que programen en Visual C, y particularmente con Forms.

Resulta que estoy tratando de incorporar un reloj al programa, de hecho lo hice, la idea es tener una base de tiempo para realizar los movimientos sincronizados.

Pero no logro incorporar la clase Stopwatch desde el diseñador gráfico.
Lo hago manualmente, compilo el programa, y funciona correctamente.

El problema es cuando vuelvo al diseñador y luego al código resulta que lo que agregué manualmente me lo ha borrado.

[gera]

Willy, hasta donde se, la gran mayoria de los servos admiten un duty de 1ms como minimo, 2ms como maximo, 1.5 como medio, y un periodo total de 20ms. Pero varia entre marca y marca.

Respecto al reloj, no podes usar el reloj del sistema y calcular un modulo para ir tomando intervalos?
Yo se que buscas hacerlo con un objeto, pero tal vez esto te sirva:
http://msdn.microsoft.com/en-us/library/system.timers.timer.interval.aspx
http://stackoverflow.com/questions/2150291/how-do-i-measure-a-time-interval-in-c

Saludos!!

[willynovi]

Gera, gracias por los link, no habia dado con ellos porque habia encontrado lo del Stopwatch y me quede renegando con ese jejejeej

Con el Stopwatch logro una resolución de 1ms, pero parece que con los ejemplos que me diste se puede lograr una resolución del orden de 1us.

El tema de no usar un objeto no es tan grave, mientras que cumpla el objetivo 

[willynovi]

Estuve avanzando un poco, pero muy poco 

El tema del tiempo digamos que funciona, he puesto un contador y pareciera que se tarda en volver entre 5ms y 15ms, aunque no se bien si es un error como he leido por ahí o porque mi programa se toma esos tiempos.
Hoy estoy usando las funciones QueryPerformanceFrequency() y QueryPerformanceCounter() en C en el soft host de la PC, de un enlace que pasó Gera.
La idea que tengo es combinar ese valor con Stopwatch y comparar los valores.

Tambien tenia idea de que sea el PIC el que marque el sincronismo enviando el dato por el USB, pero me estoy adelantando, cuando tenga algo en movimiento veremos como implementar esto.

Estoy con la parte trigonométrica por ahora, en base a tener el punto actual y el destino calculo el intermedio para poder armar la curva bezier cuadratica y de ahí obtener los puntos para armar la cinematica inversa.

Pero me adelanto un poco, como estoy pensando que el hexapod tenga algún tipo de autonomia, que micro puede ser el adecuado para utilizarlo como cerebro del robot?

Supongo que los puntos fuertes son velocidad de proceso y capacidad para realizar operaciones matemáticas.

Algún PIC32 podría ir? Cual me recomiendan?
O quizas estoy exagerando y alguno de la linea 18F me alcanza?

La idea es ir viendo como armarme de algo, alguien tiene experiencia en algo similar?

[AKENAFAB]

Hola!

Hace tiempo realice una aplicacion en visual basic y tambien requeria de cierto periodo para comunicar.
Lo deje porque no era preciso.
Todo lo de los tiempos lo controle con el uC.

A mi parecer te podría servir un pic18F(medianamente complejo) o un 24H/E  (16MIPS/40MIPS) que es más veloz y podrás procesar a 16bits.

Excelentes avances!!

Saludos!

http://ww1.microchip.com/downloads/en/devicedoc/39754h.pdf

[MGLSOFT]

Willy, estas queriendo controlar los servos directamente con temporizaciones desde el programa en PC ???
Es bastante dificil lograrlo, ya que las tareas en multitareas siempre tendran mayor prioridad de procesador que cualquier programa del usuario, por lo tanto veo bastante dificil que puedas lograr tiempos con precision del milisegundo...