Mini Sumo Robot L293D PIC16F877

Mini sumo robotun tasarımı oldukca şık robot devresinde kullanılan mikrodenetleyici pic16f877 motor sürücü için l293d kullanılmş yazılım C dili ile hazırlanmış. Mini densede çok detaylı tüm kaynakları ile paylaşılan bir Sumo robot projesi ayrıca robot yarışmasında büyük rakiplerini pist dışına atıyor :)

control.c yazılım içeriği;

/* Delta Force mini-sumo robot Control program   Sept. 15 2001
   by Dale Heatherington.
   http://www.wa4dsy.net/robot/deltaforce/index.html

   This code is a modified version of the ROBOR
   mini sumo robot control program.  Some functions
   and variables may not be used.

 This program is compiled using the Linux shareware C2C compiler
 available at: http://www.geocities.com/SiliconValley/Network/3656/

 The output .asm code is assembled with the Linux freeware assembler
 gpasm avaiable at:  http://gpasm.sourceforge.net 

----------------------------------------------------------- 

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.                                       

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.                              

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA  

-----------------------------------------------------------------        

 */

/* Be sure to set the configuration word to 3f72 when programming the chip
   It's done in the makefile */

#define FOSC   16000000
#pragma  CLOCK_FREQ  16000000

#include "16f877.h"

//#define TMR0DIV   256-(FOSC/(8000*16)) //Compiler will not do this math!
//Divider constant for 8,000 interrupts per second

//For 8000 interrupts/sec
#define TMR0DIV 133

#define BAUD 9600
//#define BAUDDIV (FOSC/(BAUD*64)) - 1
#define BAUDDIV 26

/* these values written into ADCCON0
   set the A-D channel mux address
   and converison clock to 1/32 osc.
   This makes a 32uS converson time*/
#define  AD_0           0x81     /* ch 0 right front edge sensor*/
#define  AD_1           0x89     /* ch 1 left front edge sensor*/
#define  AD_RMFB        0x91     /* ch 2 Right motor feedback*/
#define  AD_LMFB        0x99     /* ch 3 Left motor feedback*/
#define  AD_4           0xa1     /* ch 4  Drive motor current 1v/amp*/
#define  AD_5           0xa9     /* ch 5  IR recv pulse amplitude    */
#define  AD_6           0xb1     /* ch 6  unused */
#define  AD_7           0xb9     /* ch 7  Battery voltage divided by 3    */    

char tx_buf[34]@0x24;     /* Tx serial port buffer */
char iavg[4]@0x20;

/* Analog inputs */
char  speed;

char  rmfb;
char  lmfb;

//char  mode;

char  right_speed,right_prm;
char  left_speed,left_prm;

char  bot_mode;

char  IR_sens;

#define MODE_STOP 0

#define MODE_RUN 1

#define MODE_GOHOME 7
#define MODE_NULL 0xff

#define REV0 0xf9
#define REV1 0xf6
#define REV2 0xf2
#define REV3 0xed
#define REV4 0xe7
#define REV5 0xd8
#define FWD0 7
#define FWD1 10
#define FWD2 14
#define FWD3 19
#define FWD4 25
#define FWD5 40

char  prescale;
char  timer;
char  prm_ctl;
/* bits in prm_ctl */
#define  R_fwd 0
#define  L_fwd 1

//Port C bits

#define R_EDGE_LED  0  /* Low true output turns on Right Front edge detect IR LED */
#define L_EDGE_LED 1  /* Low true output turns on Left Front edge detect IR LED */

//PORT D bits
#define R_LED 0      /* Low true output turns on Right Red indicator LED */
#define M_LED 1      /* Low true output turns on Middle Green indicator LED */
#define L_LED 2      /* Low true output turns on Left Red indicator LED */

/* Keep the duty cycle of the following IR LEDs to 5% or less or they may burn out */
#define R_REFL_LED   6  /* HIGH true output turns on Right object detector IR LED */
#define L_REFL_LED   7  /* HIGH true output turns on Left object detector IR LED */

char  workB;

/* Port B motor control bits */

#define  LeftEnable 5    /* Left Motor pwm (L293D enable enable)*/
#define  RightEnable 4   /* Right Motor pwm (L293D enable enable)*/
#define  FwdLeft  3     /*Left Motor forward */
#define  RevLeft  2     /*Left Motor reverse*/
#define  FwdRight 1     /*Right Motor forward */
#define  RevRight 0     /*Right Motor reverse */

#define  P_width  22    /*Motor pulse width in interrupt ticks*/
char tx_ptr,rx_ptr,txstate,rxstate;
char  crcH,crcL;

char r_speed;
char l_speed;
char l_integrate;
char r_integrate;
char dir_ctl;

char testmode;
char speed_to_pulse_rate(char speed);

char timer2,startup_timer,motor_timer,tilt_dir, escape_timer;
char tilt_memory, border_memory, pushback_memory, cruse_memory; 

char  counter1;
char  t8k_timer;     /* 8000 ticks/sec down counter */
char   runTimer_H;  /* measures run time in seconds */
char   runTimer_L;
char spi_stat;
char  battery;
char  drive_current, drive_current_avg;

char flags, arb_code;
char debug;
char mon_dir_ctrl;
char  lf_sens, rf_sens;

char cruse_output, chase_output , current_limit_output , border_output;
char check_battery_output, continue_output, arbitrate_output;

char l_IR, r_IR, amb_light; 

char lf_avg,rf_avg,lr_avg,rr_avg, f_ratio, r_ratio;

char divide10;
char divide100;
char lr_ratio;

//Bits in flags variable
#define tick100 0
#define direction 1
#define r_odo_last 2
#define stalled 3
#define spin_left 4
#define spin_right 5
#define attack 6

char rnd;            /*random number*/
char ctr;           /* a counter */
char temp;

//----------------------------------------------------------

#define COUNT 1      /* offset to count field in tx/rx buffers */
#define ADDRESS 0    /* offset to address field in tx/rx buffers */
#define DATA 2       /* offset to start of data in tx/rx buffers */

char test_hi@0xfe;

char detect_line();
char neg(char x);
char integrate(char speed,char mfb,char integ);

//------------------------------------------------------------------

/* Interrupts from Timer 0 occur at about 8,000 per second. */
void interrupt(void)
{
   char R_pulse,L_pulse,motor_avg;
   char i,l,r,spidata;

   set_bit(PORTD,4);

   if (INTCON & 4) {   //Check for a TMR0 interrupt

      clear_bit(INTCON,2);      //Clear timer 0 interrupt flag
      TMR0 = TMR0DIV + TMR0;       //restart timer 0

      if(t8k_timer) t8k_timer--; //8000 ticks per second timer decrement

      if(prescale-- == 0){

         prescale = 79;
         if(timer != 0) timer--;    //10 ms pertick timer
         if(timer2 != 0) timer2--;
         if(motor_timer != 0) motor_timer--;

         if(divide10++ >= 9){

             if(startup_timer != 0)    startup_timer--; //100ms per tick timers
             if(escape_timer != 0)     escape_timer--;
             if(cruse_memory != 0)     cruse_memory--;
             if(pushback_memory != 0)  pushback_memory--;
             if(border_memory != 0 )   border_memory--;
             if(tilt_memory != 0 )     tilt_memory--;

             divide10 = 0;
             set_bit(flags,tick100);

         }

         if(divide100++ >= 99){  // 1 second per tick timer
            runTimer_L++;
            if(runTimer_L == 0) runTimer_H++;
            divide100 = 0;

         }

      }

      PORTB = workB;       //output the prm motor control bits to port B

      workB = 0;

         /* adjust right pulse rate modulator */
         if (right_prm < P_width) {
            set_bit(workB,RightEnable);
            if (prm_ctl & (1 << R_fwd))
               set_bit(workB,FwdRight);
            else
               set_bit(workB,RevRight);
         }
         if (right_prm-- == 0) {
            right_prm = speed_to_pulse_rate(right_speed) + P_width;
            if (right_speed > 127) clear_bit(prm_ctl,R_fwd);
            else set_bit(prm_ctl,R_fwd);
         }

         /*adjust left pulse rate modulator */
         if (left_prm < P_width) {
            set_bit(workB,LeftEnable);
            if (prm_ctl & (1 << L_fwd))
               set_bit(workB,FwdLeft);
            else
               set_bit(workB,RevLeft);
         }

         if (left_prm-- == 0) {
            left_prm = speed_to_pulse_rate(left_speed) + P_width;
            if (left_speed > 127) clear_bit(prm_ctl,L_fwd);
            else set_bit(prm_ctl,L_fwd);
         }

      }

    if (bot_mode == MODE_STOP) workB = 0;    //Stop the bot.

    if ((l_speed == 0) && (r_speed == 0)) workB = 0;
    if(testmode == 1) workB = 0;

      //Make a new random number
      if (rnd & 0x80) { //rnd msb is 1
            asm{
               movlw 1ch ;
               xorwf _rnd,F;
               bsf   STATUS,C;
               rlf   _rnd; //8 bit left shift

            }
         } else {    //rnd msb  is 0
            asm{
               bcf   STATUS,C;
               rlf   _rnd; //8 bit left shift

            }
         }

    clear_bit(PORTD,4);        

   }
//--------------------------------------------------------------
short mpy(char a, char b)
{
   return a * b;
}
//---------------------------------------------------------------
char divide(char a, char b)
{
   return a / b;
}

   //-----------------------------------------------------------------------

   // compute 16 bit CRC on 1 data byte.
   //   crcH and crcL  are global variables which accumulate the 16 bit CRC.
   //   They must be cleared before starting to compute 16 bit CRC on a string.
   //   The final output value will be in crcH and crcL.

   void crc(char data){

      char i;
     // short crc16;

      for (i=0;i<8;i++) {                //Do all 8 bits in the data byte
         if (data & 0x80) crcH ^= 0x80; //if data bit is 1 then flip crc msb
         data = data << 1;             // get next data bit
         if (crcH & 0x80) { //crc msb is 1
            asm{
               bcf   STATUS,C;
               rlf   _crcL; //16 bit left shift
               rlf   _crcH;
               movlw 80h;
               xorwf _crcH,F; //xor with 0x8005
               movlw 5;
               xorwf _crcL,F;

            }
         } else {    //crc msb  is 0
            asm{
               bcf   STATUS,C;
               rlf   _crcL; //16 bit left shift
               rlf   _crcH;
            }
         }
      }

   }
   //-----------------------------------------------------------------------
   void set_tx_buffer_crc(){
      int j = tx_buf[COUNT + 0] + 2; //get data size
      int i;

      crcH = 0;         //Clear crc16
      crcL = 0;
      for (i=0;i<j;i++) crc(tx_buf[i]); //Compute crc16 over buffer
      tx_buf[i++] = crcH;      //tack the crc bytes on the end of the buffer
      tx_buf[i++] = crcL;
      txstate = 0;            //State 0 = stopped
   }

   //-----------------------------------------------------------------------

   void make_info_pkt(){
      char i,j;
      i = ADDRESS;
      tx_buf[i] = 1;             // send to master
      i = COUNT;
      tx_buf[i] = 16;            // n bytes
      i = DATA;
      tx_buf[i++] = 0x01;        //data type = 1
      tx_buf[i++] = l_speed;        //1
      tx_buf[i++] = r_speed;        //2
      tx_buf[i++] = flags;          //3
      tx_buf[i++] = runTimer_H;     //4
      tx_buf[i++] = runTimer_L;     //5

      tx_buf[i++] = l_IR;           //6
      tx_buf[i++] = r_IR;           //7
      tx_buf[i++] = amb_light;      //8
      tx_buf[i++] = bot_mode;       //9

      tx_buf[i++] = battery;        //10 Battery voltage/3
      tx_buf[i++] = mon_dir_ctrl;   //11
      tx_buf[i++] = timer;          //12
      tx_buf[i++] = lf_sens ;       //13  Left front line sensor
      tx_buf[i++] = rf_sens ;       //14  Right front line sensor
      tx_buf[i++] = drive_current; //15
      tx_buf[i++] = arb_code;      //16

      crcH = 0;
      crcL = 0;
      set_tx_buffer_crc();  //compute and add CRC16 bits to end of pkt

      txstate = 1;          //Tell scheduler it's ready to transmit
   }

   //----------------------------------------------------------------------

  //send character if tx ready and return true
  //  else return false if tx not ready
   char tx_byte(char c){
      if (PIR1 & (1 << TXIF)) {
         TXREG = c;
         return 1;
      } else
         return 0;
   }

  //-----------------------------------------------------------------------

   void send_tx_buffer(){
      static char j;

      switch (txstate) {
      case 1:
      case 2:
      case 3:  if (tx_byte(0xff)) txstate++;
         break;
      case 4:  if (tx_byte(0xf0)) txstate++;
         break;
      case 5:  if (tx_byte(0x55)) txstate++;
         break;
      case 6:  tx_ptr = 0;
         j = tx_buf[COUNT + 0] + 4; //Must use COUNT+0 to force indirect addressing
         txstate++;
         break;

      case 7:  if (tx_byte(tx_buf[tx_ptr])) {
            j--;
            tx_ptr++ ;
            if (j == 0) txstate = 8;
         }
         break;

      case 8:  if (tx_byte(0x33)) {
            txstate = 0;
         }

      }

   }

   //-----------------------------------------------------------------------
   /* Convert a signed value to an absolute value between 127 and 0
        0 becomes 127
      127 becomes 0
     -128 becomes 0

   */
   char speed_to_pulse_rate(char speed)
   {
      if (speed > 127) {      //See if speed is negative !
         return speed & 0x7f;
      } else {
         return speed ^ 0x7f;
      }

   }

   //----------------------------------------------------------------------
   char ad_convert(char channel)
   {  char x;

      ADCON0 = channel;
      for (x=16;x>0;x--);         //wait at least 20 us for data to stabilize

      set_bit(ADCON0,GO_DONE);   //start A-D conversion

      clear_bit(PORTD,R_REFL_LED); //turn off both IR object detect LEDs
      clear_bit(PORTD,L_REFL_LED);

      while (ADCON0 & (1 << NOT_DONE));  //wait for conversion to complete

      return ADRESH;          //Return unsigned result
   }

   //---------------------------------------------------------------------

   void get_AD_values(){

    drive_current = ad_convert(AD_4);  //Read unsigned drive motor current 1v/amp
    battery = ad_convert(AD_7);        //Read battery voltage/3
    clear_bit(PORTC,R_EDGE_LED) ;      //White line sensor Right IR emitter ON
    rf_sens = ad_convert(AD_0);        //Read right front sensor
    set_bit(PORTC,R_EDGE_LED);         //White line sensor Right IR emitter OFF
    clear_bit(PORTC,L_EDGE_LED);       //White line sensor Left IR emitter ON
    lf_sens = ad_convert(AD_1);        //Read left front sensor
    set_bit(PORTC,L_EDGE_LED);         //White line sensor Left IR emitter OFF

   }

   //----------------------------------------------------------------------

   void adjust_motor_speed(){
      char i;

      rmfb = ad_convert(AD_RMFB)+128; //get right motor feedback as signed value
      lmfb = ad_convert(AD_LMFB)+128; //get left motor feedback  as signed value

      for (i=0;i<10;i++) {  //Adjust motor speeds based on feedback values

        r_integrate = integrate(r_speed,rmfb,r_integrate);
        l_integrate  = integrate(l_speed,lmfb,l_integrate);
        right_speed = r_integrate;
        left_speed  = l_integrate;

      }
   }
  //----------------------------------------------------------------------

   //dir_control constants.

   #define FWD 1
   #define FWD_LEFT 2
   #define FWD_RIGHT 3
   #define FWD_HARD_LEFT 4
   #define FWD_HARD_RIGHT 5
   #define FWD_MAX 6

   #define SPIN_LEFT 10
   #define SPIN_RIGHT 11
   #define STOP 12

   #define REV 16
   #define REV_LEFT 17
   #define REV_RIGHT 18
   #define REV_HARD_LEFT 19
   #define REV_HARD_RIGHT 20
   #define REV_MAX 21

   void dir_control(char value)
   {
      mon_dir_ctrl = value;
      if(value <= FWD_MAX)  set_bit(flags,direction) ; //Set forward flag
      if(value >= REV) clear_bit(flags,direction);     //or set the reverse flag

       switch(value){

       case FWD:     l_speed = FWD4;
                     r_speed = FWD4;
                     break;

       case FWD_MAX: l_speed = FWD5;
                     r_speed = FWD5;
                     break;

       case FWD_LEFT:
                     l_speed = FWD2;
                     r_speed = FWD5;
                     break;

       case FWD_RIGHT:
                     l_speed = FWD5;
                     r_speed = FWD2;
                     break;

       case FWD_HARD_LEFT:
                     l_speed = 0;
                     r_speed = FWD5;
                     break;

       case FWD_HARD_RIGHT:
                     l_speed = FWD5;
                     r_speed = 0;
                     break;

       case REV:     l_speed = REV4;
                     r_speed = REV4;
                     break;

       case REV_MAX: l_speed = REV5;
                     r_speed = REV5;
                     break;

       case REV_LEFT:
                     l_speed = FWD1;
                     r_speed = REV5;
                     break;

       case REV_RIGHT:
                     l_speed = REV5;
                     r_speed = FWD1;
                     break;

       case REV_HARD_LEFT:
                     l_speed = 0;
                     r_speed = REV5;
                     break;

       case REV_HARD_RIGHT:
                     l_speed = REV5;
                     r_speed = 0;
                     break;

       case SPIN_LEFT:
                     l_speed = REV4;
                     r_speed = FWD4;
                     break;

       case SPIN_RIGHT:
                     l_speed = FWD4;
                     r_speed = REV4;
                     break;

       case STOP:

                     l_speed = 0;
                     r_speed = 0;
                     break;

       }
   }

  //---------------------------------------------------------------------------------

   /* Saturating addition.  Result is clipped at +127 or -128
      instead of overflowing.
      Note the parameters a and b and signed char values (-128..+127)
      but the compiler thinks they are unsigned values (0..255) so
      I had to do some strange looking things below.
      */

   char addsat(char a, char b)
   {  char r;
      if (a > 127) goto a_minus;  //See if a is minus
      if (b > 127) goto a_b_diff; //a is plus, check b for minusness

      r = a + b;                //Both a and b are positive numbers
      if (r > 127) r = 127; //clip at +limit
      return r;

      a_minus:
      if (b < 0x80) goto a_b_diff;  //A is minus, now check b and see if it's minus too
      r = a + b;
      if (r < 0x80) r = 0x80;  //clip at -128
      return r; 

      a_b_diff:
      return a + b;     //No clipping needed

   }

     //---------------------------------------------------------------------
   /* return the absolute value of x */

   char abs(char x){
      if(x > 127){
         x ^= 0xff;
         x++;
      }
      return x;
   }

   //-----------------------------------------------------------------------
   char abs_diff(char a, char b)
   {

      if(a == 0) return b;
      if(b == 0) return a;
      if(a > b) return (a - b);
         else return ( b - a) ;

   }

   //-----------------------------------------------------------------------
   /* return the negative of x */

   char neg(char x){
      x ^= 0xff;
      x++;
      return x;
   }

   //----------------------------------------------------------------------
   /* return x if its 0 or less else return 0 */

   char clip_pos(char x){
      if(x < 128) x = 0;
      return x;
   }

   //----------------------------------------------------------------------
   /* return x if it's 0 or higher else return 0 */

   char clip_neg(char x){
      if(x > 127) x = 0;
      return x;
   }

   //---------------------------------------------------------------------
   //return the a 16 bit value
   char sign16(int x){
      char rv;
      rv = 0;
      if(x & 0x8000) rv = 1;
      return rv;
   }
   //---------------------------------------------------------------------

   //Limit x to +/- l
   char limit(char x, char l){

      if(x & 0x80){
         if( x < neg(l)) return neg(l);
         else return x;
      }
      if( x > l) return l;
      else return x;

   }
   //-----------------------------------------------------------

   char divideby2(char x){
      if(x < 128) return (x >> 1);
      else{
         return (x >> 1) | 0x80;
      }
   }

   //-------------------------------------------------------------------
   char ratio(char a, char b)
   {  short d;
      char c;
      if((a == 0) && (b == 0)) return 0;
      c = abs_diff(a,b);
      d = c * 256;
      d = d -1;
      if(a > b) c = a ; else c = b;
      if(c == 0) return 255;
       else return (d / c);
   }

   //----------------------------------------------------------------------
   char integrate(char speed,char mfb,char integ)
   {
      speed = speed + 128;   //convert to unsigned
      mfb = mfb + 128;
      if (speed > mfb) {
         integ++;
         if (integ == 0x80) integ--;  //Clip at +127
         return integ;
      } else {
         integ--;
         if (integ == 0x7f) integ++;  //Clip at -128
         return integ;
      }

   }
   //-----------------------------------------------------------------------

   #define IR_MIN 180  

   /*Reverse directon when we see the white border line */
   /* Note: IR readings sit at 250 to 255 on black and
      drop to about 110 on white. */

   void check_border()
   {  static char action;
      static char lock;
      char time;

         if(lock != 1){
            action = 0;

            if(lf_sens < IR_MIN){
               action = REV_RIGHT;
               time = 10 + (rnd & 7);
            }

            if(rf_sens < IR_MIN){
               action = REV_LEFT;
               time = 10 + (rnd & 7);
            }

            if((lf_sens <  IR_MIN) && (rf_sens < IR_MIN)){
               if(runTimer_L & 1){
                  action = REV_LEFT;     //random left or right turnaround
               }else{
                  action = REV_RIGHT;
               }
               time = 12;
            }

            if(action){
               border_memory = time;      //timeout
               border_output = REV;       //Initially backup
               lock = 1;
            }
         }

     if(border_memory){
        if((lf_sens <  IR_MIN) || (rf_sens < IR_MIN))
           border_output = REV;            //Backup until sensors see black again
        else
           border_output = action;         //Then execute the turn-around

     } else{
        border_output = 0;
        time = 0;
        lock = 0;
     }

   }
 //------------------------------------------------------------------------

   void cruse()
   {
      cruse_output = FWD;
     // if(cruse_memory == 0) cruse_memory = 12;
     // if(cruse_memory > 0) cruse_output = SPIN_RIGHT ;
    //  if(cruse_memory > 6) cruse_output = FWD;

   }

 //-----------------------------------------------------------------------

 #define lmin 8

 void chase()
   {  

      chase_output = 0;
      set_bit(PORTD,M_LED);   //Attack indictor LED off
      clear_bit(flags,attack);
      set_bit(PORTD,R_LED);
      set_bit(PORTD,L_LED);

      if((l_IR > lmin) || (r_IR > lmin)){
         if(ratio(l_IR,r_IR) > 150){
            if(l_IR > r_IR){
               chase_output = FWD_LEFT;
               clear_bit(PORTD,L_LED) ;
            }
               else{
                  chase_output = FWD_RIGHT;
                  clear_bit(PORTD,R_LED);
               }
         }else{
            chase_output = FWD_MAX;
            set_bit(flags,attack);
            clear_bit(PORTD,M_LED);    //Turn on green attack LED
         }
      }

   }
   //-----------------------------------------------------------------------

   //Stop motors while current exceeds I .
   void limit_motor_current(char I){

       if(drive_current > I){
          current_limit_output = STOP;
       }else{
          current_limit_output = 0;
       }

   }

   //-------------------------------------------------------------------------
   //Stop motors while battery reads less than 5.5 volts (or whatever "low_limit" is)
   void check_battery(char low_limit){
      if((battery < low_limit) && (runTimer_L > 6)){ //After 6 secs and bat less than 5.5 volts.
         check_battery_output = STOP;
      } else{
         check_battery_output = 0;
      }

   }

   //----------------------------------------------------------------------
   //IR object detector processing.  Normal ambiant light reading is about 50.
   void process_IR()
   {  

      amb_light = ad_convert(AD_5);             //Read ambiant light value
      if(amb_light > 100) return;               //Don't process if blinded by external IR pulse

      set_bit(PORTD,R_REFL_LED);                //Right object detect IR LED on
      r_IR = ad_convert(AD_5)  ;               //Read value , IR LED is turned off by ad_convert()
      if(r_IR > amb_light)
         r_IR = r_IR - amb_light;
      else r_IR = 0;

      set_bit(PORTD,L_REFL_LED);                //Left object detect IR LED on
      l_IR = ad_convert(AD_5) ;                 //Read value , IR LED is turned off by ad_convert()
      if(l_IR > amb_light)
         l_IR = l_IR - amb_light;
      else l_IR = 0;

      clear_bit(PORTD,R_REFL_LED); //Make sure both LEDs are off
      clear_bit(PORTD,L_REFL_LED);

  }

   //------------------------------------------------------------------------

  // Subsumption arbitration happens here.
   void arbitrate(){

     arb_code = 0;
     if(cruse_output) { arb_code = 1; dir_ctl = cruse_output; }                 //Lowest priority
     if(chase_output) { arb_code = 2; dir_ctl = chase_output; }

     if(border_output) {
        arb_code = 5;
        dir_ctl = border_output;
     }

     if(current_limit_output) { arb_code = 6; dir_ctl = current_limit_output;}
     if(check_battery_output) { arb_code = 7; dir_ctl = check_battery_output; } //Highest priority

     dir_control(dir_ctl);   //Now tell the motors what to do.

   }

   //-------------------------------------------------------------------------

   main()
   {
      char i;
      char x;
      char y;
      char z;

      test_hi = 0;

      INTCON = 0;
      STATUS = 0;
      PORTA  = 0;
      PORTB  = 0;
      PORTC  = 3;
      PORTD  = 0x3f ;
      T1CON  = 0;
      TMR0   = 0;
      workB  = 0x20;

      tx_ptr = 0;
      rx_ptr = 0;

      RCSTA = 0x90;             //SPEN + CREN (configure serial port)

      set_bit(STATUS,RP0);     //Bank1

      set_tris_b(0x00);        //Port B bits are outputs
      set_tris_c(0xc0);        //Port C outputs except 6:7 are UART pins
      TRISA = 0xff;            //port A all analog inputs
      TRISD = 0;              //port D outputs
      TRISE = 0x7;           //port E analog inputs

      OPTION_REG = 0x01;       //Prescaler 1:4 assigned to Timer 0, pullups on portB
      ADCON1 = 0;              //Set all port A to analog with VREF=VCC
      TXSTA = 0xa2;            //Set tx usart status/ctrl reg= CSRC + TXEN + TRMT
      SPBRG = BAUDDIV;         //Set baudrate
      //SSPSTAT = 0x00;          //SPI PORT   slave mode CKE = 0  SMP = 0
      PIE1 = 0;                //Disable all peripheral interrupts
      //set_bit(PIE1,SSPIE);     //Enable SPI port interrupts
      clear_bit(STATUS,RP0);   //Bank0

      //SSPCON = 0;
      //SSPCON = 0x15;            // SPI slave mode -SS disabled  clock = SCK pin  CKP=1
      //set_bit(SSPCON,SSPEN);    //Enable SPI port

      clear_bit(INTCON,T0IF);  //Clear Timer 0 interrupt flag
      TMR0   = TMR0DIV;        //Start Timer 0
      set_bit(INTCON,T0IE);    //Enable Timer 0 interrupts
      set_bit(INTCON,PEIE);   //Enable peripheral interrupts
      set_bit(INTCON,GIE);     //Enable global interrupts
      iavg[0] = 0;
      iavg[1] = 0;
      iavg[2] = 0;
      iavg[3] = 0;
      drive_current_avg = 0;
      txstate = 0;

      clear_bit(PORTD,M_LED);     //green indicator LED on

      testmode = 0;
      startup_timer = 50;
      tilt_memory = 0;
      border_memory = 0;
      pushback_memory = 0;
      cruse_memory = 0;
      timer2 = 0;
      flags = 0;
      divide10 = 0;
      divide100 = 0;
      counter1 = 10;
      runTimer_H = 0;
      runTimer_L = 0;
      left_speed = 0;
      right_speed = 0;
      r_integrate = 0;
      l_integrate = 0;
      r_speed = 0;
      l_speed = 0;
      speed = 0;
      lf_sens = 256;
      rf_sens = 256;

      bot_mode = MODE_STOP;
      dir_ctl = STOP;
      mon_dir_ctrl = dir_ctl;
      arb_code = 0;

      /* Wait here for 5 seconds after power up */
      startup_timer = 50;             //5 sec
      while(startup_timer > 0){               //Wait 5 sec before starting
         clear_bit(PORTD,3);
         if(startup_timer & 4){
             clear_bit(PORTD,M_LED);

         }
            else{
                set_bit(PORTD,M_LED); // blink the green LED during the 5 sec wait.

            }

         get_AD_values();
         if(t8k_timer == 0){
           t8k_timer = 16;      //2ms timeout value
           process_IR();      //Get info from IR object detector
        }

         chase();    //Refresh Indicator LEDs based on IR sensors

         if (txstate == 0) make_info_pkt(); //Get new data for transmission
         send_tx_buffer();  //Send data on RF link if TX ready
         set_bit(PORTD,3);

      }

      clear_bit(PORTD,3); //test
      set_bit(PORTD,M_LED);  //Indicator LED off
      bot_mode = MODE_RUN; //Allow motors to run
      t8k_timer = 16;  //timeout in 2ms

      /* Start of the main program loop */

      for(;;){

       if (txstate == 0) make_info_pkt(); //Get new data for transmission
       send_tx_buffer();  //Send data on RF link if TX ready

       if(timer2 == 0){
           get_AD_values(); //Every 10ms read the A to D converters
           timer2 = 1;
       }

       /* Now check all the sensors */

       check_border();
       cruse();
       chase();

       clear_bit(PORTD,3); //test

       limit_motor_current(30);  // 600 MA Limit
       check_battery(93);        //5.5 volt lower limit

       set_bit(PORTD,3); //test

       // Sort it all out and decide what to tell the motor controller.
       arbitrate();

       adjust_motor_speed();

        if(t8k_timer == 0){
           t8k_timer = 16;      //2ms timeout value
           process_IR();      //Get info from IR object detector
        }

        if(flags & (1 << tick100)){    //Every 100 ms do this...
             iavg[3] = iavg[2];        //Average the motor current over 400ms
             iavg[2] = iavg[1];
             iavg[1] = iavg[0];
             iavg[0] = drive_current;
             drive_current_avg = 0;
             for(i=0;i<4;i++) drive_current_avg = drive_current_avg + iavg[i];
             drive_current_avg = drive_current_avg >> 2;  //div by 4
             clear_bit(flags,tick100);
        } 

      }   //end main loop

} //End main()
/* End of source */

Kaynak: http://www.wa4dsy.net/robot/deltaforce/ Alternatif linlk: mini-sumo-robot-l293d-pic16f877