A falta de algunas cosas, tengo el codigo más o menos listo, a lo que mover los motores se refiere.
Creo que habia dos opciones básicas, controlar de estos el sentido y el nº de pasos que da, o controlar la posición de estos, he elegido la segunda porque básicamente (puede y de hecho contiene la primera), con la primera "a pelo" se podría indicar mover el motor en un sentido que ya llego al tope por limitación de la mecánica del brazo. Cuando haga pruebas "físicas" con el brazo le agregaré al programa los rangos de posición para impedir esto.
El programa es inmensamente sencillo, se trata de multiplexar los motores PaP, además esta yo creo, excesivamente comentado para ser todo entendido por culaquiera que le caiga en sus manos (debe ser así para cuando deba exponerlo ante los profesores, que por que no decirlo ... algunos no esque sean unos llumbreras!!). Va comparando la posición en la que se encuentra hasta la "posición nueva", la cual en el futuro sera ingresada por un programa via RS232.
En el ejemplo, el 1er motor da un paso (7.5º), el 2º 2 ...
///////////////////////////////////////////////////////////////////////////
//// RaDoN-Bot 877 ////
//// Por Daniel G - by RaDoN (12/04/06) ////
///////////////////////////////////////////////////////////////////////////
#include <16f877A.h> // Cabecera para el PIC16F877A
#fuses HS,NOWDT,PUT,NODEBUG,NOPROTECT,NOBROWNOUT,NOLVP
// Fusibles que se activarán cuando el dispositivo
// se programe. P.ej: NOLVP, NO Low voltage program.
// Indica que será programado con tensión alta (13.5V)
#use delay(clock=20000000) // Cristal 20 Mhz <-> 0.25 uS/instrucción
// requerida para la funciones:
// delay_us(x) y delay_ms(x)
#use rs232(baud=9600, xmit=pin_c6, rcv=pin_c7, bits=8)
//#ORG 0x1F00,0x1FFF {} //for the 8k 16F876/7 bootloader
#use fast_io(A) // Directiva que afecta al código que el compilador
#use fast_io(B) // generará para las instrucciones de entrada y salida.
#use fast_io(C) // Este método rápido de hacer I/O ocasiona que el
#use fast_io(D) // compilador realice I/O sin programar el registro
#use fast_io(E) // de dirección.
#byte port_a = 5 // Dirección del registro PORTA del PIC16F877 (05h)
#byte port_b = 6 // Dirección del registro PORTB del PIC16F877 (06h)
#byte port_c = 7 // Dirección del registro PORTC del PIC16F877 (07h)
#byte port_d = 8 // Dirección del registro PORTD del PIC16F877 (08h)
#byte port_e = 9 // Dirección del registro PORTE del PIC16F877 (09h)
#define cintura 0 // Nº de motor 0 (cero porque es el valor que apunta al
// primer elemento del vector -array-) -> RA0:3
#define hombro 1 // Nº de motor 1 -> RB0:3
#define codo 2 // Nº de motor 2 -> RB4:7
#define muneca_dcha 3 // Nº de motor 3 -> RC0:3
#define muneca_izq 4 // Nº de motor 4 -> RD0:3
#define pinza 5 // Nº de motor 5 -> RD4:7
/* Las siguientes tablas constan de 6 elementos, 1 para cada motor.
En las que se indican: la posición que ocupa, la posición a la que
se debe dirigir y el nº de la secuencia de paso que ocupa para ser
gobernado */
int16 posicion_pap[6] = {0,0,0,0,0,0}; // Posición de los motores 1-6 en pasos
int16 posicion_nueva[6] = {1,2,3,4,5,6}; // Posición a la que se deben
// dirigir los PaP recibida por la UART
int8 secuencia_pap[6] = {0,0,0,0,0,0}; // Nº de la secuencia del paso (8 pasos)
// de cada motor
/***********************************************************************/
const int8 pasos[8] = {2,6,4,12,8,9,1,3}; // Valores para activar las bobinas:
// secuencia de medio paso.
/*#int_rda
void serial_isr()
{
int num;
if(kbhit())
{
num = getc();
printf("%u\n\r", num);
}
}*/
/***********************************************************************/
// Esta funcion compara la posición del motor paso a paso correspondiente,
// con la indicada por el programa de control mediante el puerto serie (UART).
// En caso de ser diferente irá moviendolo secuencialmente y en forma
// multiplexada hasta que este ocupe la posición requerida.
/***********************************************************************/
void mover_motor(int motor) // Motor 0-6; Cintura, hombro, codo,
// muñeca_dcha, muneca_izq, pinza
{
if(posicion_pap[motor] == posicion_nueva[motor]) // Si el PaP esta en la
// posición requerida,
// mantén las bobinas
// inactivas.
{
switch(motor)
{
case cintura: port_a = port_a & 0xF0; // Limpiamos el nibble inferior
break;
case hombro: port_b = port_b & 0xF0; // Limpiamos el nibble inferior
break;
case codo: port_b = port_b & 0x0F; // Limpiamos el nibble superior
break;
case muneca_dcha: port_c = port_c & 0xF0; // Limpiamos el nibble inferior
break;
case muneca_izq: port_d = port_d & 0xF0; // Limpiamos el nibble inferior
break;
case pinza: port_d = port_d & 0x0F; // Limpiamos el nibble superior
break;
}
}
else // Si el PaP NO esta en la posición requerida, lo muevo y actualizo:
// posición y secuencia (fase en la que se encuentra).
{
switch(motor)
{
case cintura: port_a = pasos[secuencia_pap[motor]] | (port_a & 0xF0);;
break;
case hombro: port_b = (pasos[secuencia_pap[motor]]) | (port_b & 0xF0);
break;
case codo: port_b = (pasos[secuencia_pap[motor]] << 4) | (port_b & 0x0F);
break;
case muneca_dcha: port_c = (pasos[secuencia_pap[motor]]) | (port_c & 0xF0);
break;
case muneca_izq: port_d = (pasos[secuencia_pap[motor]]) | (port_d & 0xF0);
break;
case pinza: port_d = (pasos[secuencia_pap[motor]] << 4) | (port_d & 0x0F);
//printf("%Lu --> ", posicion_pap[motor]);
//printf("%u\n\r", secuencia_pap[motor]);
break;
}
if(posicion_pap[motor] > posicion_nueva[motor])
{
--posicion_pap[motor];
if(secuencia_pap[motor] == 0) secuencia_pap[motor] = 7;
else --secuencia_pap[motor];
}
if(posicion_pap[motor] < posicion_nueva[motor])
{
++posicion_pap[motor];
if(secuencia_pap[motor] == 7) secuencia_pap[motor] = 0;
else ++secuencia_pap[motor];
}
}
}
/**********************************************************************/
// Función principal
void main(void)
{
int i;
set_tris_a(0xF0); // RA7:4 entradas, RA3:0 salidas
set_tris_b(0x00); // RB7:0 salidas
set_tris_c(0xF0); // RC7:4 entradas, RC3:0 salidas
set_tris_d(0x00); // RD7:0 salidas
//enable_interrupts(int_rda);
//enable_interrupts(global);
while(1)
{
for(i=0;i<=5;++i){
mover_motor(i);}
delay_ms(500);
}
}
De momento la demostración con Isis (Proteus)
http://www.megaupload.com/es/?d=H3WZIJ01
