Código Controladora servos.

Código Controladora servos.
/*************************************************************************************************************
Control de 8 servomotores desde el Pc o uC basado en los ejemplos de las webs "Http://www.garcia-cuervo.net",
"Http://www.iearobotics.com", "Http://www.jvmbots.com".
*************************************************************************************************************/
#include <pic1687x.h>
#define servo0 RB0
#define servo1 RB1
#define servo2 RB2
#define servo3 RB3
#define servo4 RB4
#define servo5 RB5
#define servo6 RB6
#define servo7 RB7
#define centro 106
unsigned char x,num_servo_serie,serie,control_serie;
unsigned char v_servo[8]={0,0,0,0,0,0,0,0};
unsigned char servo,control_t,control,flag,flagservo,flagservo1,flagservo2,flagservo3,flagservo4,flagservo5;
unsigned char flagservo6,flagservo7,flagservo8,flagservo9;
void main (void)
{
ADCON1=0x06;
TRISA=0x00;
TRISB=0x00; //Salida
TRISC=0xBF;
RCSTA=0x90;
TXSTA=0x06;
SPBRG=25;
RCIE=1;
INTCON=0xE0; //GIE=TOIE=PEIE=1
OPTION=0x83;
/********************************************************************************************
Prescaler a 16, con el cual tenemos una interrupcion cada 4ms. Con la cual podemos conseguir
una buena resolucion para los servos. Incrementando los valores de 1 en 1.
********************************************************************************************/
PORTB=0x00;
flag=0;
flagservo=0;
control=0;
control_t=0;
for (x=0;x<8;x++)
{
v_servo[x]=centro;
}
do
{
if (flag==1)
{
flag=0;
servo0=1;
servo1=1;
servo2=1;
servo3=1;
servo4=1;
servo5=1;
servo6=1;
servo7=1;
flagservo=1;
flagservo1=1;
flagservo2=1;
flagservo3=1;
flagservo4=1;
flagservo5=1;
flagservo6=1;
flagservo7=1;
}
//"flagservo" podria estar dentro de un array, al igual que servoX?
if (flagservo==1)
{
//(TMR0>entrada_serie[x]), en un byte el numero de servo, y en otro el valor
if (TMR0>v_servo[0])
{
flagservo=0;
servo0=0;
}
}
if (flagservo1==1)
{
if (TMR0>v_servo[1])
{
flagservo1=0;
servo1=0;
}
}
if (flagservo2==1)
{
if (TMR0>v_servo[2])
{
flagservo2=0;
servo2=0;
}
}
if (flagservo3==1)
{
if (TMR0>v_servo[3])
{
flagservo3=0;
servo3=0;
}
}
if (flagservo4==1)
{
if (TMR0>v_servo[4])
{
flagservo4=0;
servo4=0;
}
}
if (flagservo5==1)
{
if (TMR0>v_servo[5])
{
flagservo5=0;
servo5=0;
}
}
if (flagservo6==1)
{
if (TMR0>v_servo[6])
{
flagservo6=0;
servo6=0;
}
}
if (flagservo7==1)
{
if (TMR0>v_servo[7])
{
flagservo7=0;
servo7=0;
}
}
}
while(1);
}
void interrupt recepcion_serie (void)
{
if (T0IF==1) //Desbordamiento cada 4 milisegundos, necesitamos una base de tiempo de 20ms
{
T0IF=0;
control_t++;
if (control_t==4)
{
TMR0=30;
}
if (control_t==5)
{
flag=1;
control_t=0;
}
}
/***********************************************************************************************************
Control de la recepción serie, el protocolo consta de 2 Bytes, el primer byte indica el servo que queremos
gobernar, y el segundo la posición a la que deseamos que se situe el servo. La variable "Control_serie"
sirve para controlar el byte que nos llega, y así poder controlar la información recibida.
***********************************************************************************************************/
if (RCIF==1)
{
control_serie++;
if (control_serie==1)
{
serie=RCREG;
v_servo[serie];
}
if (control_serie==2)
{
control=RCREG;
v_servo[serie]=control;
control_serie=0;
serie=0;
}
}
}