user_ware.c

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

#include "includes.h"
#define EXT extern
#include "main.h"

#define Num 3 //比较的系数
/****************************************************************/
/*计算传感器位置值*/
 uchar position(uchar atd[],uchar d)
{
  uint var_ad0[5];//存相加后的数据
  uint var_ad00[2];
  uchar var_i;
  uchar min_data;//记录最小数据组位置
  uint var_min=1000;
  
  for(var_i=0;var_i<d;var_i++)
    {
      atd[var_i]=255-atd[var_i];
    }
 {
    

  for(var_i=0;var_i<5;var_i++)//三个采样值累加
    {
      var_ad0[var_i]=atd[var_i]+atd[var_i+1]+atd[var_i+2];
    }
  
  for(var_i=0;var_i<5;var_i++)//计算累加后的最小值,排除污点得到白线所在位置
    {
      if(var_ad0[var_i]<=var_min)
        {
          var_min=var_ad0[var_i];
          min_data=var_i; //存放累加最小值的位置
        }
    }
  var_ad00[0]=atd[min_data]+atd[min_data+1];//计算最小值中三个数据两个相邻值之和
  var_ad00[1]=atd[min_data+1]+atd[min_data+2];
  
  if(var_ad00[0]>var_ad00[1])min_data+=1;//如果三个数中相邻两个之和最小值在第二三个中,min_data等于三个数中间的那个的位置标号
  var_ad00[0]=atd[min_data];//存放和最小的两个数据到uint型变量中
  var_ad00[1]=atd[min_data+1];
  if(var_ad00[0]>(2*var_ad00[1]))return((min_data+1)*10);//次小值大于三倍最小值,返回10倍最小值位置
  else if(var_ad00[1]>(2*var_ad00[0]))return(min_data*10);//次小值大于三倍最小值,返回最小值位置+1的10倍值
  else if(var_ad00[0]>var_ad00[1])return(min_data*10+7);//次小值不大于三倍最小值,返回10倍最小值位置+4
  else return(min_data*10+4);//次小值不大于三倍最小值,返回10倍最小值位置+7
  }
}


/***********************************************************/
/*查表得到位置值表示的表标号*/
uchar position_sw(uchar e)
  {
    switch(e)
      {
        case  0:return(0);break;
        case  4:return(1);break;
        case  7:return(2);break;
        case 10:return(3);break;
        case 14:return(4);break;
        case 17:return(5);break;
        case 20:return(6);break;
        case 24:return(7);break;
        case 27:return(8);break;
        case 30:return(9);break;
        case 34:return(10);break;
        case 37:return(11);break;
        case 40:return(12);break;
        case 44:return(13);break;
        case 47:return(14);break;
        case 50:return(15);break;
        case 54:return(16);break;
        case 57:return(17);break;
        case 60:return(18);break;
        default:;
        
      }
  }
  
  
/********************************************************************/
/*
BOOLEAN Line_Count(uchar AD_bData,uchar numb)
数线函数  <这个函数已放弃不用,效果不是很好>
*/  
/********************************************************************/  
#define COM_DATA 120  //比较值，作为数线的标准值
BOOLEAN Line_Count(uchar AD_bData[],uchar numb)
  {
    if(((AD_bData[0]<COM_DATA)&&(AD_bData[5]<COM_DATA)&&(AD_bData[6]<COM_DATA))||
       ((AD_bData[0]<COM_DATA)&&(AD_bData[4]<COM_DATA)&&(AD_bData[5]<COM_DATA))||
       ((AD_bData[1]<COM_DATA)&&(AD_bData[5]<COM_DATA)&&(AD_bData[6]<COM_DATA))||
       ((AD_bData[1]<COM_DATA)&&(AD_bData[4]<COM_DATA)&&(AD_bData[5]<COM_DATA))||
       ((AD_bData[0]<COM_DATA)&&(AD_bData[1]<COM_DATA)&&(AD_bData[5]<COM_DATA))||
       ((AD_bData[0]<COM_DATA)&&(AD_bData[1]<COM_DATA)&&(AD_bData[6]<COM_DATA))||
       ((AD_bData[1]<COM_DATA)&&(AD_bData[2]<COM_DATA)&&(AD_bData[5]<COM_DATA))||
       ((AD_bData[1]<COM_DATA)&&(AD_bData[2]<COM_DATA)&&(AD_bData[6]<COM_DATA)))return(TRUE);
     else return FALSE;
  }
/********************************************************************/
/*
  void Follow_Line(BOOLEAN Follow_Start,uchar Follow_Num,uchar Follow_Dir)循线函数
         Follow_Strt循线开始标志        
         Follow_Numb循线数线次数
         Follow_Dir循线的电机方向
*/
/********************************************************************/
BOOLEAN Follow_Start,first_start;
uchar Follow_Num,direction,directionR,directionL;
void Follow_Line(BOOLEAN Follow_Strt,uchar Follow_Numb,uchar Follow_Dir)
  {
      Follow_Start=Follow_Strt;//equ TRUE start to Follow Line
      Follow_Num=Follow_Numb;//The Follow Line Number equ your give
      direction=Follow_Dir;//equ 0x08 is forward direction, equ 0x04 is back direction 
      directionR=directionL=direction;//前进后退的时候两个电机方向相同
      first_start=TRUE;   
  }

/********************************************************************/
/*
void Dir_RL_9_18(uchar dir,uchar d_9_18)  转向函数
        dir转向方向
        d_9_18转向角度(90度或者180度)
*/
/********************************************************************/
uchar angle;
BOOLEAN Key_TaskSwerve_EN=FALSE;
void Angle_RL_9_18(uchar Angle_dir,uchar A_9_18)
  {
     if(Angle_dir==Angl_Ri)//右转
      {
        directionR=0x04;
        directionL=0x08;
      }
     else if(Angle_dir==Angl_Le)//左转 
      {
        directionR=0x08;
        directionL=0x04;
      }
         angle=A_9_18;
  }
  
/********************************************************************/
/*
void Hand_gg(uchar hand_GG) 抓取模块函数
*/
/********************************************************************/
void Hand_opco(uchar hand_OPCO)
  {
    if(hand_OPCO==Hand_Op)
      {
        
      }
    else if(hand_OPCO==Hand_Co)
      {
        
      }
  }
  
/********************************************************************/
/*
void Hand_ud(uchar hand_UD) 手臂升降函数
*/
/********************************************************************/
void Hand_updw(uchar hand_UPDW)
  {
    if(hand_UPDW==Hand_Up)
      {
        
      }
    else if(hand_UPDW==Hand_Dw)
      {
        
      }
  }

/********************************************************************/
/*
void Hand_ss(uchar Hand_Ss) 手臂伸缩函数
*/
/********************************************************************/
void Hand_spsh(uchar Hand_SPSH)
  {
    if(Hand_SPSH==Hand_Sp)
      {
        
      }
    else if(Hand_SPSH==Hand_Sh)
      {
        
      }
  }