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