Modelamiento y Control de un Robot Bípedo Usando un Sistema
Operativo Implementado en un DSP
Elvis O. Jara, Edison E. Alfaro, David R. Achanccaray and Ricardo R. Rodríguez
Resumen: Este trabajo describe el análisis y
modelamiento dinámico de un robot bípedo, además del
desarrollo de una estrategia de control del equilibrio y
desplazamiento del Robot Bípedo durante su caminata, se
usará también algoritmos de lógica difusa como una ayuda
a la optimización del control PID para los motores. Estos
robots tienen una movilidad distinta a los de ruedas
convencionales, debido a la cantidad de grados de libertad
que posee y a su alto grado de no linealidad, todo esto hace
que la estrategia de control de la caminata sea un proceso
complejo. Se mostrara además las simulaciones del diseño
en software para luego pasar a la implementación del
sistema, para ello haremos uso de un Sistema Operativo en
Tiempo Real implementado en un Procesador Digital de
Señales, y de toda la interfaz electrónica necesaria para el
control de sus motores.
Palabras clave: Robot Bípedo, Procesador Digital de
señales (DSP), control PID.
Abstract: This paper describes the analysis and dynamic
modeling of a biped robot, and the development of a
strategy of balance control and movement of Biped Robot
during its walk, It will also use fuzzy logic algorithms as an
aid to the optimization of PID control for engines.
These robots have mobility different from the
conventional wheeled mobile robots, due to the number of
degrees of freedom, which already has a high degree of
nonlinearity; this makes that the walking control strategy
become a complex process. It also showed simulations of
design in software, then the implementation of the system,.
The biped robot use a Real Time Operating System
implemented in a Digital Signal Processor, and the
necessary electronic interface to control its engines.
Key words: Biped Robot, digital signal processor (DSP),
PID control.
Manuscript received September 23, 2010. This work was developed in
the Control System and Artificial Intelligence Research Group
(GISCIA), Department of Mechanical Engineering, National University of
Engineering.
Edison E. Alfaro is a Mechatronic Engineering Student from the
National University of Engineering (UNI), Perú (e-mail:
idefim@gmail.com).
Elvis O. Jara is a Mechatronic Engineering Student from the National
University of Engineering (UNI), Perú (e-mail: elvisjara@gmail.com).
MSc . David R. Achanccaray is a Mechatronic Engineer, he works like
Professor at the National University of Engineering (UNI), Perú (e-mail:
ingmec251@gmail.com).
MSc . Ricardo R. Rodriguez is a Electronic Engineer, he works like
Professor at the National University of Engineering (UNI), Perú (e-mail:
robust@uni.pe).
I. INTRODUCTION
S
para
e quiere lograr el modelamiento y una estrategia de
control del robot, probando su estabilidad y robustez,
su posterior Implementación usando
microprocesadores avanzados. La evolución en la
construcción de robots humanoides se ha dado desde 1986,
con la construcción de un robot bípedo caminante muy
rústico. Este fue el punto de partida para numerosas
investigaciones en este campo, el cual tiene en el robot
ASIMO de la compañía HONDA, su más reciente logro. A
partir de la construcción de este prototipo, numerosas
instituciones educativas han construido su propio robot
humanoide, en el cual su desarrollo implica la aplicación de
numerosas herramientas de la Ingeniería Mecatrónica como
el diseño de controladores, inteligencia artificial, cálculo de
las ecuaciones matemáticas, etc. Finalmente, debido al
subdesarrollo en el cual se encuentra nuestro país, este
campo no ha sido desarrollado lo suficiente como para tener
antecedentes de estas investigaciones, por lo que nos
guiaremos de referencia actualizada e Innovadora en este
campo. libro Feedback Control of Dynamic Bipedal Robot
Locomotion System7
II. PROCEDIMIENTO
Como punto de partida para el desarrollo de nuestro
proyecto, se decidió averiguar experiencias anteriores sobre
desarrollo de robots bípedos en el mundo, notando que
existían diversas maneras de afrontar al problema, de estas
elegimos de referencia las que se acomodaban más a
nuestros objetivos y realidad, teniendo en cuenta además
factores económicos. Finalmente se eligió adoptar la
metodología tenida en cuenta para el robot bípedo BART
(Bipedal Autonomous Robot) desarrollado por el Instituto
para tecnologías de Control de la universidad Alemana de
Hanover1.
A. Diseño del mecanismo del Robot Bípedo
Para este diseño se hizo uso del software de SolidWork,
teniendo en cuenta que usaremos como material base marco
de aluminio AA-1060 de 1''x1''x1/8 para las articulaciones
del robot. Para definir las características dimensionales y
denominaciones de las partes se hará una analogía con la
estructura típica de una persona.
Para el diseño mecánico se tuvo en cuenta que en las
articulaciones deberán ubicarse motores DC, además en los
pies debe existir un pequeño desfase entre los apoyos
laterales para evitar la posibilidad de contacto cuando
ambos pies estén alineados, además debemos tener en
cuenta una plataforma en la cual ubicaremos toda la
circuitería necesaria para cuando implementemos el robot, como principal dispositivo al DSP, además debe añadirse al
en esta plataforma también deberán ubicarse las baterías del circuito otras interfaces, como los drivers que permitirán el
robot. Estas características mencionadas se muestran en la control de los motores. Esta circuito electrónico deberá
figura 1. ubicarse en la plataforma superior del robot junto con las
Plataforma
baterías, el esquema electrónico diseñado se muestra en el
superior anexo A.2.

1) Microprocesador

Se hará uso del procesador digital de señales DSP

TMS320F2812 de Texas Instrument2, debido a su
Articulación capacidad de procesamiento y una respuesta rápida para los
Fémur de la cadera 6 actuadores que se utilizaran. Haremos uso del módulo
eZdsp de Spectrum Digital3, el cual se muestra en la figura
2, éste módulo incluye pines de entrada y salida, memoria
Articulación RAM externa, además de conexión JTAG él cual con ayuda
de la rodilla del emulador XDS100 de Spectrum Digital nos permitirá
Tibia
emular el funcionamiento del sistema en tiempo real.

Pie Articulación
del pie

Fig. 1. Diseño del mecanismo del Robot Bípedo y descripción de sus

partes

En la tabla 1 se resume las características dimensionales del

robot bípedo, así como el peso aproximado de cada Fig. 2. Módulo del Procesador Digital de Señales (eZdsp).
elemento.
2) Modelamiento
TABLA I
CARACTERÍSTICAS DIMENSIONALES DEL MECANISMO DEL ROBOT BÍPEDO
Para el modelamiento dinámico del robot haremos uso de
Dimensiones Peso Peso Total
Cant. las ecuaciones de lagrange4. Para ello primero debemos
(mm) (gr) (gr)
fijar un sistema de referencia el cual mantendremos fijo,
Pie 200 x 130 685 1 685
elegimos dicho sistema de referencia en el punto medio de
Tibia 400 700 1 700
las articulaciones de la cadera, tal como se muestra en la
Fémur 450 745 1 745
figura 3. Inicialmente se abordará el problema asumiendo
Motor que cada pierna podrá moverse independientemente una de
150 – 254 820 3 2460
DC
la otra9, este método de análisis tiene como primer objetivo
Peso Total Pierna 4590 lograr que cada efector final pueda seguir una trayectoria
deseada.
Plataforma
400x200 350 1 350
Superior

En el proceso de construcción del robot bípedo con las

características mencionadas, se vio la necesidad de
construir un primer prototipo que nos permita visualizar
muchos aspectos no solo mecánicos sino también
electrónicos, es por ello que se desarrolló un primer
prototipo a escala 1:2 y usando servo motores comerciales
como base de actuación. Las características son similares al
modelo descrito y los planos del diseño se adjuntan en el
anexo A.1

B. Diseño de la interfaz electrónica

Una vez obtenido el control del robot simulado en

software, lo que sigue es la implementación física del Fig.3. Sistema de referencia y 6GDL, Considerando cada base del pie
sistema, para ello además del mecanismo y los motores, como efector final.
debemos diseñar la interfaz electrónica propia que incluye
 Una vez definido nuestro sistema de referencia,  Cos( q + q ) − Sen(q + q ) 
1 2 1 2
 0
procedemos a describir la dinámica de todo el robot, para 
l Sen( q )
1 2
l Sen( q )
1 2 
esto nos basaremos en las ecuaciones de Lagrange5:  − l1Cos( q1) − l2Cos (q1 + q2 ) l1Sen( q1) + l2 Sen( q1 + q2 ) 
Ji =  0
l l Sen( q ) l l Sen( q )
 12 2 12 2 
1) Cinemática directa  Cos( q ) − Sen (q ) 
 1 1 1
 
l Sen ( q ) l Sen ( q )
2 2 2 2 (4)
La cinemática directa define la posición del efector final
en función de las posiciones angulares de las articulaciones.
Para nuestro se describe con la Eq. 1. 4) Estática

El Jacobiano transpuesta es obtenido relacionando

x = l Cos ( q ) + l Cos ( q + q )
1 1 2 1 2 distancias diferenciales y velocidades angulares, esto para
y = −l Sen ( q ) − l Sen( q + q ) (1) obtener el torque necesario en cada actuador Eq.5..
1 1 2 1 2
α = q3 − pi / 2 τ   − l sin(q ) l . cos(q ) 0  F 
 1  1 1 1 1  x
τ =
 2  2
l . sin(q ) − l . sin(q ) l . cos(q ) − l . cos(q ) 1. F
 y
Donde: 2 1 1 1 1 2 2
τ  l . sin(q ) − l . sin(q ) l . cos(q ) − l . cos(q ) 0  M 
x, y: Posición del efector final (base del pie) relativa a la  3  2 2 1 1 1 1 2 2  (5)
cadera
α : Orientación del efector final Para obtener el modelo dinámico del robot haremos uso
q1: Posición angular de la articulación ubicada en la del toolbox Simmechanics de Simulink6. Una alternativa
cadera. muy útil es importar el diseño realizado en SolidWork a
q2: Posición angular de la articulación ubicada en la Simulink. La figura 4 muestra ambas piernas de nuestro
rodilla. robot bípedo modelado en Simmechanics, esta herramienta
q3: Posición angular de la articulación ubicada en el también nos permite simular la cinemática y dinámica del
tobillo. robot, la Fig 4. muestra de forma gráfica el resultado de este
l1: Longitud del eslabón fémur. modelamiento.
l2: Longitud del eslabón peroné.

2) Cinemática Inversa

La cinemática inversa define el valor de la posición

angular en función de la trayectoria deseada7, debido a que
este primer análisis se da teniendo en cuenta que se trata de
un mecanismo de tres grados de libertad para cada pierna,
las ecuaciones de cinemática inversa, mostrada en la Eq. 3,
se simplifican en gran medida, en comparación con el caso
en el que nuestra referencia fuera la base de la otra pierna,
ya que de ese modo nuestro sistema se convertiría en un
mecanismo de seis grados de libertad, lo cual complicaría
en gran medida nuestros cálculos.

 x2 + y 2 − l 2 − l 2 
− 1 1 2
q = cos
2  2l l 
 12 
− 1 
l sen ( q ) Fig. 4. Diagrama del modelado con Simmechanics de Simulink, Robot
− 1 x  (3)
q1 = tan   − tan  2 2 + l cos( q )  Bípedo.
 y  l 2 2 
 1 
q3 = α − (q1 + q 2) 5) Dinámica

3) Jacobiano Inverso La dinámica del Robot esta basada en la ecuación general

de movimiento Eq.6.
Esta matriz nos permitirá calcular la velocidad de cada
union, él cálculo de la matriz Jacobiana directa se muestra H (q)qɺɺ + C(q, qɺ ) + G(q)= T .u
(6)
en la Eq. 4.
Cálculo de la Matriz de Inercia Eq. 7

h=m l l C +m l l C
2 1 c2 1 − 2 3 1 2 1− 2
  − l 2 .m + m l 2 + m l 2 + I h 0
nuestra salida u, tal como se muestra en la Fig. 6
 c1 1 21 3. 1 1 
H = h m l2 + m l2 + I 0 .
 2 c2 3. 2 2 
 0 0 I 
3
 
(7)

Cálculo de la Matriz de Coriolis Eq.8.

 1
− l (m .l + m .l ).S .qɺ
1
l (m .l + m .l ).( S .qɺ + S .qɺ ) 0

 2 1 2 c2 3 2 1− 2 2 1 2 c2 3 2 2 1− 2 1 1− 2 2 
 1 1 
C (q, qɺ ) = − l (m .l + m .l ).(S .qɺ + .S .qɺ ) l (m .l + m .l ).S .qɺ 0.
1 2 c2 3 2 1− 2 1 2 1− 2 2 2 1 2 c2 3 2 1− 2 1
 0 0 0
 
 
(8)
Fig. 6. Funciones de pertenecia
Luego de obtener la matriz de Inercia (H) y Coriolis (C),
estos serán descritos en código script, luego el sistema Usaremos Mandani como sistema difuso, además de las
puede ser simulado matemáticamente reglas de pertenencia posibles Fig. 7.

Fig. 5. Simulación del Sistema en Código Script Fig. 7. Reglas de pertenencia

Para un análisis más real debemos tener en cuenta el Una vez diseñado nuestro controlador, cambiaremos el
efecto generado por el peso de cada eslabón de nuestro PID inicial por el F PD, como se muestra en la Fig 8.
mecanismo y cómo cambian de posición los centros de
gravedad de los mismos, para ello se vio la necesidad de
añadir una compensación de la gravedad, este factor deberá
sumarse a la señal de salida del control PID, a esta
compensación de gravedad también se le incluye un ruido
blanco que nos servirá como perturbación adicional para
nuestro controlador, la figura 6 muestra el modelo en
Simulink del sistema incluyendo las características de
control mencionadas.

III. CONTROL

A. Optimización del control PID mediante lógica difusa

El problema común del control PID está en la

Fig. 8. Control F-PD
sintonización de cada uno de los parámetros, pues nuestro
sistema tendría 18 parámetros para sintonizar, para resolver
este inconveniente, se decidió usar algoritmos de Lógica IV. CAPTURA DE PATRONES DEL MOVIMIENTO HUMANO
Difusa

1) Funciones de Pertenencia: A. Captura de data

Se usara funciones de pertenencia de tipo triangular y Luego de diseñar nuestro robot, nuestro efector final
trapezoidal para denotar al error, diferencial del error y a deberá seguir la trayectoria deseada, en nuestro caso dicha
trayectoria debe permitir el desplazamiento controlado del
robot para que en todo momento pueda conservar el
equilibrio y trasladarse.
Esta trayectoria de referencia podemos obtenerla
observando las propias características humanas al caminar,
para que luego nuestro robot trate de imitarlo.
Para lograr dicho objetivo se ubicó un LED en cada
articulación y se grabó la caminata en video, cada LED
debe ser de un color distinto para luego poder filtrarlos,
para que este proceso sea más efectivo usamos los colores
base RGB (red, green, blue) ubicados en las articulaciones
del tobillo, rodilla y cadera respectivamente, con los LEDs
encendidos se debe quitar todas las otras fuentes de luz
visibles que puedan perturbar a nuestros puntos de prueba,
tal como se muestra en la Fig. 9.
Fig. 9. Ubicación de LEDs en articulaciones para determinar la trayectoria
de referencia.
Debido a que nuestro robot no necesariamente coincide
en dimensiones con el modelo de referencia empleado, las
trayectorias obtenidas deberán ser escaladas en la misma
relación de dimensiones respectivas del robot, para nuestro
caso tomaremos como altura de la cadera 118 unidades, tal
como se muestra en la figura 8(a) donde vemos la posición
inicial de este punto.
Luego las gráficas independientes para cada articulación
serán:
(a)
(b)
(c)
Fig. 10. Trayectoria de referencia para las articulaciones del robot. (a)
Trayectoria del la cadera (b) trayectoria de la rodilla, (c) trayectoria del
tobillo.
Debido a que inicialmente se eligió abordar el problema
teniendo en cuenta que la cadera es un punto fijo y que se
cuentan con dos efectores finales, uno en cada pierna y
ubicado en el tobillo de cada una. Entonces las trayectorias
mostradas en la figura 8, las cuales son relativas a un punto
fijo en tierra, deberán cambiarse por otras en las que el
punto fijo sea la cadera, es decir cada vector de posición de
la trayectoria del tobillo deberá ser restado de la posición
respectiva de la cadera.
B. Programación del DSP orientado a un RTOS para
sistemas embebidos.
La intensión inicial de esta metodología de programación
radica en que nos facilita la gestión de los procesos que
debe ejecutar el microprocesador, para ello haremos uso de
una base de tiempo controlado por un timer del DSP, el
cual ejecutará cada proceso en un determinado tiempo,
dando la impresión de que muchos procesos se ejecutan
simultáneamente.
El robot bípedo debe controlar inicialmente 6 motores
con encoders, y un control PID para cada uno, además
luego se le añadirán sensores de fuerza en las plantas de los
pies y acelerómetros de tres ejes en la cadera, además de
una interfaz de monitoreo de los parámetros del robot desde
una PC, esta cantidad de recursos justifica el diseño de un
RTOS, aprovechando además las ventajas de rapidez
característicos de los DSPs.
La idea de la programación orientada a sistemas
operativos, está en que podemos crear tareas (task) teniendo
en cuenta que la CPU se encargará de ejecutarla solo a una
de ellas. Un pequeño Kernel será implementado con el
objetivo de que pueda distribuir la CPU entre todas las
tareas, una parte importante de dicho Kernel es el
Scheduler, que se encarga de determinar cual tarea debe
ejecutarse basándose en una prioridad asignada a la
misma10.
En la tabla 2 mostramos las principales tareas de control de
motores definidas, además se incluyó una tarea propia para
controlar al RTOS, la cual se muestra en la figura 9, donde
también podemos apreciar que al iniciar la ejecución del
programa se inicializan todas las tareas y luego se habilitan
las interrupciones del DSP, luego en el bucle infinito se
ejecutan las tareas principales, las cuales tienen como
argumento de entrada la prioridad de la tarea. Los
principales códigos en C de este programa se muestran en
el Anexo A.0

TABLA 2
DEFINICIÓN DE TAREAS DEL RTOS PARA EL CONTROL DE MOTORES
Tareas Inicialización Función principal Fig. 12. Secuencia de estados del robot bípedo para seguir una trayectoria
Motor_Cadera_izq_Task Motor_Cadera_izq_Init Motor_Cadera_izq_Run lineal determinada.
Motor_Rodilla_izq_Task Motor_Rodilla_izq_Init Motor_Rodilla_izq_Run
Motor_Tobillo_izq_Task Motor_Tobillo_izq_Init Motor_Tobillo_izq_Run
Motor_Cadera_der_Task Motor_Cadera_der_Init Motor_Cadera_der_Run
Motor_Rodilla_der_Task Motor_Rodilla_der_Init Motor_Rodilla_der_Run
Motor_Tobillo_der_Task Motor_Tobillo_der_Init Motor_Tobillo_der_Run

(a)

Fig. 11. Diagrama de flujo de los estados de una tarea del RTOS.

V. RESULTADOS

(b)
De acuerdo a lo mencionado en el punto 2, se eligió una
trayectoria que asemeja a la de un paso cuasiestático para
nuestro efector final, luego sintonizaremos los parámetros
de control PID: proporcional (Kp), Derivativo (Kd) e
Integral (Ki), para cada uno de nuestros motores.
La Fig. 12 muestra la secuencia del desplazamiento
obtenido para la trayectoria dada, esta secuencia se dio
eligiendo un control PID con los siguientes parámetros:

A. Articulación 1:Kp1=210, Ki1=10, Kd1=15

B. Articulación 2 :Kp2=80, Ki2=10, Kd2=2
C. Articulación 3 :Kp4=400, Ki4=10, Kd4=0.6
Estos valores se obtuvieron luego de hacer varias
pruebas, se recomienda en un futuro emplear una estrategia
de control utilizando algoritmos genéticos, empleando redes
neuronales que puedan entrenar las constantes óptimas para
el control PID.
(c)
Fig. 13. Diagrama del seguimiento a la referencias para (a) cadera, (b)
rodilla, (c) tobillo.
Finalmente para mostrar que nuestro control funciona
correctamente, graficamos los errores obtenidos de la data
de la posición angular de los motores, estos se muestran en
la Fig 13, notamos que se llega a un tiempo establecimiento
en corto tiempo.
 Se logró modelar el robot bípedo usando herramientas de
SolidWork y de Simulink conjuntamente.
Existe una buena perfomance en el uso de un controlador F
- PD.
Es necesario usar técnicas avanzadas para el diseño de las
funciones de pertenencia en el control Fuzzy, para así
obtener una respuesta rápida.
El uso del Sistema Operativo en Tiempo Real nos permite
una mejor administracionen la secuencia de procesos.

VII. REFERENCIAS

[1] Universidad Alemana que desarrolló el robot BART el cual sirvió de

modelo para este proyecto http://www.uni-hannover.de/ Enlace
completo del proyecto:
http://www.irt.uni-hannover.de/forschung/asr/bart.html.
(a) [2] Texas Instrument http://www.ti.com/.
[3] Spectrum digital http:/www.spectrumdigital.com/
[4] Yildirim Hurmuzlu, Frank Genot, Bernard Brogliato “Modeling,
stability and control of biped robots—a general framework”,
ELSEIVER, France 2004.
[5] C. Chevallereau, G. Bessonnet G. Abba, Y. Aoustin, Bipedal Robots-
Modeling, Design and Walking Synthesis, London 2007: chapter 2.
[6] Página de Matlab, se tiene información de Simmechanics de
Simulink www.mathworks.com
[7] R. Westervelt, W. Grizzle, C. Chevallereau, Jun Ho Choi, B. Morris,
Feedback Control of Dynamic Bipedal Robot Locomotion Systems.
France, 2007, chapter I, II, pag. 26.
[8] H. Harry Asada, Introduction to Robotics. MIT. Chapter 7, 8.
[9] S. Caux and R. Zapata, “Modeling and control of biped robot
dynamics”, Cambridge Journals United Kingdom 1998. Disponible:
http://journals.cambridge.org.
[10] Gustabo Galeano A. “Programación de Sistemas Embebidos en
tiempo en lenguaje C”, Editorial Alpha Omega. 2009.
(b)
Fig. 14. Diagramas que muestran buen funcionamiento de nuestro
controlador PID (a) Señales de errores en cada actuador. (b) Señal de
errores superpuestos.

Para poder poner a prueba nuestros resultados obtenidos,

se implemento el siguiente prototipo. Fig 15.

Fig. 15. Implementación del prototipo

VI. CONCLUSIONES
 ANEXO-A.0 MAQUINA DE ESTADOS PARA UNA TAREA DEL RTOS

/* Esta tarea controla al servomotor de la cadera del motor izquierdo, la posición angular
PROGRAMA PRINCIPAL se actualiza por entrada SCI la cual es afectada en el estado “interrumpido”.*/
//*******************************************************************************
#include "inclusiones.h" **

void main(void) #include "inclusiones.h"

{
DSP_Init(); // Definiciones de estado de tareas
Motor_Cadera_Izq_Init(); #define __MOTOR_CAD_IZQ
Motor_Cadera_Der_Init(); #ifdef __MOTOR_CAD_IZQ
Motor_Rodilla_Izq_Init();
Motor_Rodilla_Der_Init(); #define PRIORIDAD_MOTOR_CAD_IZQ = 10;
Motor_Tobillo_Izq_Init();
Motor_Tobillo_Der_Init(); #define STD_SUSPENDIDO 0
ENABLE_INTERRUPTS; #define STD_DISPONIBLE 1
while(TRUE) #define STD_ESPERA 2
{ #define STD_EJECUCION 3
DSP_Run(); #define STD_INTERRUMPIDO 4
Motor_Cadera_Izq_Init();
Motor_Cadera_Der_Init(); #define CAD_IZQ 0 // posicion asignada en el vector buffer_RX de SCI.h
Motor_Rodilla_Izq_Init();
Motor_Rodilla_Der_Init(); // Definiciones para el servomotor
Motor_Tobillo_Izq_Init(); #define PIN_MOTOR_CAD_IZQ GpioDataRegs.GPBDAT.bit.GPIOB0
Motor_Tobillo_Der_Init(); #define SET_POINT_SERVO 5 // Unidades <>
} // Variables externas
} extern static unsigned long BASE_TIEMPO; // Se incrementa cada 100uSeg*50 = 5mSeg
volatile unsigned char flag_buffer_full;
CONTROL DE TIEMPOS USANDO TIMERS
// Variables del fichero
interrupt void cpu_timer0_isr(void){
static unsigned char std_cad_izq = STD_SUSPENDIDO;
static char escala_base_tiempo = 0; // Calibrar este valor,
static unsigned char pulso = 5; // 3 < pulso < 10
//*******************************************************************************
CpuTimer0.InterruptCount++;
*******
// Limpiamos el contador
// MAQUINA DE ESTADOS
if( CpuTimer0.InterruptCount == TOTAL_PULSOS_PERIODO ) CpuTimer0.InterruptCount =
0;
void Motor_Cadera_Izq_Run(void){
//PIN_INT = ~PIN_INT;
static unsigned long int base_tiempo_motor_cad_izq = 0;
escala_base_tiempo ++;
if(escala_base_tiempo <= 50){
if(flag_buffer_full = 1) std_cad_izq = STD_INTERRUMPIDO;
escala_base_tiempo = 0;
BASE_TIEMPO ++;
switch(std_cad_izq){
}
case STD_SUSPENDIDO:
std_cad_izq = STD_SUSPENDIDO;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
break;
}
 case STD_DISPONIBLE: MONITOREO DE LOS SERVOMOTORES MEDIANTE EL MÓDULO SCI
base_tiempo_motor_cad_izq = BASE_TIEMPO;
std_cad_izq = STD_ESPERA; void Motor_Cadera_Izq_Run(void){
static unsigned long int base_tiempo_motor_cad_izq = 0;
break;
case STD_ESPERA: if(flag_buffer_full = 1) std_cad_izq = STD_INERRUMPIDO;
if(BASE_TIEMPO - base_tiempo_motor_cad_izq > PRIORIDAD_MOTOR_CAD_IZQ)
std_cad_izq = STD_EJECUCION; switch(std_cad_izq){
else std_cad_izq = STD_ESPERA; case STD_SUSPENDIDO:
break; std_cad_izq = STD_SUSPENDIDO;
case STD_EJECUCION: break;
Motor_Cadera_Izq_Task(); case STD_DISPONIBLE:
base_tiempo_motor_cad_izq = BASE_TIEMPO;
break; std_cad_izq = STD_ESPERA;
case STD_INTERRUMPIDO:
pulso = buffer_RX[CAD_IZQ]; break;
std_cad_izq = STD_ESPERA; case STD_ESPERA:
break; if(BASE_TIEMPO - base_tiempo_motor_cad_izq > PRIORIDAD_MOTOR_CAD_IZQ)
default: // falta implementar std_cad_izq = STD_EJECUCION;
break; else std_cad_izq = STD_ESPERA;
} break;
} case STD_EJECUCION:
Motor_Cadera_Izq_Task();
// EJECUCIÓN PRINCIPAL – Control del ancho de pulso del PWM para el servomotor
void Motor_Cadera_Izq_Task(void){ break;
if(CpuTimer0.InterruptCount < pulso){ // Pulso se actualiza por lectura del SCI case STD_INTERRUMPIDO:
PIN_MOTOR_CAD_IZQ = 1; pulso = buffer_RX[CAD_IZQ];
} std_cad_izq = STD_ESPERA;
else if (CpuTimer0.InterruptCount >= pulso){ break;
PIN_MOTOR_CAD_IZQ = 0; default: // falta implementar
} break;
} }
}
#endif
ANEXO-A.1
ANEXO-A.2