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 🙂

Sumo robot 2002 ve 2003 robot yarışmalarını birincilikle kazanmış daha önce 9v ile çalışan sistemini 12v ile çalıştırıp daha hızlı hale getirmişler ve arka uç saldırılarını önlemeye yardımcı olmak için arkaya bir açılır sahte beyaz çizgi eklenmiş.

Ayrıca, ön uç kaldırıldığında geriye doğru itilmesini önlemek için alt arka kısımda bir lastik fren balatası eklenmiş. Tekerlekler için Özel çelik jant, Lego lastiği, stok Lego tekerleği kullanılmış.

PIC16F877 robot kontrol, L293D motor sürücü ve IR sensör kartı delikli plakete kurularak Sumo robot şasesine yerleştirilmiş

PIC16F877 L293D Sumo Robot Devre Şeması

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: wa4dsy.net/robot/deltaforce/