Resumen: Este trabajo describe el anlisis y

modelamiento dinmico de un robot bpedo, adems del

desarrollo de una estrategia de control del equilibrio y
desplazamiento del Robot Bpedo durante su caminata, se
usar tambin algoritmos de lgica difusa como una ayuda
a la optimizacin 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 adems las simulaciones del diseo
en software para luego pasar a la implementacin del
sistema, para ello haremos uso de un Sistema Operativo en
Tiempo Real implementado en un Procesador Digital de
Seales, y de toda la interfaz electrnica necesaria para el
control de sus motores.

Palabras clave: Robot Bpedo, Procesador Digital de
seales (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

e quiere lograr el modelamiento y una estrategia de
control del robot, probando su estabilidad y robustez,
para su posterior Implementacin usando
microprocesadores avanzados. La evolucin en la
construccin de robots humanoides se ha dado desde 1986,
con la construccin de un robot bpedo caminante muy
rstico. Este fue el punto de partida para numerosas
investigaciones en este campo, el cual tiene en el robot
ASIMO de la compaa HONDA, su ms reciente logro. A
partir de la construccin de este prototipo, numerosas
instituciones educativas han construido su propio robot
humanoide, en el cual su desarrollo implica la aplicacin de
numerosas herramientas de la Ingeniera Mecatrnica como
el diseo de controladores, inteligencia artificial, clculo de
las ecuaciones matemticas, etc. Finalmente, debido al
subdesarrollo en el cual se encuentra nuestro pas, 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 System
7

II. PROCEDIMIENTO

Como punto de partida para el desarrollo de nuestro
proyecto, se decidi averiguar experiencias anteriores sobre
desarrollo de robots bpedos en el mundo, notando que
existan diversas maneras de afrontar al problema, de estas
elegimos de referencia las que se acomodaban ms a
nuestros objetivos y realidad, teniendo en cuenta adems
factores econmicos. Finalmente se eligi adoptar la
metodologa tenida en cuenta para el robot bpedo BART
(Bipedal Autonomous Robot) desarrollado por el Instituto
para tecnologas de Control de la universidad Alemana de
Hanover
1
.

A. Diseo del mecanismo del Robot Bpedo

Para este diseo 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 caractersticas dimensionales y
denominaciones de las partes se har una analoga con la
estructura tpica de una persona.
Para el diseo mecnico se tuvo en cuenta que en las
articulaciones debern ubicarse motores DC, adems en los
pies debe existir un pequeo desfase entre los apoyos
laterales para evitar la posibilidad de contacto cuando
ambos pies estn alineados, adems debemos tener en
cuenta una plataforma en la cual ubicaremos toda la
Modelamiento y Control de un Robot Bpedo Usando un Sistema
Operativo Implementado en un DSP
Elvis O. Jara, Edison E. Alfaro, David R. Achanccaray and Ricardo R. Rodrguez
S


circuitera necesaria para cuando implementemos el robot,
en esta plataforma tambin debern ubicarse las bateras del
robot. Estas caractersticas mencionadas se muestran en la
figura 1.

Fig. 1. Diseo del mecanismo del Robot Bpedo y descripcin de sus
partes

En la tabla 1 se resume las caractersticas dimensionales del
robot bpedo, as como el peso aproximado de cada
elemento.

TABLA I
CARACTERSTICAS DIMENSIONALES DEL MECANISMO DEL ROBOT BPEDO

Dimensiones
(mm)
Peso
(gr)
Cant.
Peso Total
(gr)
Pie 200 x 130 685 1 685
Tibia 400 700 1 700
Fmur 450 745 1 745
Motor
DC
150 254 820 3 2460
Peso Total Pierna 4590

Plataforma
Superior
400x200 350 1 350

En el proceso de construccin del robot bpedo con las
caractersticas mencionadas, se vio la necesidad de
construir un primer prototipo que nos permita visualizar
muchos aspectos no solo mecnicos sino tambin
electrnicos, es por ello que se desarroll un primer
prototipo a escala 1:2 y usando servo motores comerciales
como base de actuacin. Las caractersticas son similares al
modelo descrito y los planos del diseo se adjuntan en el
anexo A.1

B. Diseo de la interfaz electrnica

Una vez obtenido el control del robot simulado en
software, lo que sigue es la implementacin fsica del
sistema, para ello adems del mecanismo y los motores,
debemos disear la interfaz electrnica propia que incluye
como principal dispositivo al DSP, adems debe aadirse al
circuito otras interfaces, como los drivers que permitirn el
control de los motores. Esta circuito electrnico deber
ubicarse en la plataforma superior del robot junto con las
bateras, el esquema electrnico diseado se muestra en el
anexo A.2.

1) Microprocesador

Se har uso del procesador digital de seales DSP
TMS320F2812 de Texas Instrument
2
, debido a su
capacidad de procesamiento y una respuesta rpida para los
6 actuadores que se utilizaran. Haremos uso del mdulo
eZdsp de Spectrum Digital
3
, el cual se muestra en la figura
2, ste mdulo incluye pines de entrada y salida, memoria
RAM externa, adems de conexin JTAG l cual con ayuda
del emulador XDS100 de Spectrum Digital nos permitir
emular el funcionamiento del sistema en tiempo real.


Fig. 2. Mdulo del Procesador Digital de Seales (eZdsp).

2) Modelamiento

Para el modelamiento dinmico del robot haremos uso de
las ecuaciones de lagrange
4
. Para ello primero debemos
fijar un sistema de referencia el cual mantendremos fijo,
elegimos dicho sistema de referencia en el punto medio de
las articulaciones de la cadera, tal como se muestra en la
figura 3. Inicialmente se abordar el problema asumiendo
que cada pierna podr moverse independientemente una de
la otra
9
, este mtodo de anlisis tiene como primer objetivo
lograr que cada efector final pueda seguir una trayectoria
deseada.


Fig.3. Sistema de referencia y 6GDL, Considerando cada base del pie
como efector final.

Tibia
Fmur
Pie Articulacin
del pie
Articulacin
de la rodilla
Articulacin
de la cadera
Plataforma
superior


Una vez definido nuestro sistema de referencia,
procedemos a describir la dinmica de todo el robot, para
esto nos basaremos en las ecuaciones de Lagrange
5
:

1) Cinemtica directa

La cinemtica directa define la posicin del efector final
en funcin de las posiciones angulares de las articulaciones.
Para nuestro se describe con la Eq. 1.

2 / 3
)
2 1
(
2
)
1
(
1
)
2 1
(
2
)
1
(
1
pi q
q q Sen l q Sen l y
q q Cos l q Cos l x
=
+ =
+ + =

(1)


Donde:
x, y: Posicin del efector final (base del pie) relativa a la
cadera
: Orientacin del efector final
q1: Posicin angular de la articulacin ubicada en la
cadera.
q2: Posicin angular de la articulacin ubicada en la
rodilla.
q3: Posicin angular de la articulacin ubicada en el
tobillo.
l1: Longitud del eslabn fmur.
l2: Longitud del eslabn peron.

2) Cinemtica Inversa

La cinemtica inversa define el valor de la posicin
angular en funcin de la trayectoria deseada
7
, debido a que
este primer anlisis se da teniendo en cuenta que se trata de
un mecanismo de tres grados de libertad para cada pierna,
las ecuaciones de cinemtica inversa, mostrada en la Eq. 3,
se simplifican en gran medida, en comparacin con el caso
en el que nuestra referencia fuera la base de la otra pierna,
ya que de ese modo nuestro sistema se convertira en un
mecanismo de seis grados de libertad, lo cual complicara
en gran medida nuestros clculos.

) 2 1 ( 3
)
2
cos(
2
1
)
2
(
2 1
tan
1
tan 1
2 1
2
2
2
2
1
2 2
1
cos
2
q q q
q l
l
q sen l
y
x
q
l l
l l y x
q
+ =
+

=
+

=
|
|

\
|
|

\
|
|
|
|

\
|

(3)

3) Jacobiano Inverso

Esta matriz nos permitir calcular la velocidad de cada
union, l clculo de la matriz Jacobiana directa se muestra
en la Eq. 4.

(
(
(
(
(
(
(
(

+ + +
+ +
=
1
)
2
(
2
)
1
(
)
2
(
2
)
1
(
0
)
2
(
2 1
)
2 1
(
2
)
1
(
1
)
2
(
2 1
)
2 1
(
2
)
1
(
1
0
)
2
(
1
)
2 1
(
)
2
(
1
)
2 1
(
q Sen l
q Sen
q Sen l
q Cos
q Sen l l
q q Sen l q Sen l
q Sen l l
q q Cos l q Cos l
q Sen l
q q Sen
q Sen l
q q Cos
Ji
(4)


4) Esttica

El Jacobiano transpuesta es obtenido relacionando
distancias diferenciales y velocidades angulares, esto para
obtener el torque necesario en cada actuador Eq.5..

(
(
(

(
(
(

(
(
(

=
M
y
F
x
F
q l q l q l q l
q l q l q l q l
q l q l
.
0 )
2
cos( .
2
)
1
cos( .
1
)
1
sin( .
1
)
2
sin( .
2
1 )
2
cos( .
2
)
1
cos( .
1
)
1
sin( .
1
)
2
sin( .
2
0 )
1
cos( .
1
)
1
sin(
1
3
2
1

(5)


Para obtener el modelo dinmico del robot haremos uso
del toolbox Simmechanics de Simulink
6
. Una alternativa
muy til es importar el diseo realizado en SolidWork a
Simulink. La figura 4 muestra ambas piernas de nuestro
robot bpedo modelado en Simmechanics, esta herramienta
tambin nos permite simular la cinemtica y dinmica del
robot, la Fig 4. muestra de forma grfica el resultado de este
modelamiento.

Fig. 4. Diagrama del modelado con Simmechanics de Simulink, Robot
Bpedo.

5) Dinmica

La dinmica del Robot esta basada en la ecuacin general
de movimiento Eq.6.

u T q G q q C q q H . ) ( ) , ( ) ( = + +
(6)

Clculo de la Matriz de Inercia Eq. 7

2 1 2 1 3 2 1 2 1 2
+

= C l l m C
c
l l m h




.
3
0 0
0
2
2
2 . 3
2
2 2
0
1
2
1 . 3
2
1 2 1
.
2
1
(
(
(
(

+ +
+ + +
=
I
I l m
c
l m h
h I l m l m m
c
l
H

(7)

Clculo de la Matriz de Coriolis Eq.8.

.
0 0 0
0
1
.
2 1
).
2
.
3 2
.
2
(
1 2
1
)
2
.
2 1
.
2
1
1
.
2 1
).(
2
.
3 2
.
2
(
1
0 )
2
.
2 1 1
.
2 1 2
1
).(
2
.
3 2
.
2
(
1 2
.
2 1
).
2
.
3 2
.
2
(
1 2
1
) , (
(
(
(
(
(

+
= q S l m
c
l m l q S q S l m
c
l m l
q S q S l m
c
l m l q S l m
c
l m l
q q C

(8)

Luego de obtener la matriz de Inercia (H) y Coriolis (C),
estos sern descritos en cdigo script, luego el sistema
puede ser simulado matemticamente


Fig. 5. Simulacin del Sistema en Cdigo Script

Para un anlisis ms real debemos tener en cuenta el
efecto generado por el peso de cada eslabn de nuestro
mecanismo y cmo cambian de posicin los centros de
gravedad de los mismos, para ello se vio la necesidad de
aadir una compensacin de la gravedad, este factor deber
sumarse a la seal de salida del control PID, a esta
compensacin de gravedad tambin se le incluye un ruido
blanco que nos servir como perturbacin adicional para
nuestro controlador, la figura 6 muestra el modelo en
Simulink del sistema incluyendo las caractersticas de
control mencionadas.

III. CONTROL

A. Optimizacin del control PID mediante lgica difusa

El problema comn del control PID est en la
sintonizacin de cada uno de los parmetros, pues nuestro
sistema tendra 18 parmetros para sintonizar, para resolver
este inconveniente, se decidi usar algoritmos de Lgica
Difusa

1) Funciones de Pertenencia:

Se usara funciones de pertenencia de tipo triangular y
trapezoidal para denotar al error, diferencial del error y a
nuestra salida u, tal como se muestra en la Fig. 6


Fig. 6. Funciones de pertenecia

Usaremos Mandani como sistema difuso, adems de las
reglas de pertenencia posibles Fig. 7.


Fig. 7. Reglas de pertenencia

Una vez diseado nuestro controlador, cambiaremos el
PID inicial por el F PD, como se muestra en la Fig 8.

Fig. 8. Control F-PD

IV. CAPTURA DE PATRONES DEL MOVIMIENTO HUMANO

A. Captura de data

Luego de disear nuestro robot, nuestro efector final
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 caractersticas humanas al caminar,
para que luego nuestro robot trate de imitarlo.
Para lograr dicho objetivo se ubic un LED en cada
articulacin y se grab la caminata en video, cada LED
debe ser de un color distinto para luego poder filtrarlos,
para que este proceso sea ms 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. Ubicacin 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 debern ser escaladas en la misma
relacin 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 posicin
inicial de este punto.
Luego las grficas independientes para cada articulacin
sern:

(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, debern cambiarse por otras en las que el
punto fijo sea la cadera, es decir cada vector de posicin de
la trayectoria del tobillo deber ser restado de la posicin
respectiva de la cadera.

B. Programacin del DSP orientado a un RTOS para
sistemas embebidos.

La intensin inicial de esta metodologa de programacin
radica en que nos facilita la gestin 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 impresin de que muchos procesos se ejecutan
simultneamente.
El robot bpedo debe controlar inicialmente 6 motores
con encoders, y un control PID para cada uno, adems
luego se le aadirn sensores de fuerza en las plantas de los
pies y acelermetros de tres ejes en la cadera, adems de
una interfaz de monitoreo de los parmetros del robot desde
una PC, esta cantidad de recursos justifica el diseo de un
RTOS, aprovechando adems las ventajas de rapidez
caractersticos de los DSPs.
La idea de la programacin 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 pequeo 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 basndose en una prioridad asignada a la
misma
10
.
En la tabla 2 mostramos las principales tareas de control de
motores definidas, adems se incluy una tarea propia para
controlar al RTOS, la cual se muestra en la figura 9, donde
tambin podemos apreciar que al iniciar la ejecucin 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 cdigos en C de este programa se muestran en
el Anexo A.0

TABLA 2
DEFINICIN DE TAREAS DEL RTOS PARA EL CONTROL DE MOTORES
Tareas Inicializacin Funcin principal
Motor_Cadera_izq_Task Motor_Cadera_izq_Init Motor_Cadera_izq_Run
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


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

De acuerdo a lo mencionado en el punto 2, se eligi una
trayectoria que asemeja a la de un paso cuasiesttico para
nuestro efector final, luego sintonizaremos los parmetros
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 parmetros:

A. Articulacin 1:Kp1=210, Ki1=10, Kd1=15
B. Articulacin 2 :Kp2=80, Ki2=10, Kd2=2
C. Articulacin 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 genticos, empleando redes
neuronales que puedan entrenar las constantes ptimas para
el control PID.


Fig. 12. Secuencia de estados del robot bpedo para seguir una trayectoria
lineal determinada.


(a)

(b)


(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 posicin angular de los motores, estos se muestran en
la Fig 13, notamos que se llega a un tiempo establecimiento
en corto tiempo.



(a)

(b)
Fig. 14. Diagramas que muestran buen funcionamiento de nuestro
controlador PID (a) Seales de errores en cada actuador. (b) Seal de
errores superpuestos.

Para poder poner a prueba nuestros resultados obtenidos,
se implemento el siguiente prototipo. Fig 15.


Fig. 15. Implementacin del prototipo


VI. CONCLUSIONES

Se logr modelar el robot bpedo usando herramientas de
SolidWork y de Simulink conjuntamente.
Existe una buena perfomance en el uso de un controlador F
- PD.
Es necesario usar tcnicas avanzadas para el diseo de las
funciones de pertenencia en el control Fuzzy, para as
obtener una respuesta rpida.
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.
[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 robotsa 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] Pgina de Matlab, se tiene informacin 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. Programacin de Sistemas Embebidos en
tiempo en lenguaje C, Editorial Alpha Omega. 2009.





























ANEXO-A.0


PROGRAMA PRINCIPAL

#include "inclusiones.h"

void main(void)
{
DSP_Init();
Motor_Cadera_Izq_Init();
Motor_Cadera_Der_Init();
Motor_Rodilla_Izq_Init();
Motor_Rodilla_Der_Init();
Motor_Tobillo_Izq_Init();
Motor_Tobillo_Der_Init();
ENABLE_INTERRUPTS;
while(TRUE)
{
DSP_Run();
Motor_Cadera_Izq_Init();
Motor_Cadera_Der_Init();
Motor_Rodilla_Izq_Init();
Motor_Rodilla_Der_Init();
Motor_Tobillo_Izq_Init();
Motor_Tobillo_Der_Init();
}
}

CONTROL DE TIEMPOS USANDO TIMERS
interrupt void cpu_timer0_isr(void){
static char escala_base_tiempo = 0; // Calibrar este valor,

CpuTimer0.InterruptCount++;

// Limpiamos el contador
if( CpuTimer0.InterruptCount == TOTAL_PULSOS_PERIODO ) CpuTimer0.InterruptCount =
0;
//PIN_INT = ~PIN_INT;
escala_base_tiempo ++;
if(escala_base_tiempo <= 50){
escala_base_tiempo = 0;
BASE_TIEMPO ++;
}

PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
MAQUINA DE ESTADOS PARA UNA TAREA DEL RTOS

/* Esta tarea controla al servomotor de la cadera del motor izquierdo, la posicin angular
se actualiza por entrada SCI la cual es afectada en el estado interrumpido.*/
//*******************************************************************************
**

#include "inclusiones.h"

// Definiciones de estado de tareas
#define __MOTOR_CAD_IZQ
#ifdef __MOTOR_CAD_IZQ

#define PRIORIDAD_MOTOR_CAD_IZQ = 10;

#define STD_SUSPENDIDO 0
#define STD_DISPONIBLE 1
#define STD_ESPERA 2
#define STD_EJECUCION 3
#define STD_INTERRUMPIDO 4

#define CAD_IZQ 0 // posicion asignada en el vector buffer_RX de SCI.h

// Definiciones para el servomotor
#define PIN_MOTOR_CAD_IZQ GpioDataRegs.GPBDAT.bit.GPIOB0
#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;

// Variables del fichero
static unsigned char std_cad_izq = STD_SUSPENDIDO;
static unsigned char pulso = 5; // 3 < pulso < 10
//*******************************************************************************
*******

// MAQUINA DE ESTADOS

void Motor_Cadera_Izq_Run(void){
static unsigned long int base_tiempo_motor_cad_izq = 0;

if(flag_buffer_full = 1) std_cad_izq = STD_INTERRUMPIDO;

switch(std_cad_izq){
case STD_SUSPENDIDO:
std_cad_izq = STD_SUSPENDIDO;
break;


case STD_DISPONIBLE:
base_tiempo_motor_cad_izq = BASE_TIEMPO;
std_cad_izq = STD_ESPERA;

break;
case STD_ESPERA:
if(BASE_TIEMPO - base_tiempo_motor_cad_izq > PRIORIDAD_MOTOR_CAD_IZQ)
std_cad_izq = STD_EJECUCION;
else std_cad_izq = STD_ESPERA;
break;
case STD_EJECUCION:
Motor_Cadera_Izq_Task();

break;
case STD_INTERRUMPIDO:
pulso = buffer_RX[CAD_IZQ];
std_cad_izq = STD_ESPERA;
break;
default: // falta implementar
break;
}
}

// EJECUCIN PRINCIPAL Control del ancho de pulso del PWM para el servomotor
void Motor_Cadera_Izq_Task(void){
if(CpuTimer0.InterruptCount < pulso){ // Pulso se actualiza por lectura del SCI
PIN_MOTOR_CAD_IZQ = 1;
}
else if (CpuTimer0.InterruptCount >= pulso){
PIN_MOTOR_CAD_IZQ = 0;
}
}

#endif












MONITOREO DE LOS SERVOMOTORES MEDIANTE EL MDULO SCI

void Motor_Cadera_Izq_Run(void){
static unsigned long int base_tiempo_motor_cad_izq = 0;

if(flag_buffer_full = 1) std_cad_izq = STD_INERRUMPIDO;

switch(std_cad_izq){
case STD_SUSPENDIDO:
std_cad_izq = STD_SUSPENDIDO;
break;
case STD_DISPONIBLE:
base_tiempo_motor_cad_izq = BASE_TIEMPO;
std_cad_izq = STD_ESPERA;

break;
case STD_ESPERA:
if(BASE_TIEMPO - base_tiempo_motor_cad_izq > PRIORIDAD_MOTOR_CAD_IZQ)
std_cad_izq = STD_EJECUCION;
else std_cad_izq = STD_ESPERA;
break;
case STD_EJECUCION:
Motor_Cadera_Izq_Task();

break;
case STD_INTERRUMPIDO:
pulso = buffer_RX[CAD_IZQ];
std_cad_izq = STD_ESPERA;
break;
default: // falta implementar
break;
}
}





ANEXO-A.1









ANEXO-A.2