El desarrollo de un intérprete/compilador FORTH para el controlador de señal digital de 16 bits dsPIC33 de Microchip y mi interés en la robótica finalmente se cruzaron recientemente cuando agregué dos conjuntos de rutinas impulsadas por interrupciones en “tiempo real” al FORTHdsPIC. Estas son las definiciones de PWM del servomotor y espera/pausa. Pensé en comenzar con un servomotor básico para pasatiempos en lugar de un simple motor de CC. Porque nunca lo había usado antes y quería que el robot caminara en lugar de rodar para cambiar.
¿Qué es un servomotor?
Un servomotor es generalmente un motor de CC simple y una caja de cambios con un potenciómetro (“pot”) conectado para proporcionar retroalimentación de la posición angular del eje de salida a la electrónica que controla el motor. La olla está unida mecánicamente al eje de salida, por lo que el eje de salida no puede girar 360 grados. De hecho, los servos controlados por radio o aficionados estándar están limitados a unos 180 grados de rotación del eje. Por supuesto, la caja de engranajes permite que el propio motor de CC gire muchas veces por media rotación del eje de salida del servo, lo que aumenta el par disponible al hacerlo. Los servos estándar se utilizan para tirar/empujar palancas que impulsan superficies de control en aviones, aceleradores en modelos de autos, etc. Este “recorrido” limitado hace que los servos sean ideales como articulaciones motorizadas para pequeños robots humanoides. El punto principal a tener en cuenta es que la señal de control de entrada, presumiblemente del microcontrolador, establece la posición angular del eje de salida en lugar de la velocidad de rotación como en un motor de CC normal. La señal de control (Figura 1) es un tren de pulso continuo con un período de 20ms, el ancho de cada pulso determina la posición angular absoluta del eje.
Entonces, una serie de pulsos de 0,6 ms de ancho impulsarán el motor a la posición 0°, 1,5 ms a 90° y 2,4 ms a 180°. Los números de 1 ms, 1,5 ms y 2 ms a menudo se citan en la literatura del fabricante, pero rara vez se trata de una rotación perfecta de 180°. Puede ser necesaria la experimentación. Cuando se aplica una señal a un servo, el servo se mueve muy rápidamente desde cualquier lugar, se mueve a la posición seleccionada y se detiene. Si continúa impulsándolo, se mantendrá en esa posición y resistirá la fuerza de rotación del eje que intenta moverlo. (¡Hasta cierto punto, es solo un motor pequeño después de todo!) La señal tiene la forma de modulación de ancho de pulso (PWM) y se genera fácilmente desde el pin de comparación de salida del dsPIC33. Puede haber hasta 8 de estos, lo que es suficiente para crear un robot andante bastante sofisticado.
Servo como articulación de robot eléctrico
Para probar la funcionalidad servo del FORTHdsPI C, necesitaba un robot humanoide muy simple. Afortunadamente, tuve un robot construido por el fabricante de semiconductores Freescale para demostrar la placa mecatrónica Coldfire. Compré un cable de extensión de servo largo para formar una conexión entre el motor del robot y el sistema RS EDP que estoy usando para el desarrollo. Los robots de Freescale son mecánicamente tan simples como sea posible, pero con lo que percibimos como “caminar”. Solo se utilizan cuatro servos, dos para cada pierna en las posiciones de tobillo y cadera. El resultado es una especie de movimiento anciano, pero muestra la inestabilidad deliberada que es un factor clave en la locomoción humanoide. Un ciclo de pasos consta de cuatro operaciones de movimiento principales:
1. Inclínese hacia la izquierda, mantenga el equilibrio sobre el pie izquierdo y levante el pie derecho del suelo.
2. Usando su cadera izquierda, rote su pie derecho hacia adelante sobre su pie izquierdo.
3. Vuelva a colocar el pie derecho en el suelo y párese derecho.
4. Repita los pasos 1-3 anteriores mientras mantiene el equilibrio sobre la pierna derecha.
Por supuesto, los humanos se inclinan hacia adelante en lugar de hacia los lados para lograr el mismo efecto, pero esto requiere una articulación de rodilla que funcione.
implementación
El movimiento 1 está dirigido por la palabra LEANL, que consta de solo dos líneas.
( Servo 1 = right ankle, Servo 2 = left ankle ) ( Servo 3 = left hip, Servo 4 = right hip ) : LEANL ( Balance on left leg ) 111 90 DO I 1 SERVO I 2 SERVO 20 WAIT LOOP 151 110 DO I 1 SERVO 20 WAIT LOOP ;
El primer DO… LOOP rota ambas articulaciones del tobillo 20° desde una posición vertical de 90° a 110°. El segundo bucle gira el tobillo derecho entre 40° y 150° adicionales. Los movimientos de la pierna derecha se utilizan para empujar el robot y desequilibrarlo con la pierna izquierda. Sin la ayuda de la pierna derecha, el robot simplemente subiría “hasta el punto” como una bailarina de ballet y no sería de mucha ayuda. Otro punto destacable es el uso de DO LOOP. El formato de la palabra FORTHdsPIC SERVO es como sacar dos parámetros de la pila: el número de identificación del servo y el ángulo objetivo de 0 a 180 grados. Las dos líneas de programa en LEANL se pueden reducir a:
111 1 SERVO 111 2 SERVO 151 1 SERVO
El problema es la velocidad. El servo se mueve muy rápidamente hacia el ángulo objetivo, lo que da como resultado un robot que se mueve como un juguete. El bucle asegura que se inserten pausas entre cada ángulo de rotación del servo. La palabra FORTHdsPIC WAIT proporciona esta pausa y toma un parámetro de pila en milisegundos. Entonces, 20 WAIT suspende la ejecución del programa durante 20 milisegundos.
El movimiento 2 está dirigido por la palabra MOVRLF, que consta de una sola línea.
MOVRLF ( Rotate on left leg ) 59 120 DO I 3 SERVO I 4 SERVO 20 WAIT -1 +LOOP ;
La palabra consiste en un solo lazo que rota ambas articulaciones de la cadera simultáneamente en 60°. El efecto de esto es torcer su cuerpo hacia la izquierda mientras mantiene sus pies paralelos y apuntando en la dirección del viaje. Tenga en cuenta el uso de +LOOP ya que necesitamos contar hacia atrás de 120 a 60.
El tercer movimiento lo ejecuta la palabra RETR, que consta de tres líneas:
RETR ( Return right leg to floor ) 119 150 DO I 1 SERVO 20 WAIT -1 +LOOP 89 110 DO I 2 SERVO 20 WAIT -1 +LOOP 89 120 DO I 1 SERVO 20 WAIT -1 +LOOP ;
La primera fila devuelve el tobillo derecho a 120°. En segundo lugar, gire el tobillo izquierdo hacia atrás para volver a poner el robot en posición vertical. Sin embargo, antes de que el servo del tobillo izquierdo complete el movimiento, el robot vuelve a su pierna derecha y “se desequilibra”. Para limitar este movimiento brusco, el tobillo derecho se devolvió parcialmente a la posición de 90° en la fila 1. La tercera línea de código completa la rotación del tobillo derecho a 90° para poner el robot en posición vertical.
gran paso para los robots
Hacer estas tres palabras una tras otra hará que el robot avance un paso.
HALFSTEPL LEANL MOVRLF RETR ;
Otro conjunto de tres palabras para mover la pierna izquierda completa el “ciclo” de pasos y permite que el robot camine. Un poco de experimentación debería darle el valor óptimo para la palabra WAIT.
Apéndice: código ensamblador FORTHdsPIC para SVOSET y SERVO
Initialise servo motor output control (Output Compare) ; SVOSET = Setup PWM timer and output compare modules
; Uses Timer 2
; Up to 8 servo motor drivers
; PWM frequency: 50Hz SVOSET: mov #0x0020,w0
mov w0,T2CON ;Set up Timer 2, Prescaler = 64
clr TMR2 ;Clear Timer 2
mov #12000,w0
mov w0,PR2 ;Set period = 20ms
mov #6,w0
mov w0,OC1CON ;Set PWM mode OC1
mov w0,OC2CON ;Set PWM mode OC2
mov w0,OC3CON ;Set PWM mode OC3
mov w0,OC4CON ;Set PWM mode OC4
mov w0,OC5CON ;Set PWM mode OC5
mov w0,OC6CON ;Set PWM mode OC6
mov w0,OC7CON ;Set PWM mode OC7
mov w0,OC8CON ;Set PWM mode OC8
mov #0x1000,w0
mov w0,IPC1 ;Set Timer 2 interrupt priority
bclr IFS0,#T2IF ;Clear Timer 2 Interrupt flag
bset IEC0,#T2IE ;Enable Timer 2 Interrupt
bset T2CON,#TON ;Start Timer 2
bra NEXT ; SERVO = Set PWM duty cycle
; Set servo pulse width: 0 -> 0.6ms, 180 -> 2.4ms
; Assumes PR2 = 12000, 50Hz
; Update occurs on next Timer 2 interrupt
; Entry: TOS - Servo channel no. (1 - 8)
; TOS-1 - Angle, 0 -> 180 degrees SERVO: mov [--w13],w0 ;Get channel no. from P stack
mov #VARBL+24,w2 ;Get base variable address
mul.uu w0,#2,w0 ;Form address offset
add w0,w2,w2 ;Form servo variable address
mov [--w13],w0 ;Get required angle from P stack
mul.uu w0,#6,w0 ;Convert angle to pulse width
add #360,w0
mov w0,[w2] ;Update channel angle variable
bra NEXT
La palabra SERVO toma un ángulo de la pila y lo convierte al valor correspondiente necesario para establecer el ancho de pulso del canal seleccionado. Si el valor del registro del período Timer2 es 12000 (período de pulso de 20 ms), los cálculos realizados por SERVO son:
OCxRS = (Angle x 6) + 360
OCxRS es el registro de ancho de pulso del canal seleccionado.
Rutina de interrupción para el temporizador 2
Output Compare Timer Interrupt Service Routine
; Servomotor PWM mode, fixed interrupt interval 20ms __T2Interrupt:
push w0
mov VARBL+26,w0
mov w0,OC1RS ;Pulse width for next PWM 1 cycle
mov VARBL+28,w0
mov w0,OC2RS ;Pulse width for next PWM 2 cycle
mov VARBL+30,w0
mov w0,OC3RS ;Pulse width for next PWM 3 cycle
mov VARBL+32,w0
mov w0,OC4RS ;Pulse width for next PWM 4 cycle
mov VARBL+34,w0
mov w0,OC5RS ;Pulse width for next PWM 5 cycle
mov VARBL+36,w0
mov w0,OC6RS ;Pulse width for next PWM 6 cycle
mov VARBL+38,w0
mov w0,OC7RS ;Pulse width for next PWM 7 cycle
mov VARBL+40,w0
mov w0,OC8RS ;Pulse width for next PWM 8 cycle
pop w0
bclr IFS0,#T2IF ;Clear Timer 2 Interrupt flag
retfie
Sigue mis publicaciones en Twitter (@williammarshal1) si estás atascado. Enlazo nuevos componentes electrónicos y kits de desarrollo interesantes (¡algunos incluso están disponibles a través de mi empleador!) y retuiteo las publicaciones que encuentro sobre robótica, exploración espacial y otros temas.