start()

begin//延时10秒   delay(5)//初始化变量call init()//清空原有运动堆栈resetMotion()//建立上电任务taskCreate "robotpower",100,robotpower()wait(isPowered())//建立生产任务taskCreate "ProductionCycle",10,ProductionCycle()//建立安全区域检测任务taskCreate "CheckSafeArea",10,CheckSafeArea()//建立屏幕刷新任务taskCreate "HMI",80,HMI()//  //
end

init()

begin_bPartReverse=false_bPartRight=false_bCheckArrived=false_bStartCheck=false_bSyncErr=falsedoERobotDone=falsedoEhome=falsedoERobotError=falseopen(tGripper)_nDifY=0_nItem=0sState="已停止"
end

HMI()

beginuserPage()//切换到用户屏幕cls()gotoxy(0,2)put("生产状态：")gotoxy(0,4)put("工件序号：")gotoxy(0,6)put("光电信号：")gotoxy(0,8)put("正反检测：")gotoxy(0,10)put("检测偏差：")while true//刷新生产状态gotoxy(10,2)put("                          ")gotoxy(10,2)put(sState)//刷新工件序号gotoxy(10,4)put("                          ")gotoxy(10,4)put(toString("",_nItem+1))//刷新激光传感器信号gotoxy(10,6)put("                          ")if diFCheckSensor==truegotoxy(10,6)put("ON")elsegotoxy(10,6)put("OFF")endIf//刷新正反向判断gotoxy(10,8)put("                          ")if _bPartRightgotoxy(10,8)put("正向")elseIf _bPartReversegotoxy(10,8)put("反向")endIf//刷新检测偏差gotoxy(10,10)put("                          ")gotoxy(10,10)put(toString("0.2",_nDifY)+"mm")delay(0)endWhile
end

robotpower()

beginwhile true//控制器在远程模式// if (workingMode()==4 or workingMode()==1)if workingMode()==4if !isPowered()enablePower()//控制器运行模式切换延时时间delay(1)wait(isPowered())endIfelseIf workingMode()==1enablePower()//wait(isPowered())elseIf workingMode()==2 or workingMode()==3disablePower()doERobotDone=falseendIfdelay(0)endWhile
end

ProductionCycle()

begin//开始回零//wait(diEPlcReady==true or (workingMode()==1 and isPowered()and delay(2)) or doEhome==true)wait(diEPlcReady==true or doEhome==true or diEManualStart==true)doERobotDone=falsesState="回原点"l_pHome=jointToPoint(tGripper,world,jHome)l_pHere=here(tGripper,world)l_pHere.trsf.z=max(l_pHere.trsf.z,50)movel(l_pHere,tGripper,mSpeedL)movej(jHome,tGripper,mSpeedL)waitEndMove()open(tGripper)doEhome=true//循环生产while truecall Pallet()delay(0)endWhile
end

CheckSafeArea()

beginwhile truel_pHere=here(tGripper,world)if l_pHere.trsf.x>nXLimitMax or l_pHere.trsf.x<nXLimitMin or l_pHere.trsf.y>nYLimitMax or l_pHere.trsf.y<nYLimitMin// disablePower()stopMove()resetMotion()l_bAlarm=trueelsel_bAlarm=falseendIfif l_bAlarm and !l_bLastAlarmpopUpMsg("机器人超出安全工作空间！")logMsg("机器人超出安全工作空间！")endIfl_bLastAlarm=l_bAlarm//判断机器人是否在home位l_pHome=jointToPoint(tGripper,world,jHome)if distance(l_pHere,l_pHome)<5doEhome=trueelsedoEhome=falseendIfdelay(0)endWhile
end

Pallet()

begin//在进行开启/关闭阀门时//最好是使用waitendmove()等点到位以后再使用//wait(diEPlcDone==true)//doERobotDone=falsewait(diEStart==true or diEManualStart==true)doERobotDone=false//if !isPowered() //call start()//endIf//抓放循环，5次for _nItem=0 to 4//运动开始//wait(diEStart==true or workingMode()==1)//去取件sState="取件中"l_pPickAppro=appro(pPick[_nItem],{0,0,-nPickAppro,0,0,0})movej(l_pPickAppro,tGripper,mSpeedH)doEhome=falsemovel(pPick[_nItem],tGripper,mSpeedL)waitEndMove()close(tGripper)l_nMoveID=movel(l_pPickAppro,tGripper,mSpeedL)//去检测wait(getMoveId()>l_nMoveID+0.5)sState="检测中"movej(pCheckReady,tGripper,mSpeedH)movel(pCheckStart,tGripper,mSpeedL)l_nMoveID=movel(pCheckOver,tGripper,mSpeedLL)//开始检测激光传感器状态wait(getMoveId()>l_nMoveID)//        _bStartCheck=true//        wait(_bCheckArrived)//        _bCheckArrived=false//    do//    until (diFCheckSensor==false)//传感器感应到后，停止手臂运动，清除运动指令//检测导光条的边缘，若检测不到信号则报警if watch(diFCheckSensor==false,3)==falsesState="无导光条"doERobotError=truestopMove()resetMotion()wait(diEReset==true)doERobotError=false//PLC复位后给出回原信号，机械手回原wait(diEPlcReady==true)doERobotDone=falsedoERobotError=falsesState="回原点"l_pHome=jointToPoint(tGripper,world,jHome)l_pHere=here(tGripper,world)l_pHere.trsf.z=max(l_pHere.trsf.z,50)movel(l_pHere,tGripper,mSpeedL)movej(jHome,tGripper,mSpeedL)waitEndMove()open(tGripper)doEhome=truereturnendIfstopMove()resetMotion()//检测到导光条，外部绿灯亮doESensor=!diFCheckSensor//delay(0.5)//计算工件Y向位置偏差l_pHere=here(tGripper,world)_nDifY=l_pHere.trsf.y-pCheckMark.trsf.y//向上提起，判断正反向l_pCheckRight=l_pHere//向上提起到pCheckUp的高度l_pCheckRight.trsf.z=pCheckUp.trsf.z//向里伸入0.2mml_pCheckRight.trsf.y=l_pHere.trsf.y+0.2movel(l_pCheckRight,tGripper,mSpeedL)waitEndMove()//延时检测delay(0.1)//如果光电感应不到，判断为正向,并计算调整后的放件位置if diFCheckSensor==true_bPartRight=true_bPartReverse=false//l_pPlace=compose(pPlace[0,_nItem],fPlacePallet,{0,_nDifY,0,0,0,0})l_pPlace=appro(pPlace[0,_nItem],{-_nDifY,0,0,0,0,0})//否则，为反向,并计算调整后的放件位置else_bPartRight=false_bPartReverse=true//l_pPlace=compose(pPlace[1,_nItem],fPlacePallet,{0,-_nDifY,0,0,0,0})l_pPlace=appro(pPlace[1,_nItem],{-_nDifY,0,0,0,0,0})endIf//向上提起，离开检测区l_pCheckAppro=appro(l_pCheckRight,{0,0,-20,0,0,0})movel(l_pCheckAppro,tGripper,mSpeedL)if _bPartRightl_nMoveID=movej(pCheckReady,tGripper,mSpeedH)elsel_nMoveID=movej(jhomen,tGripper,mSpeedH)endIfwaitEndMove()//去放件wait(getMoveId()>l_nMoveID+0.3)sState="放件中"//外部绿灯灭doESensor=false//去放件准备位l_pPlaceAppro=appro(l_pPlace,{0,0,-nPlaceAppro,0,0,0})movej(l_pPlaceAppro,tGripper,mSpeedH)//根据压入深度，计算压入点l_pPress=appro(l_pPlace,{0,0,nPressDeepth,0,0,0})//计算压入准备位，在压入点上方nPressAppro高，角度与放件位相同。l_pPressAppro=appro(l_pPress,{0,0,-nPressAppro,0,0,0})//到压入准备位,打开夹爪movel(l_pPressAppro,tGripper,mSpeedL)waitEndMove()open(tGripper)//延时nPressDelay s,保证夹爪已打开delay(nPressDelay)//压入卡槽movel(l_pPress,tGripper,mSpeedL)//回准备位l_nMoveID=movel(l_pPlaceAppro,tGripper,mSpeedL)wait(getMoveId()>l_nMoveID+0.5)_bPartReverse=false_bPartRight=falsedelay(0)endFor//放件完成，回原点,发送完成信号sState="回原点"movej(jHome,tGripper,mSpeedH)waitEndMove()doERobotDone=true//     delay(10)
end