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

e quiere lograr el modelamiento y una estrategia de
control del robot, probando su estabilidad y robustez,
para 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 System
7

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
Hanover
1
.

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
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
S


circuitería necesaria para cuando implementemos el robot,
en esta plataforma también deberán ubicarse las baterías del
robot. Estas características mencionadas se muestran en la
figura 1.

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
elemento.

TABLA I
CARACTERÍSTICAS DIMENSIONALES DEL MECANISMO DEL ROBOT BÍPEDO

Dimensiones
(mm)
Peso
(gr)
Cant.
Peso Total
(gr)
Pie 200 x 130 685 1 685
Tibia 400 700 1 700
Fémur 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 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
sistema, para ello además del mecanismo y los motores,
debemos diseñar la interfaz electrónica propia que incluye
como principal dispositivo al DSP, además debe añadirse al
circuito otras interfaces, como los drivers que permitirán el
control de los motores. Esta circuito electrónico deberá
ubicarse en la plataforma superior del robot junto con las
baterías, el esquema electrónico diseñado se muestra en el
anexo A.2.

1) Microprocesador

Se hará uso del procesador digital de señales DSP
TMS320F2812 de Texas Instrument
2
, debido a su
capacidad de procesamiento y una respuesta rápida para los
6 actuadores que se utilizaran. Haremos uso del módulo
eZdsp de Spectrum Digital
3
, el cual se muestra en la figura
2, éste módulo incluye pines de entrada y salida, memoria
RAM externa, además de conexión JTAG él cual con ayuda
del emulador XDS100 de Spectrum Digital nos permitirá
emular el funcionamiento del sistema en tiempo real.


Fig. 2. Módulo del Procesador Digital de Señales (eZdsp).

2) Modelamiento

Para el modelamiento dinámico 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 método de análisis 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
Fémur
Pie Articulación
del pie
Articulación
de la rodilla
Articulación
de la cadera
Plataforma
superior


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

1) Cinemática directa

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.

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: Posición del efector final (base del pie) relativa a la
cadera
α : Orientación del efector final
q1: Posición angular de la articulación ubicada en la
cadera.
q2: Posición angular de la articulación ubicada en la
rodilla.
q3: Posición angular de la articulación ubicada en el
tobillo.
l1: Longitud del eslabón fémur.
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 deseada
7
, 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.

) 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 cálculo 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) Estática

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 dinámico del robot haremos uso
del toolbox Simmechanics de Simulink
6
. Una alternativa
muy útil es importar el diseño realizado en SolidWork a
Simulink. La figura 4 muestra ambas piernas de nuestro
robot bípedo modelado en Simmechanics, esta herramienta
también nos permite simular la cinemática y dinámica del
robot, la Fig 4. muestra de forma gráfica el resultado de este
modelamiento.

Fig. 4. Diagrama del modelado con Simmechanics de Simulink, Robot
Bípedo.

5) Dinámica

La dinámica del Robot esta basada en la ecuación general
de movimiento Eq.6.

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

Cálculo 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)

Cálculo 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 serán descritos en código script, luego el sistema
puede ser simulado matemáticamente


Fig. 5. Simulación del Sistema en Código Script

Para un análisis más real debemos tener en cuenta el
efecto generado por el peso de cada eslabón de nuestro
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
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
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, además de las
reglas de pertenencia posibles Fig. 7.


Fig. 7. Reglas de pertenencia

Una vez diseñado 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 diseñar 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 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
misma
10
.
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
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 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.


Fig. 12. Secuencia de estados del robot bípedo 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 posición 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) 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

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.
[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.





























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 posición 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;
}
}

// EJECUCIÓN 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 MÓDULO 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




Sign up to vote on this title
UsefulNot useful