Robot seguidor

[filth]

Hola a todos, lo primero darle las gracias a RedPic por su ayuda, me ha librado de muchos comederos de cabeza.

Mi proyecto consiste en el desarrollo de un robot que mantenga una distancia constante a un objeto móvil. Para medir la distancia he utilizado un sensor de ultrasonidos SRF04. También he utilizado un robot comercial, el Mark III.

Aclarar que he utilizado código de RedPic (muchas, muchas gracias) y lo he modificado para esta aplicación en concreto.

A continuación intentaré detallar un poco el funcionamiento del programa:

• He utilizado un 16F877 con oscilador de 20MHz
• La actuación sobre los servos es proporcional a la distancia medida por el sensor de ultrasonidos
• La atención al SRF04 es por interrupción en el pin RB0 (RedPic tenías razón: podía utilizar este)
• Se realizan medidas consecutivas respetando los tiempos del sensor y se modifica la actuación sobre los servos en función de una media de las medidas tomadas. En este caso he puesto 10, pero podría ser cualquiera

Bueno pues aquí os dejo el código...

Código: [Seleccionar]

/////////////////////
// Configuraciones //
/////////////////////

#include <16f877.h>
#fuses HS,NOWDT,NOPROTECT,NOLVP
#use delay(clock=20000000)
#use rs232(baud=38400, xmit=PIN_C6, rcv=PIN_C7)

#include <stdlib.h>

///////////////////////////////
// Constantes y definiciones //
///////////////////////////////

// SRF04
#define SRF04_PORT_TRIGGER PIN_D7                // Pin usado para el pulso de Disparo
#define SRF04_PORT_ECHO    PIN_B0                // Pin usado para recibir el eco ultrasónico

#define MAX_ECHO_RESPONSE_Ticks 25000            // Maximo tiempo de espera del eco en Ticks de Timer1 (20 ms)
#define CALC_DISTANCE(t) (t / 73.52)             // Convierte de Ticks a distancia
#define DISTANCE_INFINITE (0xffff)               // Distancia infinita, no hay obstaculo en el rango de medida

// SERVOS
#define PIN_SERVO1 PIN_B1
#define PIN_SERVO2 PIN_B2

////////////////////////
// Variables Globales //
////////////////////////

// Variables control SRF04
long echo_delay;
float distance = DISTANCE_INFINITE;
int1 flagHayDatos=0;
int16 t1=0x00,t2=0x00,tt=0x00;
float st=0.0;
int1 hay_flanco_de_bajada=0;
int1 flagToggleFlanco=0;       // Flag para cambiar de flanco
char Keypress=0x00;

// Variables control SERVOS
int ValTIMER0;
const int AJUSTE_FINO_DE_RTCC =  243;
int1 flagRTCC   = 0;
int  contRTCC   = 0;
int1 flagSERVO1 = 0;
int1 flagSERVO2 = 0;
// Periodos predefinidos
const int ticks_PULSO_ATRAS_RAPIDO_SERVO1 = 97;
const int ticks_PULSO_ATRAS_RAPIDO_SERVO2 = 20;
const int ticks_PULSO_ATRAS_LENTO_SERVO1 = 62;
const int ticks_PULSO_ATRAS_LENTO_SERVO2 = 56;
const int ticks_PULSO_PARADO = 59;
const int ticks_PULSO_ADELANTE_RAPIDO_SERVO1 = 20;
const int ticks_PULSO_ADELANTE_RAPIDO_SERVO2 = 97;
const int ticks_PULSO_ADELANTE_LENTO_SERVO1 = 56;
const int ticks_PULSO_ADELANTE_LENTO_SERVO2 = 62;
int  tSERVO1;
int  tSERVO2;

// Variables de control general
int orden=1;                     // Controla el flujo del proceso
int cont_timer1=0;               // Contador de desbordamientos de Timer1
float suma=0.0;                  // Suma las medidas realizadas
int cont_medida=0;               // Contador de medidas realizadas
float media=0.0;                 // Media de las medidas
int1 ya=0;                       // Bit que indica cuando puedo tengo calculada una media

////////////////////
// INTERRUPCIONES //
////////////////////

#int_TIMER1
void siyatehagocaso(){
   cont_timer1++;

   // Espera
   if(orden==2){        // Desbordamiento del Timer1 mientras espera entre pulsos
      if(cont_timer1==1){
         set_timer1(6072);
      }
      if(cont_timer1=2){
         cont_timer1=0;    // Espero 125000 ticks en total lo que son 0.1 segundos
         orden=3;
      }
   }

   // Disparo
   if(orden==4){        // Desbordamiento del Timer1 mientras mando el pulso de disparo
      output_low(SRF04_PORT_TRIGGER);                // Termino el pulso
      orden=1;                                       // Reinicio el proceso
   }
}

#int_ext
void handle_ext_int(){

   if(flagToggleFlanco==0){   // He recibido Flanco de Subida
      t1=get_timer1();        // Guardo en t1 el valor de TMR1 al Flanco de Subida
      ext_int_edge(0,H_TO_L); // Configuro para capturar siguiente flanco de Bajada
      flagToggleFlanco=1;     // Indico que el siguiente flanco será de Bajada

   } else {                   // He recibido Flanco de Bajada
      t2=get_timer1();        // Guardo en t2 el valor de TMR1 al Flanco de Bajada
      ext_int_edge(0,L_TO_H); // Configuro para capturar siguiente flanco de subida
      flagToggleFlanco=0;     // Indico que el siguiente flanco será de Subida
      set_timer1(0);          // Reinicio TMR1
      if(flagHayDatos==0){    // Si los datos anteriores han sido procesados ...
         flagHayDatos=1;      // Indico que ya hay nuevos datos de flancos para calcular
      }
   }
}

#int_RTCC
RTCC_isr(){

   ++contRTCC;
   if(contRTCC==3){
      set_TIMER0(AJUSTE_FINO_DE_RTCC);
   }
   if(contRTCC==4){
      flagRTCC=1;
      contRTCC=0x00;
   }
}

///////////////////////////
// Funciones de programa //
///////////////////////////

// Configuración inicial
void Configuracion(void){
   setup_counters(RTCC_INTERNAL,RTCC_DIV_128);
   output_low(SRF04_PORT_TRIGGER);               // A bajo el pin del disparador
   setup_timer_1(T1_INTERNAL | T1_DIV_BY_4);     // Timer1 con preescaler 1:4

   ext_int_edge(0,L_TO_H);     // Configuro captura de 1er flanco de subida
   flagToggleFlanco = 0;       // inicializo el Flag para cambiar de flanco
   enable_interrupts(int_ext); // Habilito las interrupciones necesarias
   enable_interrupts(int_TIMER1);
   enable_interrupts(INT_RTCC);
   enable_interrupts(global);                    // todas las interrupciones activadas

   printf("CORRE CORRE QUE TE PILLO\r\n\n");

   tSERVO1=ticks_PULSO_PARADO;      // Inicio con los servos parados
   tSERVO2=ticks_PULSO_PARADO;
}

// Control del PWM para mover los servos
void Control_PWM(void){
   // DISPARO DEL PULSO PWM
   if(flagRTCC==1){
      flagRTCC=0;
      output_high(PIN_SERVO1);
      output_high(PIN_SERVO2);
      flagSERVO1=1;
      flagSERVO2=1;
   }

   // CONTROL DE ANCHO DEL PULSO PWM
   if(flagSERVO1==1){
      valTIMER0 = get_TIMER0();
      if(valTIMER0>tSERVO1){
         flagSERVO1=0;
         output_low(PIN_SERVO1);
      }
   }
   if(flagSERVO2==1){
      valTIMER0 = get_TIMER0();
      if(valTIMER0>tSERVO2){
         flagSERVO2=0;
         output_low(PIN_SERVO2);
      }
   }
}

// Modifico el ancho de pulso de los servos
void Configura_Servos(void){
   if(media<85){
      tSERVO1=ticks_PULSO_ATRAS_RAPIDO_SERVO1;
      tSERVO2=ticks_PULSO_ATRAS_RAPIDO_SERVO2;
   }
   if(media>85 && media<95){
      tSERVO1=ticks_PULSO_ATRAS_LENTO_SERVO1;
      tSERVO2=ticks_PULSO_ATRAS_LENTO_SERVO2;
   }
   if(media>95 && media<105){
      tSERVO1=ticks_PULSO_PARADO;
      tSERVO2=ticks_PULSO_PARADO;
   }
   if(media>105 && media<115){
      tSERVO1=ticks_PULSO_ADELANTE_LENTO_SERVO1;
      tSERVO2=ticks_PULSO_ADELANTE_LENTO_SERVO2;
   }
   if(media>115){
      tSERVO1=ticks_PULSO_ADELANTE_RAPIDO_SERVO1;
      tSERVO2=ticks_PULSO_ADELANTE_RAPIDO_SERVO2;
   }
   ya=0;       // Indico que ya he utilizado la media
}

// Procesa los datos devueltos por la interrupción en RB0
void Procesa_Datos(void){
   if(t2 > t1){                       // Compruebo que estoy en la misma vuelta de TMR1
      tt = t2 - t1;                   // Calculo en Tick's de TMR1 el tiempo entre flancos
      if (tt >= MAX_ECHO_RESPONSE_Ticks){
         distance = DISTANCE_INFINITE;
      }
      else {
         distance = CALC_DISTANCE(tt);
      }
      flagHayDatos=0;                     // Indico que ya han sido procesados los datos.
      suma=suma+distance;                 // Sumo cada medida a las anteriores
      cont_medida++;                      // Cuento la nueva medida
      if(cont_medida==10){                // Cuando llevo x, calculo la media
         media=suma/cont_medida;          // Calculo la media
         suma=0.0;                        // Pongo a cero el valor de suma
         cont_medida=0;                   // Pongo a cero el contador de medidas
         ya=1;                            // Indico que ya he calculado una media
      }
   }
}
/////////////////////////////
// MAIN Programa Principal //
/////////////////////////////

void main(void) {

   Configuracion();

   do {
      Control_PWM();

      // Espera para mandar otro pulso
      if (orden==1){
         set_timer1(0);
         orden=2;
      }

      // Envio del pulso ultrasónico
      if(orden==3){
         set_TIMER1(65523);                  // Precargo el Timer para tener 10.4 uS
         output_high(SRF04_PORT_TRIGGER);
         orden=4;
      }

      // Proceso datos recibidos
      if(flagHayDatos==1){                  // Detecto que ya hay datos de flancos ...
         Procesa_Datos();
      }

      // Control de los SERVOS
      if(ya==1){
         printf("\r\nMedia -> %3.2f cm \r\n",media);  // Muestro la media en pantalla
         // Decido en función del valor de la media
         Configura_Servos();
      }
   } while (TRUE);
}

Espero que a alquien le sea útil como para mi lo han sido otros post de este foro. Una vez más gracias por todo 

[LordLafebre]

Hola:

Felicidades  , seria bueno si nos pones unas fotitos o un video de su funcionameiento 

[Nocturno]

Eso, eso, fotos; que un proyecto sin fotos es como un jardín sin flores.

Felicidades por terminarlo.

[ElizabethBathory]

Hola chicos, necesito de su ayuda T_T  les cuento estoy haciendo el programa de un robot sumo para un torneo de la universidad.. pero la verdad muy poco se programar y muy poco se sobre pics.. me base en el codigo de redpic sobre el control pwm de los servos.. ya q mi carrito tendra 2 servos. ademas le pondre 1 cny70 y 4 bumper de contacto (creo q asi se llaman )..  usare el pic 16f84a, como entradas usare el puerto B (RB1,RB2,RB3,RB4,RB5) donde ubicare los sensores, y el puerto A sera donde conecte los servos.. RA1 y RA2. Bueno en el codigo que les muestro aun no he puesto el comportamiento del cny70. solo los bumpers y los servos. trate de compilarlo  pero me bota un error que dice asii:     
                                                     Executing: "C:\Program files\Picc\CCSC.exe" +FM "pwm-swt" +DF +LN +T +A +M -Z +Y=9 +EA
                                                      *** Error 18 "pwm-swt.c" Line 0(0,0): File can not be opened
                                                             File: pwm-swt.c
                                                      *** Error 1 "pwm-swt.c" Line 0(0,0): Illegal C character in input file
                                                            2 Errors,  0 Warnings.
                                                       BUILD FAILED: Fri Aug 13 12:46:12 2010

Entonces la verdad no entiendo que pasa.. que tengo mal.. por fis ayudenme.. quiero tener mi carrito listo pronto.. cualquier consejo se los agradeceria de corazon.
bye besos!

Código: [Seleccionar]

// Ejemplo con un servo FUTABA S3003
// Alimentación y pulsos a 5V

// Cuadro de Tiempos :

//    Periodo 20 ms (Frecuencia 50 Hz)
//    Ancho Pulso minimo 0.5 ms
//    Ancho pulso medio  1.5 ms
//    Ancho pulso maximo 2.5 ms

//    TMR0 a 1:16 -> 1 RTCC cada 4.096 ms
//                -> 1 Tick cada 0.096 / 256 = 0.016 ms
//                -> 20 ms = (4 x RTCC completas) + (1 * RTCC - 30 ticks)

//    Ancho Pulso minimo 0.5 ms ->  31 ticks de TMR0
//    Ancho pulso medio  1.5 ms ->  93 ticks de TMR0
//    Ancho pulso maximo 2.5 ms -> 155 ticks de TMR0



#include <16f84A.h>
#fuses XT,NOWDT
#use delay(clock=4000000)
#byte PORTA=5
#byte PORTB=6
#byte PWRTE=1
#byte PORT_B_PULLUPS(1)

#define PIN_SERVOD PIN_A1    //servo derecha
#define PIN_SERVOI PIN_A2         //servo izquierda

const int RTCC         =30;
const int Pulsominimo  =31;
const int Pulsomedio   =93;
const int Pulsomaximo =155;

int1 flagRTCC    =0;
int  contRTCC    =0;
int1 flagSERVOD  =0;
int1 flagSERVOI  =0;
int  tSERVOD     =Pulsomedio;
int  tSERVOI     =Pulsomedio;



void main ()
{
  int ValTIMER0;
  set_tris_A=0;
  set_tris_B=1;
  PortA=0;
  PortB=0;
 
 do
 {
     // DISPARO DEL PULSO PWM

      if(flagRTCC==1)
   {
         flagRTCC=0;
         output_high(PIN_SERVOD);
         output_high(PIN_SERVOI);
         flagSERVOD=1; 
         flagSERVOI=1;   
       }

      // CONTROL DE ANCHO DEL PULSO PWM
     
       if(flagSERVOD==1)
    {
             valTIMER0 = get_TIMER0();
          if(valTIMER0>tSERVOD)
        {
             flagSERVOD=0;
             output_low(PIN_SERVOD);
            }   
         }
 
        if(flagSERVOI==1)
     {
             valTIMER0 = get_TIMER0();
           if(valTIMER0>tSERVOI)
         {
             flagSERVOI=0;
             output_low(PIN_SERVOI);
            }   
         }
 
  if((bit_test(PORTB,2))||(bit_test(PORTB,4))==0)
   {
     tSERVOD=Pulsominimo;
     tSERVOI=Pulsominimo;
     delay_ms(20);
     tSERVOD=Pulsomedio;
     tSERVOI=Pulsomedio;
   }
  if((bit_test(PORTB,3))||(bit_test(PORTB,5))==0)
  {
    tSERVOD=Pulsomaximo;
    tSERVOI=Pulsomaximo;
    delay_ms(20);
    tSERVOD=Pulsomedio;
    tSERVOI=Pulsomedio;
  }
 }
}

[ElizabethBathory]

Hola de nuevo yo... uhm.. le hice cambios al codigo pero sigue sin compilarme.. no se que sucede, si me falta instalar algo o si tngo mal instalado el mplab con el ccs. ahora me manda este mensaje:

Build aborted.  The project contains no source files.
BUILD FAILED: Fri Aug 13 19:26:34 2010

si hay algunos errores bobos (lo mas seguro es q si...) disculpen mi novatada T_T

Código: [Seleccionar]

#include <16f84A.h>
#fuses TX,NOWDT
#use delay(clock=4000000)
#byte PORTA=5
#byte PORTB=6
#byte PORT_B_PULLUPS(TRUE)

#define PIN_SERVOD PIN_A1    //servo derecha
#define PIN_SERVOI  PIN_A2                       //servo izquierda

const int RTCC         =30;
const int Pulsominimo  =31;
const int Pulsomedio   =93;
const int Pulsomaximo =155;

int1 flagRTCC    =0;
int  contRTCC    =0;
int1 flagSERVOD  =0;
int1 flagSERVOI  =0;
int  tSERVOD     =Pulsomedio;
int  tSERVOI     =Pulsomedio;



void main ()
{
  int ValTIMER0;
  set_tris_A=0;
  set_tris_B=1;
  PortA=0;
  PortB=0;
 
 do
 {
     // DISPARO DEL PULSO PWM

      if(flagRTCC==1)
   {
         flagRTCC=0;
         output_high(PIN_SERVOD);
         output_high(PIN_SERVOI);
         flagSERVOD=1; 
         flagSERVOI=1;   
       }

      // CONTROL DE ANCHO DEL PULSO PWM
     
       if(flagSERVOD==1)
    {
             valTIMER0 = get_TIMER0();
          if(valTIMER0>tSERVOD)
        {
             flagSERVOD=0;
             output_low(PIN_SERVOD);
            }   
         }
 
        if(flagSERVOI==1)
     {
             valTIMER0 = get_TIMER0();
           if(valTIMER0>tSERVOI)
         {
             flagSERVOI=0;
             output_low(PIN_SERVOI);
            }   
         }
 
        if((bit_test(PORTB,2))||(bit_test(PORTB,4))==0)         // si RB2 O RB4 =0,entonces gire a la izquierda.
          {
            tSERVOD=Pulsominimo;
            tSERVOI=Pulsominimo;
             delay_ms(20);
            tSERVOD=Pulsomedio;                                         // avance servos
            tSERVOI=Pulsomedio;
          }
       
          if((bit_test(PORTB,3))||(bit_test(PORTB,5))==0)       // si RB3 O RB5 =0,entonces gire a la derecha.
            {
              tSERVOD=Pulsomaximo;
              tSERVOI=Pulsomaximo;
              delay_ms(20);
              tSERVOD=Pulsomedio;                                       // avance de servos
              tSERVOI=Pulsomedio;
            }

         if(bit_test(PORTB,1)==1)                                       // si RB1 =1,entonces avance.(cny70 superficie negra =1)
           {
             tSERVOD=Pulsomedio;
             tSERVOI=Pulsomedio;
           }
         if(bit_test(PORTB,1)==0)                                      // si RB1 =0,entonces gire izquierda (cny70 superficie blanca=0)
           {
             tSERVOD=Pulsomedio;
             tSERVOI=Pulsomedio;
           }
  }
}

[Abner_emanuel]

Un saludo mis amigos. Del primer código de este post.

/////////////////////
// Configuraciones //
/////////////////////

#include <16f877.h>
#fuses HS,NOWDT,NOPROTECT,NOLVP
#use delay(clock=20000000)
#use rs232(baud=38400, xmit=PIN_C6, rcv=PIN_C7)

#include <stdlib.h>

///////////////////////////////
// Constantes y definiciones //
///////////////////////////////

// SRF04
#define SRF04_PORT_TRIGGER PIN_D7                // Pin usado para el pulso de Disparo
#define SRF04_PORT_ECHO    PIN_B0                // Pin usado para recibir el eco ultrasónico

#define MAX_ECHO_RESPONSE_Ticks 25000            // Maximo tiempo de espera del eco en Ticks de Timer1 (20 ms)
#define CALC_DISTANCE(t) (t / 73.52)             // Convierte de Ticks a distancia
#define DISTANCE_INFINITE (0xffff)               // Distancia infinita, no hay obstaculo en el rango de medida

// SERVOS
#define PIN_SERVO1 PIN_B1
#define PIN_SERVO2 PIN_B2

////////////////////////
// Variables Globales //
////////////////////////

// Variables control SRF04
long echo_delay;
float distance = DISTANCE_INFINITE;
int1 flagHayDatos=0;
int16 t1=0x00,t2=0x00,tt=0x00;
float st=0.0;
int1 hay_flanco_de_bajada=0;
int1 flagToggleFlanco=0;       // Flag para cambiar de flanco
char Keypress=0x00;

// Variables control SERVOS
int ValTIMER0;
const int AJUSTE_FINO_DE_RTCC =  243;
int1 flagRTCC   = 0;
int  contRTCC   = 0;
int1 flagSERVO1 = 0;
int1 flagSERVO2 = 0;
// Periodos predefinidos
const int ticks_PULSO_ATRAS_RAPIDO_SERVO1 = 97;
const int ticks_PULSO_ATRAS_RAPIDO_SERVO2 = 20;
const int ticks_PULSO_ATRAS_LENTO_SERVO1 = 62;
const int ticks_PULSO_ATRAS_LENTO_SERVO2 = 56;
const int ticks_PULSO_PARADO = 59;
const int ticks_PULSO_ADELANTE_RAPIDO_SERVO1 = 20;
const int ticks_PULSO_ADELANTE_RAPIDO_SERVO2 = 97;
const int ticks_PULSO_ADELANTE_LENTO_SERVO1 = 56;
const int ticks_PULSO_ADELANTE_LENTO_SERVO2 = 62;
int  tSERVO1;
int  tSERVO2;

// Variables de control general
int orden=1;                     // Controla el flujo del proceso
int cont_timer1=0;               // Contador de desbordamientos de Timer1
float suma=0.0;                  // Suma las medidas realizadas
int cont_medida=0;               // Contador de medidas realizadas
float media=0.0;                 // Media de las medidas
int1 ya=0;                       // Bit que indica cuando puedo tengo calculada una media

////////////////////
// INTERRUPCIONES //
////////////////////

#int_TIMER1
void siyatehagocaso(){
   cont_timer1++;

   // Espera
   if(orden==2){        // Desbordamiento del Timer1 mientras espera entre pulsos
      if(cont_timer1==1){
         set_timer1(6072);
      }
      if(cont_timer1=2){
         cont_timer1=0;    // Espero 125000 ticks en total lo que son 0.1 segundos
         orden=3;
      }
   }

   // Disparo
   if(orden==4){        // Desbordamiento del Timer1 mientras mando el pulso de disparo
      output_low(SRF04_PORT_TRIGGER);                // Termino el pulso
      orden=1;                                       // Reinicio el proceso
   }
}

#int_ext
void handle_ext_int(){

   if(flagToggleFlanco==0){   // He recibido Flanco de Subida
      t1=get_timer1();        // Guardo en t1 el valor de TMR1 al Flanco de Subida
      ext_int_edge(0,H_TO_L); // Configuro para capturar siguiente flanco de Bajada
      flagToggleFlanco=1;     // Indico que el siguiente flanco será de Bajada

   } else {                   // He recibido Flanco de Bajada
      t2=get_timer1();        // Guardo en t2 el valor de TMR1 al Flanco de Bajada
      ext_int_edge(0,L_TO_H); // Configuro para capturar siguiente flanco de subida
      flagToggleFlanco=0;     // Indico que el siguiente flanco será de Subida
      set_timer1(0);          // Reinicio TMR1
      if(flagHayDatos==0){    // Si los datos anteriores han sido procesados ...
         flagHayDatos=1;      // Indico que ya hay nuevos datos de flancos para calcular
      }
   }
}

#int_RTCC
RTCC_isr(){

   ++contRTCC;
   if(contRTCC==3){
      set_TIMER0(AJUSTE_FINO_DE_RTCC);
   }
   if(contRTCC==4){
      flagRTCC=1;
      contRTCC=0x00;
   }
}

///////////////////////////
// Funciones de programa //
///////////////////////////

// Configuración inicial
void Configuracion(void){
   setup_counters(RTCC_INTERNAL,RTCC_DIV_128);
   output_low(SRF04_PORT_TRIGGER);               // A bajo el pin del disparador
   setup_timer_1(T1_INTERNAL | T1_DIV_BY_4);     // Timer1 con preescaler 1:4

   ext_int_edge(0,L_TO_H);     // Configuro captura de 1er flanco de subida
   flagToggleFlanco = 0;       // inicializo el Flag para cambiar de flanco
   enable_interrupts(int_ext); // Habilito las interrupciones necesarias
   enable_interrupts(int_TIMER1);
   enable_interrupts(INT_RTCC);
   enable_interrupts(global);                    // todas las interrupciones activadas

   printf("CORRE CORRE QUE TE PILLO\r\n\n");

   tSERVO1=ticks_PULSO_PARADO;      // Inicio con los servos parados
   tSERVO2=ticks_PULSO_PARADO;
}

// Control del PWM para mover los servos
void Control_PWM(void){
   // DISPARO DEL PULSO PWM
   if(flagRTCC==1){
      flagRTCC=0;
      output_high(PIN_SERVO1);
      output_high(PIN_SERVO2);
      flagSERVO1=1;
      flagSERVO2=1;
   }

   // CONTROL DE ANCHO DEL PULSO PWM
   if(flagSERVO1==1){
      valTIMER0 = get_TIMER0();
      if(valTIMER0>tSERVO1){
         flagSERVO1=0;
         output_low(PIN_SERVO1);
      }
   }
   if(flagSERVO2==1){
      valTIMER0 = get_TIMER0();
      if(valTIMER0>tSERVO2){
         flagSERVO2=0;
         output_low(PIN_SERVO2);
      }
   }
}

// Modifico el ancho de pulso de los servos
void Configura_Servos(void){
   if(media<85){
      tSERVO1=ticks_PULSO_ATRAS_RAPIDO_SERVO1;
      tSERVO2=ticks_PULSO_ATRAS_RAPIDO_SERVO2;
   }
   if(media>85 && media<95){
      tSERVO1=ticks_PULSO_ATRAS_LENTO_SERVO1;
      tSERVO2=ticks_PULSO_ATRAS_LENTO_SERVO2;
   }
   if(media>95 && media<105){
      tSERVO1=ticks_PULSO_PARADO;
      tSERVO2=ticks_PULSO_PARADO;
   }
   if(media>105 && media<115){
      tSERVO1=ticks_PULSO_ADELANTE_LENTO_SERVO1;
      tSERVO2=ticks_PULSO_ADELANTE_LENTO_SERVO2;
   }
   if(media>115){
      tSERVO1=ticks_PULSO_ADELANTE_RAPIDO_SERVO1;
      tSERVO2=ticks_PULSO_ADELANTE_RAPIDO_SERVO2;
   }
   ya=0;       // Indico que ya he utilizado la media
}

// Procesa los datos devueltos por la interrupción en RB0
void Procesa_Datos(void){
   if(t2 > t1){                       // Compruebo que estoy en la misma vuelta de TMR1
      tt = t2 - t1;                   // Calculo en Tick's de TMR1 el tiempo entre flancos
      if (tt >= MAX_ECHO_RESPONSE_Ticks){
         distance = DISTANCE_INFINITE;
      }
      else {
         distance = CALC_DISTANCE(tt);
      }
      flagHayDatos=0;                     // Indico que ya han sido procesados los datos.
      suma=suma+distance;                 // Sumo cada medida a las anteriores
      cont_medida++;                      // Cuento la nueva medida
      if(cont_medida==10){                // Cuando llevo x, calculo la media
         media=suma/cont_medida;          // Calculo la media
         suma=0.0;                        // Pongo a cero el valor de suma
         cont_medida=0;                   // Pongo a cero el contador de medidas
         ya=1;                            // Indico que ya he calculado una media
      }
   }
}
/////////////////////////////
// MAIN Programa Principal //
/////////////////////////////

void main(void) {

   Configuracion();

   do {
      Control_PWM();

      // Espera para mandar otro pulso
      if (orden==1){
         set_timer1(0);
         orden=2;
      }

      // Envio del pulso ultrasónico
      if(orden==3){
         set_TIMER1(65523);                  // Precargo el Timer para tener 10.4 uS
         output_high(SRF04_PORT_TRIGGER);
         orden=4;
      }

      // Proceso datos recibidos
      if(flagHayDatos==1){                  // Detecto que ya hay datos de flancos ...
         Procesa_Datos();
      }

      // Control de los SERVOS
      if(ya==1){
         printf("\r\nMedia -> %3.2f cm \r\n",media);  // Muestro la media en pantalla
         // Decido en función del valor de la media
         Configura_Servos();
      }
   } while (TRUE);
}

Alguien sabe cómo funciona este robot, el autor dice que sigue un objeto móvil, pareciera un carrito con llantas y dos motores que pudiera seguir. Pero en el código describe el uso de servomotores y los servo solo giran muy poco. O será que se colocan los servos con chasis de tal manera que funcione con pan tilt?.

Alguien puede opinar?. Me gustaría armarlo.

Saludos.