检测机器人当前是否在机械原点或等待位置

检测机器人当前是否在机械原点

• 获取机器人当前关节数据jointdata，与预定义的机械原点在一个自定义的范围内作对比。
• RAPID程序

  ! description 					   : 检测机器人当前是否在机械原点，默认机械原点各轴位置为0
  ! optional param {num} errorRange : 可选参数，容错范围，默认值是0.5毫米（线性轴）或度（旋转轴）
  ! return {bool}				   : 若机器人当前在以机械原点为中心的区间内，返回TRUE，否则返回FALSE
  FUNC bool isRobotAtHome(\num errorRange)
      ! 记录各个轴对比的计数器
      VAR num counter := 0;
      ! 定义容错范围
      VAR num errRange := 0.5;
      ! 定义当前关节数据
      VAR jointtarget curJointTar;
      VAR robjoint curRobJ;
      
      ! 检测是否启用可选参数
      IF Present(errorRange) errRange := errorRange;
      ! 获取当前关节数据
      curJointTar := CJointT();
      curRobJ := curJointTar.robax;
      
      ! 各个轴与以机械原点为中心的区间（-errRange, errRange）进行对比
      IF curRobJ.rax_1>-errRange AND curRobJ.rax_1<errRange Incr counter;
      IF curRobJ.rax_2>-errRange AND curRobJ.rax_2<errRange Incr counter;
      IF curRobJ.rax_3>-errRange AND curRobJ.rax_3<errRange Incr counter;
      IF curRobJ.rax_4>-errRange AND curRobJ.rax_4<errRange Incr counter;
      IF curRobJ.rax_5>-errRange AND curRobJ.rax_5<errRange Incr counter;
      IF curRobJ.rax_6>-errRange AND curRobJ.rax_6<errRange Incr counter;
      RETURN counter = 6;
  ENDFUNC
  

检测机器人当前是否在等待位置

• 获取机器人当前位置数据robtarget，与指定的等待位置在一个自定义的范围内作对比。
• RAPID程序

  ! description                     : 检测机器人当前是否在等待位置
  ! param {robtarget} waitPos       : 等待位置
  ! param {tooldata} toolOnWaitPos  : 创建等待位置的工具数据
  ! param {wobjdata} wobjOnWaitPos  : 创建等待位置的工件数据
  ! optional param {num} transRange : 可选参数，位置坐标的容错范围
  ! optional param {num} rotRange   : 可选参数，姿态数据四元数的容错范围
  ! return {bool}                   : 若机器人当前在以等待位置为中心的区间内，返回TRUE，否则返回FALSE
  FUNC bool isRobotAtWaitPos(robtarget waitPos,INOUT tooldata toolOnWaitPos,INOUT wobjdata wobjOnWaitPos,\num transRange,\num rotRange)
      ! 进行位置数据对比的计数器
      VAR num counter:=0;
      ! 定义默认的位置坐标区间范围，姿态数据区间范围
      VAR num tRange:=25;
      VAR num rRange:=0.1;
      ! 定义当前位置数据
      VAR robtarget curPos;
  
      ! 检测是否启用可选参数
      IF Present(transRange) tRange:=transRange;
      IF Present(rotRange) rRange:=rotRange;
      ! 获取当前位置数据
      curPos := CRobT(\Tool:=toolOnWaitPos, \WObj:=wobjOnWaitPos);
      
      ! 对比位置坐标和姿态数据
      IF curPos.trans.x>waitPos.trans.x-tRange AND curPos.trans.x<waitPos.trans.x+tRange Incr counter;
      IF curPos.trans.y>waitPos.trans.y-tRange AND curPos.trans.y<waitPos.trans.y+tRange Incr counter;
      IF curPos.trans.z>waitPos.trans.z-tRange AND curPos.trans.z<waitPos.trans.z+tRange Incr counter;
      IF curPos.rot.q1>waitPos.rot.q1-rRange AND curPos.rot.q1<waitPos.rot.q1+rRange Incr counter;
      IF curPos.rot.q2>waitPos.rot.q2-rRange AND curPos.rot.q2<waitPos.rot.q2+rRange Incr counter;
      IF curPos.rot.q3>waitPos.rot.q3-rRange AND curPos.rot.q3<waitPos.rot.q3+rRange Incr counter;
      IF curPos.rot.q4>waitPos.rot.q4-rRange AND curPos.rot.q4<waitPos.rot.q4+rRange Incr counter;
      
      RETURN counter=7;
  ENDFUNC