zq_taskstart_vardef.c

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

/*修改记录:
  V1.00 2008年6月7日 3点34分
        修改目的:
                  以前的地图中每个Step中只能存储一种动作状态,而在实际中可能有循线数线手臂上升同时运行的情况.
        修改位置:
                  对地图中的值进行了修改,同一个行中可以包含多个信息.对查询地图方式进行修改,取消查地图优先级,所有动作状态将处在同一优先级别.
  需要注意的问题: 
                  对于任务的调度问题,由于不是每次只执行一种状态,那么就需要对多动作状态的完成返回
                  过程进行限制,防止在其中一个动作任务完成而另一个动作任务尚未完成的时候查询地图而
                  导致任务失败
  V1.01 2008年6月18日 4点30分
        修改目的：
                  增加一个手臂伸缩任务并在地图中增加相应的启动任务标识，满足不同机器人的手臂需求
        修改位置：
                  在地图中增加了一个列，这一列记录路线中要手臂伸缩的位置及动作是伸或缩
                  在查询地图程序中增加了相应的手臂伸缩动作检测部分
                  创建了手臂伸缩任务
                  在TaskStart.C中增加了判断手臂伸缩完成再进行新一轮地图查询的设置
*/
/****************************************************************************
#define NOLL000  0//不动作                 NOLL0000
#define Dirc_Fr 1//前进               Front
#define Dirc_Bc  2//后退               Back2
#define Angl_Ri 1//右转             Right
#define Angl_Le  2//左转             Left2
#define Angl_90    90//转弯90度        Ag090
#define Angl_18   180//转弯180度      Ag180
#define Hand_Co  1//手臂抓取  (Closed)  Close
#define Hand_Op 2//手臂松开   (Open)    Open2
#define Hand_Up   1//手臂抬升 (Up)      Up001
#define Hand_Dw 2//手臂下降   (Down)    Down2
#define Hand_Sp 1 //手臂伸长  (Spread)  Sprd1
#define Hand_Sh  2 //手臂缩短 (Short)   Short
#define Line_01   1
#define Line_02   2
#define Line_03   3
#define Line_04   4
#define Line_05   5
#define Line_06   6
#define Line_07   7
#define Line_08   8
#define Line_09   9
#define Sped_15   15
#define Sped_20   20
#define Sped_25   25
#define Sped_30   30
#define Sped_35   35
#define Sped_40   40
#define Sped_45   45
#define Sped_50   50
#define Sped_55   55
#define Sped_60   60
#define Sped_65   65

//    Line_01,  Dirc_Fr,  Sped_50,  Angl_Le,  Angl_90,  Hand_Op,  Hand_Up,  Hand_Sp,
//    Line_02,  Dirc_Bc,  Sped_30,  Angl_Ri,  Angl_18,  Hand_Co,  Hand_Dw,  Hand_Sh,

****************************************************************************/
const uchar Run_Linemap[40][8]= //[40]当前步骤,[8]中[0]数线次数,[1]机器人方向,[3]转弯方向,[4]转弯角度,[5]手臂动作,[6]上升下降动作
  {
      Line_05,  Dirc_Fr,  Sped_50,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  Angl_Le,  Angl_90,  NOLL000,  NOLL000,  NOLL000,
      
      Line_05,  Dirc_Fr,  Sped_50,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  Angl_Ri,  Angl_90,  NOLL000,  NOLL000,  NOLL000,
      
      Line_05,  Dirc_Fr,  Sped_50,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  Angl_Ri,  Angl_18,  NOLL000,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  Angl_Ri,  Angl_18,  NOLL000,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  Angl_Ri,  Angl_18,  NOLL000,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  Angl_Ri,  Angl_18,  NOLL000,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  Hand_Co,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  Hand_Up,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  Angl_Le,  Angl_18,  NOLL000,  NOLL000,  NOLL000,
      
      Line_02,  Dirc_Fr,  Sped_50,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  Hand_Sp,
      
      NOLL000,  NOLL000,  NOLL000,  Angl_Ri,  Angl_18,  NOLL000,  NOLL000,  Hand_Sh,
      
      Line_02,  Dirc_Fr,  Sped_50,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  Angl_Le,  Angl_18,  NOLL000,  NOLL000,  NOLL000,
      
      Line_02,  Dirc_Fr,  Sped_50,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  Angl_Ri,  Angl_18,  NOLL000,  NOLL000,  NOLL000,
      
      Line_02,  Dirc_Fr,  Sped_50,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  Angl_Le,  Angl_18,  NOLL000,  NOLL000,  NOLL000,
      
      Line_02,  Dirc_Fr,  Sped_50,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  Angl_Ri,  Angl_18,  NOLL000,  NOLL000,  NOLL000,
      
      Line_02,  Dirc_Fr,  Sped_50,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  Hand_Op,  NOLL000,  NOLL000,
      
      NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  Hand_Dw,  NOLL000,
      
      Line_02,  Dirc_Fr,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
      
      Line_02,  Dirc_Fr,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
      
      Line_02,  Dirc_Fr,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
      
      Line_02,  Dirc_Fr,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
      
      Line_02,  Dirc_Fr,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,  NOLL000,
  };
uchar Line_Step=0;//线路动作步骤  
uchar Line_num,Dir_FrBc,Angle_RL,Angle_9_18,Hand_OpCo,Hand_UpDw,Hand_SpSh;
BOOLEAN Key_TaskSnatch_EN,Key_TaskUpdown_EN,key_TaskSpreadShort_EN;
uint MidspeedC;
void ZQ_Run(void)
  {
    Line_num  =Run_Linemap[Line_Step][0];//读取数线次数
    Dir_FrBc  =Run_Linemap[Line_Step][1];//读取循线的方向
    MidspeedC =Run_Linemap[Line_Step][2];//读取循线的速度设定值
    Angle_RL  =Run_Linemap[Line_Step][3];//读取转弯的方向
    Angle_9_18=Run_Linemap[Line_Step][4];//读取转弯的角度
    Hand_OpCo =Run_Linemap[Line_Step][5];//读取手臂的抓放动作
    Hand_UpDw =Run_Linemap[Line_Step][6];//读取手臂的上升下降动作
    Hand_SpSh =Run_Linemap[Line_Step][7];//读取手臂的伸缩动作
    
    Line_Step++;
    if(Line_num!=NOLL000)//数线不为零表示这个动作是数线动作
      {
         if(Dir_FrBc==Dirc_Fr)Follow_Line(TRUE,Line_num,0x08);//正向行走
          else Follow_Line(TRUE,Line_num,0x04);//反向行走 
         Follow_Start=TRUE;//恢复数线任务
      }
    if(Angle_RL!=NOLL000)//转向不为零表示这个动作为转向动作
      {
        Angle_RL_9_18(Angle_RL,Angle_9_18);//调用转弯任务开启函数
        Key_TaskSwerve_EN=TRUE;//恢复转弯任务
      }
    if(Hand_OpCo!=NOLL000)
      {
        Hand_opco(Hand_OpCo);
        Key_TaskSnatch_EN=TRUE;//恢复手臂抓取任务
      }
    if(Hand_UpDw!=NOLL000)
      {
        Hand_updw(Hand_UpDw);
        Key_TaskUpdown_EN=TRUE;
      }
    if(Hand_SpSh!=NOLL000)
      {
        Hand_spsh(Hand_SpSh);
        key_TaskSpreadShort_EN=TRUE;
      }
  }