followline.lst

蛇形机器人程序

C51 COMPILER V7.09   FOLLOWLINE                                                            06/11/2004 03:11:39 PAGE 1   


C51 COMPILER V7.09, COMPILATION OF MODULE FOLLOWLINE
OBJECT MODULE PLACED IN FollowLine.OBJ
COMPILER INVOKED BY: D:\Keil\C51\BIN\C51.EXE FollowLine.c OMF2 ROM(COMPACT) OPTIMIZE(SIZE) REGFILE(.\FollowLine.ORC) BRO
                    -WSE DEBUG

line level    source

   1          /*
   2                  ┍┑   ┍┑     ┍┑   ┍┑
   3                  ┕┙   ┕┙     ┕┙   ┕┙　
   4                          5         2.5     5
   5          */
   6          
   7          /*                      parameters needed to be adjusted
   8          
   9          */
  10          
  11          #include<REG2051.h>
  12          #include<FollowLine.h>
  13          #include<intrins.h>
  14          
  15          //~~~~~~~~~~~~~~~~~~~~~
  16          bit DirectionRight;
  17          bit DirectionLeft;
  18          
  19                                                                                                                                                            
  20          //~~~~~~~~~~~~~Serial In Sensor time~~~~~~~~
  21          bdata unsigned char LineCurStatus = 0xFF;
  22          // Logic 1 
  23          sbit CurStaMidSensor1 = LineCurStatus^0;        //Mid Sensor1 Current State        1 is On
  24          sbit CurStaMidSensor2 = LineCurStatus^1;        //Mid Sensor2 Current State        1 is On
  25          sbit CurStaHozSensor1 = LineCurStatus^2;        //Hoz Sensor Current State         1 is On 
  26          sbit CurStaHozSensor2 = LineCurStatus^3;        //Hoz Sensor Current State         1 is On
  27          
  28          //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  29          bdata unsigned char LinePreStatus = 0xFF;
  30          sbit PreStaMidSensor1 = LinePreStatus^0;        //Mid Sensor1 Previous State
  31          sbit PreStaMidSensor2 = LinePreStatus^1;        //Mid Sensor2 Previous State
  32          sbit PreStaHozSensor1 = LinePreStatus^2;        //Hoz Sensor Previous State
  33          sbit PreStaHozSensor2 = LinePreStatus^3;        //Hoz Sensor Previous State
  34          
  35          //movement status
  36          char PreLeftSpeed = 0;          //Speed Status
  37          char PreRightSpeed = 0;
  38          bit PreLeftDirection = 1 ;
  39          bit PreRightDirection = 1;      
  40          
  41          // further adjustment should replace
  42          unsigned char iLostLoopTime = 0;                        //current scaning time
  43          unsigned char LoopLostLine1 = 100;      //Maxmium scaning time flag left
  44          unsigned char LoopLostLine2 = 100;      //Maxmium scaning time flag     Right
  45          const unsigned char CatchTime1 = 50;            //Maxmiun scaning time for CatchLine
  46          const unsigned char CatchTime2 = 50;
  47          
  48          unsigned int iFollowLineTime = 0;                       //Parameter to control FollowLineTime
  49          
  50          //LineCounter   
  51          unsigned char LineCounterP = 0x00;
  52          unsigned char LineCounterN = 0x00;
  53          
  54          bit CounterFlag = 1;                                            // 0 counter enable
C51 COMPILER V7.09   FOLLOWLINE                                                            06/11/2004 03:11:39 PAGE 2   

  55          bit BackAdjFlag = 1;                                            // to forbid lineCounter when adjust moving backward
  56          
  57          //~~~~~~~~~~~~~~~~~Function Declaration~~~~~~~~~~~~~~~~~~~~
  58          extern void Delay(unsigned int time);
  59          void MotorLeft(char Speed,bit Direction);
  60          void MotorRight(char Speed,bit Direction);
  61          void FollowLine(char Speed);                    // follow normal while line
  62          void Stop(void);
  63          
  64          //void TurnLeft(char Speed);
  65          //
  66          void MotorLeft(char Speed,bit Direction)
  67          {
  68   1              if((Speed != PreLeftSpeed)||(PreLeftDirection != Direction))     // 
  69   1              {
  70   2                      DirectionLeft = Direction; //forward
  71   2                      PreLeftDirection = DirectionLeft;
  72   2                      LeftSpeed  = Speed;
  73   2                      PreLeftSpeed = Speed;   //save speed status
  74   2                      iLeftCounter = 0;
  75   2              }
  76   1              if (TR0 == 0)             //open T0 is necessary
  77   1              {TR0 = 1;}
  78   1              else
  79   1              {}
  80   1      
  81   1      }
  82          void MotorRight(char Speed,bit Direction)
  83          {
  84   1              if((Speed != PreRightSpeed)||(PreRightDirection != Direction))   // 
  85   1              {
  86   2                      DirectionRight = Direction; //forward
  87   2                      PreRightDirection = DirectionRight;
  88   2                      RightSpeed = Speed;
  89   2                      PreRightSpeed = Speed;  //save speed status
  90   2                      iRightCounter = 0; 
  91   2              }
  92   1              if (TR0 == 0)
  93   1              {TR0 = 1;}
  94   1              else
  95   1              {}                                              
  96   1      
  97   1      }
  98          
  99          void Stop(void)
 100          {
 101   1              TR0 = 0;
 102   1              DriveLeftMo1 = 0;
 103   1              DriveLeftMo2 = 0;
 104   1              DriveRightMo1 = 0;
 105   1              DriveRightMo2 = 0;
 106   1              _nop_();
 107   1              _nop_();
 108   1              PreLeftSpeed = 0;                                                 
 109   1              PreRightSpeed = 0;
 110   1      }
 111          
 112          //~~~~~~~~~~~~~~~~~~~Standard Follow Line~~~~~~~~~~~~~~~~~~~~~~~
 113          void FollowLine( char Speed)
 114          {
 115   1              unsigned char iLostLoopTime = 0;
 116   1              iStopStatus= 0;
C51 COMPILER V7.09   FOLLOWLINE                                                            06/11/2004 03:11:39 PAGE 3   

 117   1              if(iStopStatus >= 9000 )                        //time for move backward
 118   1              {
 119   2                      Stop();
 120   2                      Delay(1200);
 121   2                      MotorLeft((Speed-3),0);
 122   2                      _nop_ ();
 123   2                      MotorRight((Speed-3),0);
 124   2                      _nop_ ();
 125   2                      Delay(10000);
 126   2                      Stop();
 127   2                      Delay(1200);
 128   2                      iStopStatus = 0;
 129   2              }
 130   1                      if ((CurStaMidSensor1 == 0)&&(CurStaMidSensor2 == 0))
 131   1                              //Both mid sensors are on line 
 132   1                              //solve the uncertainty on CROSSING POINT
 133   1                      {
 134   2                              MotorLeft((Speed+3),1);
 135   2                              _nop_ ();
 136   2                              MotorRight((Speed+3),1);
 137   2                              _nop_ ();
 138   2      
 139   2                      //      Delay(Speed*1);
 140   2                      }
 141   1                      else if((CurStaMidSensor1 == 0)&&(CurStaMidSensor2 == 1))
 142   1                                      //left is on  ignore Hozsensor
 143   1                      {
 144   2                              MotorLeft((Speed-2),1); //left leaning more
 145   2                              _nop_ ();
 146   2                              MotorRight((Speed+1),1);         //original value is +2
 147   2                              _nop_ ();
 148   2                      //      Delay(Speed*1);
 149   2                      }
 150   1                      else if((CurStaMidSensor1 == 1)&&(CurStaMidSensor2 == 0))
 151   1                              //right is on  ignore Hozsensor