Proyecto: Placa controladora de 28 servos para robot humanoide

[Manofwar]

Hola a todos.

Leyendo el subforo de Robótica vi hace unas semanas un video de unos bots humanoides luchando entre ellos. Me dejaron alucinando y no he podido resistirme a tener uno de estos juguetes.

Buscando por la red encontré varios bots comerciales a unos precios desorbitados, por lo que decidí diseñar uno desde cero para abaratar
costes.

Conseguidos unos servos, bastante bien de precio en China, ahora faltaba su control. Así que a diseñar una placa capaz de controlar un número elevado de servos.

En principio, mi intención era controlar 16 servos, suficiente para un humanoide tipo Robonova. Metido en faena pense que quizas en un futuro quisiera unas pinzas, garras o algo así como manos para mi bot o aumentar los grados de libertad, asi que había que aumentar el número de servos a controlar.

Tras ver las posibilidades de los pics facilitos de conseguir me decidí por un 16f877 a 20Mhz. Con los puertos que tiene hay para controlar
24 servos y tener comunicación por hardware con el exterior.Aunque esto último he tenido que sacrificarlo para conseguir una mayor velocidad de la rutina de control, redundando en una mayor precisión en el posicionamiento de los servos.

Bien, las características de la placa controladora son:

1. Refresco de las posiciones de los 24 servos cada 20mS.
2. Rango de trabajo desde 500us (-90º) hasta 2500us (+90º).
3. Control de posicion y velocidad independiente de cada servo.
4. Lectura de la posición actual de cada servo.   
5. Comunicación por usart e i2c, para controlar los servos desde un pc o desde un PIC.

En este último punto es donde me encuentro ahora; la comunicación.

La usart por software la tengo implementada, eso sí a 115200, ya que a una velocidad inferior, según mis calculos, me sería dificil ordenar
un cambio de posición de todos los servos a la vez cada 20ms, como es mi intención.

Aquí es donde solicito vuestra ayuda para diseñar un protocolo con el menor número de bytes posible por orden.

Mi idea es esta:

Mover-> M(S,P1,P2,V)

- Donde M indica que vamos a mover los servos
- S es el número de servo a mover, de 1 a 24. (1 byte)
- P1 y P2. Estos 2 bytes son la posición a la que debe ir el servo, ejemplo: 0x5DC = 1000us
- V la velocidad a la que se movera el servo (1byte), ejemplo: un valor de 10 correspondería a 10us cada 20ms.

Esta serie de bytes debería repetirse para cada servo.

Así que si queremos ordenar a los 24 servos que inicien un movimiento a la vez, tendríamos que enviar algo como esto:

M(S,P1,P2,V,S,P1,P2,V,S,P1,P2,V,etc...)

No se si esta sería la mejor forma también para el i2c, puesto que desconozco casi todo de este bus.

Bueno, espero haberme expresado con claridad y que podáis ayudarme.

PD: En breve subire el firmware y una simulación en Proteus, para que la vayais probando y entro todos consigamos una buena placa.
Ahora que recuerdo: ¿Alguien me puede decir si hay software para cinemática de robots?

[Manofwar]

Como lo prometido es deuda, aquí os dejo la simulación en proteus y su firmware.

Los servos se mueven del siguiente modo:

NN,PPPP. Donde NN es el número de servo a mover
PPPP. La posición a donde debe ir el servo

ejemplo:

05,0500. Con esto moveremos el servo número 5 a la posición 500us(-90º)

Ahora mismo la comunicación sólo interpreta 7 bytes, el equivalente a mover un sólo servo cada 20ms.

Si queréis dar una orden para varios servos a la vez, sería del siguiente modo:

05,050010,25007,2000 = servo 5 a 500us (-90º), servo 10 a 2500us (90º), servo 7 a 2000us (45º)

Para que el pic acepte la orden se debe pausar la simulación, escribir la orden en el puerto serie virtual y despúes pulsar el play de la simulación. Con esto conseguimos que cada 20ms el pic lea una orden de posicionamiento.

Se que es muy rudimentario pero este protocolo sólo es para pruebas. En la versión definitiva haré un software de control más depurado.

En el próximo firmware que suba espero tener los algoritmos de velocidad terminados. Ahora mismo estoy buscando un error en ellos que me vuelven locos los servos. Se posicionan todos bien y a la velocidad individual que le digo, pero cuando el último llega a su posición todos al unísono comienzan a bailar un regetón  que no sé de donde viene.

Ahora a probarlo y a detectar errores

[Manofwar]

Hola LordLabre, como me has sugerido te contesto aquí.

Lo estoy haciendo en ensamblador ya que me permite más control sobre todos los procesos.

[LordLafebre]

Hola:

gracias por aceptar la sugerencia Manofwar, solo una preguntilla mas, ya probaste con los servos en la realidad?

[Artifox]

Hola Manofwar, quize escribir ayer pero se me borro todo antes de enviar Primero que nada dejame decirte que se ve muy bien tu controladora baje los archivos los corri y me parece mejor que la mia jeje...  Creo que la diferencia radica en que yo basicamente atiendo un servo a la vez, es decir haciendo el control secuencialmente con lo que puedo controlar 8 servos tranquilamente y para logra hacer 16, controlo 2 servos a la vez en paralelo obteniendo 16 en total.



Y veo que tu haces los 24 servos en paralelo... yo queria hacer lo mismo pero no se me ocurrio como hacerlo. Y probablemente sea porque yo trabajo en CCS y no en Asembler (aunque en mi algoritmo hay algo de ASM) A ver si nos muestras tu magia
Aca dejo lo que he avanzado, aun no esta implementado la aprte de comunicacion pues aun tengo que mejorar y probar algunas cosas... A ver que les parece...

Código: CSS

1. #include <16F876A.h>
2. #fuses   XT, NOWDT, NOLVP, NOCPD, NOWRT, NODEBUG, NOPROTECT, NOBROWNOUT
3. #use     delay(clock=4000000)
4. #define  Nservos  16
5. #define  High  True
6. #define  Low   False
7. #byte    timer0 = 0x01
8. #byte    porta =  0x05
9. #byte    portb =  0x06
10. #byte    portc =  0x07
11. #bit     t0if =   0xb.2
12. #bit     Servo01 =   portb.0
13. #bit     Servo02 =   portb.1
14. #bit     Servo03 =   portb.2
15. #bit     Servo04 =   portb.3
16. #bit     Servo05 =   portb.4
17. #bit     Servo06 =   portb.5
18. #bit     Servo07 =   portb.6
19. #bit     Servo08 =   portb.7
20. #bit     Servo09 =   porta.0
21. #bit     Servo10 =   porta.1
22. #bit     Servo11 =   porta.2
23. #bit     Servo12 =   porta.3
24. #bit     Servo13 =   porta.5
25. #bit     Servo14 =   portc.0
26. #bit     Servo15 =   portc.1
27. #bit     Servo16 =   portc.2
28. int8  Index, Angulo[Nservos], s[2], time[2];
29.  
30. void Servo(int n, int1 Estado);
31. void Ini_Ang();
32. void retardo(int t);
33.  
34. void main()
35. {
36.    Index=0;
37.    setup_timer_0(RTCC_DIV_16);                     // Configura el tiempo de 2.5ms entre cada Interrupcion
38.    output_a(0);
39.    output_b(0);
40.    output_c(0);
41.    Ini_Ang();
42.    enable_interrupts( GLOBAL );
43.    while(1)
44.    {
45.       set_timer0(100);
46.       Servo(2*Index,High);
47.       Servo(2*Index+1,High);
48.       delay_us(300);
49.       if (Angulo[2*Index]>=Angulo[2*Index+1])
50.       {
51.          if (Angulo[2*Index]=Angulo[2*Index+1])
52.          {
53.             Time[0] = Angulo[2*Index];
54.             Time[1] = 0;
55.             s[0] = 2*Index;
56.             s[1] = 2*Index+1;
57.          }
58.          else
59.          {
60.             Time[0] = Angulo[2*Index+1];
61.             Time[1] = Angulo[2*Index]-Angulo[2*Index+1];
62.             s[0] = 2*Index+1;
63.             s[1] = 2*Index;
64.          }
65.       }
66.       else
67.       {
68.             Time[0] = Angulo[2*Index];
69.             Time[1] = Angulo[2*Index+1]-Angulo[2*Index];
70.             s[0] = 2*Index;
71.             s[1] = 2*Index+1;
72.       }
73.       retardo(time[0]);
74.       Servo(s[0],Low);
75.       retardo(time[1]);
76.       Servo(s[1],Low);
77.       #asm                                      // espera hasta que los 2.5ms hallan pasado
78.       Loop:
79.          BTFSS t0if
80.          GOTO  Loop
81.          BCF   t0if
82.       #endasm
83.       Index++;
84.       if (Index>=8)
85.          Index=0;
86.    }
87. }
88.  
89. void retardo(int t)
90. {
91.    int w;
92.    if (t!=0)
93.    {
94.       for (w=0;w<8;w++)
95.          delay_us(t);
96.    }
97. }
98.  
99. void Ini_Ang()
100. {
101.    int k;
102.    for(k=0;k<Nservos;k++)
103.       Angulo[k]=5*k;
104. }
105.  
106. void Servo(int n, int1 Estado)
107. {
108.    n+=n;
109.    if (Estado)
110.    {
111.       #asm
112.          MOVF  n,0
113.          ADDWF 2,1
114.          BSF   Servo01
115.          GOTO  SalidaH
116.          BSF   Servo02
117.          GOTO  SalidaH
118.          BSF   Servo03
119.          GOTO  SalidaH
120.          BSF   Servo04
121.          GOTO  SalidaH
122.          BSF   Servo05
123.          GOTO  SalidaH
124.          BSF   Servo06
125.          GOTO  SalidaH
126.          BSF   Servo07
127.          GOTO  SalidaH
128.          BSF   Servo08
129.          GOTO  SalidaH
130.          BSF   Servo09
131.          GOTO  SalidaH
132.          BSF   Servo10
133.          GOTO  SalidaH
134.          BSF   Servo11
135.          GOTO  SalidaH
136.          BSF   Servo12
137.          GOTO  SalidaH
138.          BSF   Servo13
139.          GOTO  SalidaH
140.          BSF   Servo14
141.          GOTO  SalidaH
142.          BSF   Servo15
143.          GOTO  SalidaH
144.          BSF   Servo16
145.       SalidaH:
146.       #endasm
147.    }
148.    else
149.    {
150.       #asm
151.          MOVF  n,0
152.          ADDWF 2,1
153.          BCF   Servo01
154.          GOTO  SalidaL
155.          BCF   Servo02
156.          GOTO  SalidaL
157.          BCF   Servo03
158.          GOTO  SalidaL
159.          BCF   Servo04
160.          GOTO  SalidaL
161.          BCF   Servo05
162.          GOTO  SalidaL
163.          BCF   Servo06
164.          GOTO  SalidaL
165.          BCF   Servo07
166.          GOTO  SalidaL
167.          BCF   Servo08
168.          GOTO  SalidaL
169.          BCF   Servo09
170.          GOTO  SalidaL
171.          BCF   Servo10
172.          GOTO  SalidaL
173.          BCF   Servo11
174.          GOTO  SalidaL
175.          BCF   Servo12
176.          GOTO  SalidaL
177.          BCF   Servo13
178.          GOTO  SalidaL
179.          BCF   Servo14
180.          GOTO  SalidaL
181.          BCF   Servo15
182.          GOTO  SalidaL
183.          BCF   Servo16
184.       SalidaL:
185.       #endasm
186.    }
187. }

Este codigo esta hecho para un 16F876 con un cristal de 4MHz, pronto lo pondre para 20MHz(aunque no se porque razon he tenido algunos problemas con este cristal) Y como podras dar cuenta atiendo 2 servos a la vez, por esa razon debo tener cuidado con la cantidad de tiempo que utilice para la comunicacion.

Vamos a ver como anda todo... pero me sigue pareciendo mas optimo tu protocolo, en especial porque optimizas totalmente el tiempo, a ver si me das ideas para mejorar el mio.

Nos vemos

[Manofwar]

Hola a todos

LordLafebre, sólo he podido probarlo con 3 servos, que son lo que tengo, pero esta misma mañana he recibido de China 6 que había pedido . A ver si puede esta noche y conecto todos a ver como trabaja. En cuanto a esto una pregunta ¿Sabéis si el consumo de un servo a través del pin de control es elevado?¿Es necesaria alguna resistencia que limite el consumo?. Sobre todo por no quemar el PIC cuando los 24 servos se conecten a la vez

Artifox, te comento por encima como lo hago.

Utilizo la interrupción del CCPR1 con el TMR1.

Tengo los siguientes registros (5 por pulso) en RAM.

PULSO_X_H y PULSO_X_L : Tiempo de duración del ancho de pulso

Máscara de los puertos al finalizar dicho tiempo: PUERTOB_X, PUERTOC_X, PUERTOD_X

Las máscaras tienen todos los bits a 1 excepto el que debe de cambiar a 0 para realizar un AND con el puerto en cuestión.

Ejemplo: '11110111', cambiará a cero el bit 3 de puerto X respetando al resto de los bits.


Al inicio de los 20ms todos las salidas de los puertos están a 1, cargo el CCPR1 con el tiempo del ancho del pulso menor, previamente hay que ordenar ascendentemente todos los pulsos según su duración.

Cuando llega la interrupción del comparador CCPR1, hacemos un AND con las máscaras de todos los puertos, cargamos un nuevo valor en el CCPR1 y vuelta a empezar hasta que se realiza para los 24 pulsos.

Una vez hecho esto, vienen 2 registros (INI_20ms_H, INI_20ms_L) que son los de inicio de los 20ms con un valor que se cargará en el comparador CCPR1 hasta completar los 20ms.

Aquí os pego el código que hace esto.

Código: ASM

1. ORG 0
2.        
3.         goto    inicio
4.  
5.         ORG 4
6.  
7. ;-----------------------------------------------
8. ;           DETECCION DE INTERRUPCIONES
9. ;-----------------------------------------------
10.  
11. ;-----------------------------------------------
12. ;   PUESTA A CERO DE PINES DE LOS PUERTOS
13. ; SEGUN PULSOS VARIABLES DE CONTROL DE SERVOS
14. ;-----------------------------------------------
15.         clrf    PIR1            ;Borramos la bandera indicador de interrupción
16.         btfsc   CCPR1H,7
17.         goto    ini_20ms
18.         incf    FSR,F
19.         movf    INDF,W
20.         andwf   PORTB,F
21.         bsf     FSR,7   ;-------------------------
22.         movf    INDF,W  ;
23.         andwf   PORTC,F ;
24.         incf    FSR,F   ;---------------------------
25.         movf    INDF,W  ;
26.         andwf   PORTD,F ;-------------------------
27.  
28.         bcf     FSR,7
29.         movf    INDF,W          ;Nos posicionamos en el registro bajo del siguiente cambio en los puertos
30.         movwf   CCPR1H          ;
31.         incf    FSR,F           ;con el siguiente ancho
32.         movf    INDF,W          ;
33.         movwf   CCPR1L          ;de pulso que se detectará
34.         retfie
35.  
36. ini_20ms
37.         movlw   PULSO_1_H       ;Nos posicionamos al principio de los pulsos
38.         movwf   FSR
39.         clrf    TMR1H           ;Iniciamos el
40.         clrf    TMR1L           ;Timer
41.         goto    $+1
42.         bcf     BANDERAS,0
43.         movf    INDF,W          ;Cargamos el comparador
44.         movwf   CCPR1H          ;
45.         incf    FSR,F           ;con el valor del ancho
46.         movf    INDF,W          ;
47.         movwf   CCPR1L          ;de pulso de menor valor
48.         movlw   0xff            ;--------------------------
49.         movwf   PORTB           ;
50.         goto    $+1                     ;
51.         movwf   PORTC           ;
52.         goto    $+1                     ;
53.         movwf   PORTD           ;
54.         goto    $+1                     ;
55.         retfie
56.  
57. bucle
58.         btfss   TMR1H,6         ;El control de los servos ha acabado
59.         goto    bucle           ;y podemos comprobar la usart
60.         btfsc   BANDERAS,0      ;sumar velocidades y ordenar los pulsos
61.         goto    bucle
62.         call    usart
63.         call    VELOCIDAD
64.         call    ORDENA_PULSOS
65.         bsf     BANDERAS,0      ;Ponemos a cero la bandera para que no vuelva a ejecutar hasta dentro de 20ms
66.         goto    bucle

[Artifox]

Hola Manofwar, muy bueno tu algoritmo... estoy tratando de hacer algo parecido en CCS a ver como me sale (si bien tomare tu idea como base, tienes todo el credito) No estoy usando el modulo de comparacion, aunque no descarto no hacerlo, solo estoy haciendo comparaciones simples por codigo, me hice mi algoritmo de ordenar los tiempos, pero me falta terminar la parte en donde se pone en bajo la señal de control, poco a poco ire avanzando. Ojala lo tenga para el fin de semana, ya veremos cuando lo termine, pues parece que hay algunas variaciones en el diseño mecanico y tengo que controlar 26 servos

Por otro lado, viendo bien las funciones de tu controladora, me quede con una duda, Como le haces para leer la posicion en la cual esta un servo???? Mandas un pulso por el pin de control y lees la respuesta??? o usas servos digitales???  creo que estoy algo confundido con eso.

Ya nos vemos 

[Manofwar]

Hola Artifox

Como he comentado en Robótica, he leido antes aquel subforo, sólo leo la posición teórica en donde se encuentra el servo. Tengo unos registros con la posición actual de los servo, que utilizo para sumar o restar, según el sentido del giro, de la velocidad a la que queremos que se muevan los servos.

[Manofwar]

Hola de nuevo

Solucionados los problemas con las rutinas de velocidad, he subido el nuevo firmware a http://www.ual.es/personal/salguero/control_servos/CONTROLADORA_28_SERVOS.htm. El procedimiento para mover los servos es el mismo, aún no se puede escoger la velocidad, pero tienen velocidades diferentes a la hora de posicionarse.

Este fin de semana espero tener tiempo y hacer un pequeño software de control desde el pc, para no estar escribiendo en el hyperterminal

[JHOSEF]

Hola Manofwar, que intersante esta el proyecto que nos presentas... e estado merodeando por todas las paginas que he podido... y de hecho tenia que pasar por el FORO, te comento que estamos realizado un proyecto que consiste en un robot cuadrupedo.. similar al de nuestro amigo ARTIFOX, donde tenemos que manejar  servos (3 por cada pata) y pues, esta propuesta y la tarjeta controladora que nos presentas se ve precisa... hemos descargado el diseño y el hex, y pues va muy bien, pero queremos controlar la posicion y velocidad de cada motor... y como vemos el hex no nos ayuda... teniamos la idea de manejar de 3 en 3 servos... ya incluso habia logrado eso.. pero tu protocolo me deja chikito :$ quisieramos que nos des la idea mas clara de como controlar los 12 servos a la vez, te agradeceriamos mucho.. si puedes enviarnos informacion sobre control de servos... por nuestra cuenta seguiremos intentandolo... y ya estaremos comentando en el FORO y seguramente publicando nuetros avances.... nos vemos... saludos desde PERU.

[Artifox]

Hola nuevamente, he dejado algunas ideas acerca de la controladora en Proyecto: Robot Humanoide especificamente para ver cual es la manera de controlar los servomotores.

Por otro lado JHOSEF seria bueno que revises este link del foro acerca de los Servos Futaba.

Nos vemos....

[Manofwar]

Hola a todos, perdonad por no responder antes, pero tengo problemas de conexión en casa y en el trabajo ahora estoy demasiado liado.

Jhosef, gracias por tu interés. Te comento que todo lo que sé sobre el control de  servos está en el link que te ha dicho Artifox. En cuanto a la forma de controlarlos yo lo hago como explico  aquí http://www.todopic.com.ar/foros/index.php?topic=12528.msg71656#msg71656. Si hay alguna parte que no esté bien explicada, dímelo e intentaré aclarartelo.

Artifox corro a leer ese post.

He estado diseñando el impreso de la placa, que una vez comprobado su correcto funcionamiento colgaré, y haciendo la estructura del robot. Con esto último no he conseguido los resultados deseados. Sin las herramientas adecuadas para trabajar el aluminio no consigo un acabado de las piezas que me convenza, aunque como estoy loco por ver al robot en pie, las utilizaré y después veré si compro algo de esto http://www.superrobotica.com/S360235.htm. 

[Artifox]

Hola Manofwar, yo tambien ando atareado y sin mucho tiempo, pero te comento que nosotros ya estamos en proceso de produccion de las piezas para el robot, todo se esta haciendo en aluminio, en base a planchas de 2mm de espesor. Incluso para no comprar esas piezas que me comentas, se le ha hecho unas modificaciones a los servos con el fin de ponerle un contraeje a cada servo.

Por otro lado una duda, como hace tu algoritmo si llega el caso que tengas 2 o mas posiciones iguales, es decir si tengo para varios servos la misma posicion(por consiguiente el mismo tiempo en alto) En tu algoritmo de ordenarlos valores tambien incluye eso????

Bueno ya nos comunicamos... Nos vemos

[Manofwar]

Hola Artifox

¿Podrías decirme algo más del segundo eje?. Yo había pensado en taladrar la parte posterior del servo y poner un tornillo como segundo eje, aunque al final he optado por montarlo en las articulaciones, que son algo parecido a esto


claro que sin las herramientas adecuadas no me quedan tan bonitos pero la función la realizan igual.

Pues sí, tengo en cuenta las posiciones iguales y además hay que tener en cuenta también otra cosa algo más complicada de resolver. Se trata de aquellos pulsos con una diferencia de tiempo, entre ellos, inferior al tiempo de ejecución del código de la interrupción.

Vamos que durante la ejecución del código de la interrupción llega el tiempo del siguiente pulso.

Un ejemplo:

Tiempo de ejecución del código de interrupción=5us
Pulso 1=1500us
Pulso 2=1504us

Ahora mismo la solución que le he dado es tratar ambos pulsos como si tuviesen la misma duración. Si ejecutas mi simulación y cambias la posición de algún servo llevandolo a -90º o a 90º veras que cuando pasa por la posición que ya ocupa otro de los servos hay una fluctuación de la posición de este segundo servo y es debido a esto.

Esta fluctuación no se si afectará a los servos. Según lo comentado, en el hilo de los servos Futaba en el subforo de Robótocia, sobre la zona muerta de los servos no debería afectar. En cuanto termine algunas articulaciones del robot y monte todo en su placa definitiva comprobaré esto.

[Artifox]

Hola Manofwar, sorry si no respondi antes, he estado algo cabezon con unas compras para el proyecto. He puesto algunas fotos de los servos modificados en Robot Humanoide

Con respecto al algoritmo aun no lo termino para tratar de evitar el problema de las posiciones iguales, ojala q funcione

Ya nos vemos