position.c

本人s12的一部分机器人巡线程序

    28,29,29,30,30,31,31,32,32,33,33,34,34,35,35,36,35,35,34,34,33,33,32,32,31,31,30,30,29,29,28,//.4
    28,28,29,29,30,30,31,31,32,32,33,33,34,34,35,35,35,34,34,33,33,32,32,31,31,30,30,29,29,28,28,//.2
    27,28,28,29,29,30,30,31,31,32,32,33,33,34,34,35,34,34,33,33,32,32,31,31,30,30,29,29,28,28,27,//5
    27,27,28,28,29,29,30,30,31,31,32,32,33,33,34,34,34,33,33,32,32,31,31,30,30,29,29,28,28,27,27,//.8
    26,27,27,28,28,29,29,30,30,31,31,32,32,33,33,34,33,33,32,32,31,31,30,30,29,29,28,28,27,27,26,//.6
    26,26,27,27,28,28,29,29,30,30,31,31,32,32,33,33,33,32,32,31,31,30,30,29,29,28,28,27,27,26,26,//.4
    25,26,26,27,27,28,28,29,29,30,30,31,31,32,32,33,32,32,31,31,30,30,29,29,28,28,27,27,26,26,25,//.2
    25,25,26,26,27,27,28,28,29,29,30,30,31,31,32,32,32,31,31,30,30,29,29,28,28,27,27,26,26,25,25,//4
    24,25,25,26,26,27,27,28,28,29,29,30,30,31,31,32,31,31,30,30,29,29,28,28,27,27,26,26,25,25,24,//.8
    24,24,25,25,26,26,27,27,28,28,29,29,30,30,31,31,31,30,30,29,29,28,28,27,27,26,26,25,25,24,24,//.6
    23,24,24,25,25,26,26,27,27,28,28,29,29,30,30,31,30,30,29,29,28,28,27,27,26,26,25,25,24,24,23,//.4
    23,23,24,24,25,25,26,26,27,27,28,28,29,29,30,30,30,29,29,28,28,27,27,26,26,25,25,24,24,23,23,//.2
    22,23,23,24,24,25,25,26,26,27,27,28,28,29,29,30,29,29,28,28,27,27,26,26,25,25,24,24,23,23,22,//3
    22,22,23,23,24,24,25,25,26,26,27,27,28,28,29,29,29,28,28,27,27,26,26,25,25,24,24,23,23,22,22,//.8
    21,22,22,23,23,24,24,25,25,26,26,27,27,28,28,29,28,28,27,27,26,26,25,25,24,24,23,23,22,22,21,//.6
    21,21,22,22,23,23,24,24,25,25,26,26,27,27,28,28,28,27,27,26,26,25,25,24,24,23,23,22,22,21,21,//.4
    20,21,21,22,22,23,23,24,24,25,25,26,26,27,27,28,27,27,26,26,25,25,24,24,23,23,22,22,21,21,20,//.2
    20,20,21,21,22,22,23,23,24,24,25,25,26,26,27,27,27,26,26,25,25,24,24,23,23,22,22,21,21,20,20,//2
    19,20,20,21,21,22,22,23,23,24,24,25,25,26,26,27,26,26,25,25,24,24,23,23,22,22,21,21,20,20,19,//.8
    19,19,20,20,21,21,22,22,23,23,24,24,25,25,26,26,26,25,25,24,24,23,23,22,22,21,21,20,20,19,19,//.6
    18,19,19,20,20,21,21,22,22,23,23,24,24,25,25,26,25,25,24,24,23,23,22,22,21,21,20,20,19,19,18,//.4
    18,18,19,19,20,20,21,21,22,22,23,23,24,24,25,25,25,24,24,23,23,22,22,21,21,20,20,19,19,18,18,//.2
    17,18,18,19,19,20,20,21,21,22,22,23,23,24,24,25,24,24,23,23,22,22,21,21,20,20,19,19,18,18,17,//1
    17,17,18,18,19,19,20,20,21,21,22,22,23,23,24,24,24,23,23,22,22,21,21,20,20,19,19,18,18,17,17,//.8
    16,17,17,18,18,19,19,20,20,21,21,22,22,23,23,24,23,23,22,22,21,21,20,20,19,19,18,18,17,17,16,//.6
    16,16,17,17,18,18,19,19,20,20,21,21,22,22,23,23,23,22,22,21,21,20,20,19,19,18,18,17,17,16,16,//.4
    15,16,16,17,17,18,18,19,19,20,20,21,21,22,22,23,22,22,21,21,20,20,19,19,18,18,17,17,16,16,15,//.2
    15,15,16,16,17,17,18,18,19,19,20,20,21,21,22,22,22,21,21,20,20,19,19,18,18,17,17,16,16,15,15//0

};
*/

#define FA 21
#define FB 22
#define FC 23
#define FD 24
#define FE 25
#define FF 26
#define FG 27
#define FH 28
#define FI 29
#define FJ 30   //表格的中心值
#define FK 28
#define FL 29
#define FM 30
#define FN 31
//功能和上个表完全一样,只是这个用宏定义代替数字,本人认为不够直观
//横排表示的是上次的位置值,竖排表示的是当前位置值
const  uchar rightforward[19][19]=
{
// 0   4  7  1  4  7  2  4  7  3  4  7  4  4  7  5  4  7  6
   FJ,FJ,FK,FK,FL,FL,FM,FM,FN,FN,FN,FM,FM,FL,FL,FK,FK,FJ,FJ,        
   FI,FJ,FJ,FK,FK,FL,FL,FM,FM,FN,FM,FM,FL,FL,FK,FK,FJ,FJ,FI, 
   FI,FI,FJ,FJ,FK,FK,FL,FL,FM,FM,FM,FL,FL,FK,FK,FJ,FJ,FI,FI, 
   FH,FI,FI,FJ,FJ,FK,FK,FL,FL,FM,FL,FL,FK,FK,FJ,FJ,FI,FI,FH, 
   FH,FH,FI,FI,FJ,FJ,FK,FK,FL,FL,FL,FK,FK,FJ,FJ,FI,FI,FH,FH, 
   FG,FH,FH,FI,FI,FJ,FJ,FK,FK,FL,FK,FK,FJ,FJ,FI,FI,FH,FH,FG, 
   FG,FG,FH,FH,FI,FI,FJ,FJ,FK,FK,FK,FJ,FJ,FI,FI,FH,FH,FG,FG,    
   FF,FG,FG,FH,FH,FI,FI,FJ,FJ,FK,FJ,FJ,FI,FI,FH,FH,FG,FG,FF, 
   FF,FF,FG,FG,FH,FH,FI,FI,FJ,FJ,FJ,FI,FI,FH,FH,FG,FG,FF,FF, 
   FE,FF,FF,FG,FG,FH,FH,FI,FI,FJ,FI,FI,FH,FH,FG,FG,FF,FF,FE, 
   FE,FE,FF,FF,FG,FG,FH,FH,FI,FI,FI,FH,FH,FG,FG,FF,FF,FE,FE, 
   FD,FE,FE,FF,FF,FG,FG,FH,FH,FI,FH,FH,FG,FG,FF,FF,FE,FE,FD, 
   FD,FD,FE,FE,FF,FF,FG,FG,FH,FH,FH,FG,FG,FF,FF,FE,FE,FD,FD, 
   FC,FD,FD,FE,FE,FF,FF,FG,FG,FH,FG,FG,FF,FF,FE,FE,FD,FD,FC, 
   FC,FC,FD,FD,FE,FE,FF,FF,FG,FG,FG,FF,FF,FE,FE,FD,FD,FC,FC, 
   FB,FC,FC,FD,FD,FE,FE,FF,FF,FG,FF,FF,FE,FE,FD,FD,FC,FC,FB, 
   FB,FB,FC,FC,FD,FD,FE,FE,FF,FF,FF,FE,FE,FD,FD,FC,FC,FB,FB, 
   FA,FB,FB,FC,FC,FD,FD,FE,FE,FF,FE,FE,FD,FD,FC,FC,FB,FB,FA, 
   FA,FA,FB,FB,FC,FC,FD,FD,FE,FE,FE,FD,FD,FC,FC,FB,FB,FA,FA  
};
const  uchar leftforward[19][19]=
{
   FA,FA,FB,FB,FC,FC,FD,FD,FE,FE,FE,FD,FD,FC,FC,FB,FB,FA,FA,
   FA,FB,FB,FC,FC,FD,FD,FE,FE,FF,FE,FE,FD,FD,FC,FC,FB,FB,FA,
   FB,FB,FC,FC,FD,FD,FE,FE,FF,FF,FF,FE,FE,FD,FD,FC,FC,FB,FB,
   FB,FC,FC,FD,FD,FE,FE,FF,FF,FG,FF,FF,FE,FE,FD,FD,FC,FC,FB,
   FC,FC,FD,FD,FE,FE,FF,FF,FG,FG,FG,FF,FF,FE,FE,FD,FD,FC,FC,
   FC,FD,FD,FE,FE,FF,FF,FG,FG,FH,FG,FG,FF,FF,FE,FE,FD,FD,FC,
   FD,FD,FE,FE,FF,FF,FG,FG,FH,FH,FH,FG,FG,FF,FF,FE,FE,FD,FD,
   FD,FE,FE,FF,FF,FG,FG,FH,FH,FI,FH,FH,FG,FG,FF,FF,FE,FE,FD,
   FE,FE,FF,FF,FG,FG,FH,FH,FI,FI,FI,FH,FH,FG,FG,FF,FF,FE,FE,
   FE,FF,FF,FG,FG,FH,FH,FI,FI,FJ,FI,FI,FH,FH,FG,FG,FF,FF,FE,
   FF,FF,FG,FG,FH,FH,FI,FI,FJ,FJ,FJ,FI,FI,FH,FH,FG,FG,FF,FF,
   FF,FG,FG,FH,FH,FI,FI,FJ,FJ,FK,FJ,FJ,FI,FI,FH,FH,FG,FG,FF,
   FG,FG,FH,FH,FI,FI,FJ,FJ,FK,FK,FK,FJ,FJ,FI,FI,FH,FH,FG,FG,
   FG,FH,FH,FI,FI,FJ,FJ,FK,FK,FL,FK,FK,FJ,FJ,FI,FI,FH,FH,FG,
   FH,FH,FI,FI,FJ,FJ,FK,FK,FL,FL,FL,FK,FK,FJ,FJ,FI,FI,FH,FH,
   FH,FI,FI,FJ,FJ,FK,FK,FL,FL,FM,FL,FL,FK,FK,FJ,FJ,FI,FI,FH,
   FI,FI,FJ,FJ,FK,FK,FL,FL,FM,FM,FM,FL,FL,FK,FK,FJ,FJ,FI,FI,
   FI,FJ,FJ,FK,FK,FL,FL,FM,FM,FN,FM,FM,FL,FL,FK,FK,FJ,FJ,FI,
   FJ,FJ,FK,FK,FL,FL,FM,FM,FN,FN,FN,FM,FM,FL,FL,FK,FK,FJ,FJ
};




#define SA 47
#define SB 48
#define SC 49
#define SD 50
#define SE 51
#define SS 52
#define SG 53
#define SH 54
#define SI 55
#define SJ 56
#define SK 57
#define SL 58
#define SM 59
#define SN 60

const  uchar leftforward_START[19][19]=
{
// 0   4  7  1  4  7  2  4  7  3  4  7  4  4  7  5  4  7  6
   SJ,SJ,SK,SK,SL,SL,SM,SM,SN,SN,SN,SM,SM,SL,SL,SK,SK,SJ,SJ,        
   SI,SJ,SJ,SK,SK,SL,SL,SM,SM,SN,SM,SM,SL,SL,SK,SK,SJ,SJ,SI, 
   SI,SI,SJ,SJ,SK,SK,SL,SL,SM,SM,SM,SL,SL,SK,SK,SJ,SJ,SI,SI, 
   SH,SI,SI,SJ,SJ,SK,SK,SL,SL,SM,SL,SL,SK,SK,SJ,SJ,SI,SI,SH, 
   SH,SH,SI,SI,SJ,SJ,SK,SK,SL,SL,SL,SK,SK,SJ,SJ,SI,SI,SH,SH, 
   SG,SH,SH,SI,SI,SJ,SJ,SK,SK,SL,SK,SK,SJ,SJ,SI,SI,SH,SH,SG, 
   SG,SG,SH,SH,SI,SI,SJ,SJ,SK,SK,SK,SJ,SJ,SI,SI,SH,SH,SG,SG,    
   SS,SG,SG,SH,SH,SI,SI,SJ,SJ,SK,SJ,SJ,SI,SI,SH,SH,SG,SG,SS, 
   SS,SS,SG,SG,SH,SH,SI,SI,SJ,SJ,SJ,SI,SI,SH,SH,SG,SG,SS,SS, 
   SE,SS,SS,SG,SG,SH,SH,SI,SI,SJ,SI,SI,SH,SH,SG,SG,SS,SS,SE, 
   SE,SE,SS,SS,SG,SG,SH,SH,SI,SI,SI,SH,SH,SG,SG,SS,SS,SE,SE, 
   SD,SE,SE,SS,SS,SG,SG,SH,SH,SI,SH,SH,SG,SG,SS,SS,SE,SE,SD, 
   SD,SD,SE,SE,SS,SS,SG,SG,SH,SH,SH,SG,SG,SS,SS,SE,SE,SD,SD, 
   SC,SD,SD,SE,SE,SS,SS,SG,SG,SH,SG,SG,SS,SS,SE,SE,SD,SD,SC, 
   SC,SC,SD,SD,SE,SE,SS,SS,SG,SG,SG,SS,SS,SE,SE,SD,SD,SC,SC, 
   SB,SC,SC,SD,SD,SE,SE,SS,SS,SG,SS,SS,SE,SE,SD,SD,SC,SC,SB, 
   SB,SB,SC,SC,SD,SD,SE,SE,SS,SS,SS,SE,SE,SD,SD,SC,SC,SB,SB, 
   SA,SB,SB,SC,SC,SD,SD,SE,SE,SS,SE,SE,SD,SD,SC,SC,SB,SB,SA, 
   SA,SA,SB,SB,SC,SC,SD,SD,SE,SE,SE,SD,SD,SC,SC,SB,SB,SA,SA  
};
const  uchar rightforward_START[19][19]=
{
   SA,SA,SB,SB,SC,SC,SD,SD,SE,SE,SE,SD,SD,SC,SC,SB,SB,SA,SA,
   SA,SB,SB,SC,SC,SD,SD,SE,SE,SS,SE,SE,SD,SD,SC,SC,SB,SB,SA,
   SB,SB,SC,SC,SD,SD,SE,SE,SS,SS,SS,SE,SE,SD,SD,SC,SC,SB,SB,
   SB,SC,SC,SD,SD,SE,SE,SS,SS,SG,SS,SS,SE,SE,SD,SD,SC,SC,SB,
   SC,SC,SD,SD,SE,SE,SS,SS,SG,SG,SG,SS,SS,SE,SE,SD,SD,SC,SC,
   SC,SD,SD,SE,SE,SS,SS,SG,SG,SH,SG,SG,SS,SS,SE,SE,SD,SD,SC,
   SD,SD,SE,SE,SS,SS,SG,SG,SH,SH,SH,SG,SG,SS,SS,SE,SE,SD,SD,
   SD,SE,SE,SS,SS,SG,SG,SH,SH,SI,SH,SH,SG,SG,SS,SS,SE,SE,SD,
   SE,SE,SS,SS,SG,SG,SH,SH,SI,SI,SI,SH,SH,SG,SG,SS,SS,SE,SE,
   SE,SS,SS,SG,SG,SH,SH,SI,SI,SJ,SI,SI,SH,SH,SG,SG,SS,SS,SE,
   SS,SS,SG,SG,SH,SH,SI,SI,SJ,SJ,SJ,SI,SI,SH,SH,SG,SG,SS,SS,
   SS,SG,SG,SH,SH,SI,SI,SJ,SJ,SK,SJ,SJ,SI,SI,SH,SH,SG,SG,SS,
   SG,SG,SH,SH,SI,SI,SJ,SJ,SK,SK,SK,SJ,SJ,SI,SI,SH,SH,SG,SG,
   SG,SH,SH,SI,SI,SJ,SJ,SK,SK,SL,SK,SK,SJ,SJ,SI,SI,SH,SH,SG,
   SH,SH,SI,SI,SJ,SJ,SK,SK,SL,SL,SL,SK,SK,SJ,SJ,SI,SI,SH,SH,
   SH,SI,SI,SJ,SJ,SK,SK,SL,SL,SM,SL,SL,SK,SK,SJ,SJ,SI,SI,SH,
   SI,SI,SJ,SJ,SK,SK,SL,SL,SM,SM,SM,SL,SL,SK,SK,SJ,SJ,SI,SI,
   SI,SJ,SJ,SK,SK,SL,SL,SM,SM,SN,SM,SM,SL,SL,SK,SK,SJ,SJ,SI,
   SJ,SJ,SK,SK,SL,SL,SM,SM,SN,SN,SN,SM,SM,SL,SL,SK,SK,SJ,SJ
};

/**/
void ATD_Position(void)
          {
              ATD1_Get_Rog();
              ATD0_Get_Rog();//AD采集数据
            if(direction==0x08)
            {
              FrontPositionT=position(AD_bData0,7);//求取编码位置
              e_f=position_sw(FrontPositionT);//得到传感器位置对应规则表标号
              if(first_start==TRUE) //如果程序初次运行则需要把当前位置当作运行前的位置状态来处理
                {
                  e_fL=e_f;
                  first_start=FALSE;//取消首次运行标志
                }
              if((e_fL<=2)&&(e_f>9))e_f=e_fL;//防止全部传感器出白线后的乱位置检测起作用
              else if((e_fL>=16)&&(e_f<9))e_f=e_fL;
              e_fL=e_f;//保存本次状态
            }
             else if(direction==0x04)
             {
               FrontPositionT=position(AD_bData1,7);
               e_b=position_sw(FrontPositionT);
               if(first_start==TRUE) //如果程序初次运行则需要把当前位置当作运行前的位置状态来处理
                {
                  e_bL=e_b;
                  first_start=FALSE;//取消首次运行标志
                }
              if((e_bL<=2)&&(e_b>9))e_b=e_bL;//防止全部传感器出白线后的乱位置检测起作用
              else if((e_bL>=16)&&(e_b<9))e_b=e_bL;
              e_bL=e_b;//保存本次状态
             }
            
          }