--LUA语言在机器人行业中的应用:抓娃娃机动作程序-- --Build Date: 2017-5-9 by Anthony Yoo --输入信号-- local front,behind,left,right,down = 2,0,3,4,1 --摇杆控制:前、后、左、右、下(同时还能实现伺服使能和系统启动的功能) local safeSpace = 5 --安全空间监测 --输出信号-- local catch = 22 --抓取 --其它变量-- local pHere local limitSpace,qiyiSpace = 100,40 --进入设定的XY坐标区域时限制速度(减速处理) local homeSpeed,downSpeed,putPlace = 30,30,15 --回待机点速度,下降速度,放娃娃速度 --点位-- local standby,putDollplace = p10,p11 --待机点,放娃娃位置 DO(catch,OFF) --手爪复位 WDI(1,ON) --等待启动 Delay(50) MotOn() --伺服使能 Delay(50) sysrate(homeSpeed) Delay(100) MovJ(J3,8) MovP(standby,"CP=80") --待机点 --------------------------------------------------------------------------- while true do Delay(5) --程序防呆 --------------------------------------------------------------------------- if DI(front) == ON then --X+ publicwrite(0x00,1) --通知CPU#2灯慢闪 print("向前方向运行\n") if DI(safeSpace) == ON then --如果安全空间被触发 linerun(AX,-1,30) --往反方向走,避开中心的奇异点 else pHere = getcart() --获取当前笛卡尔坐标 if pHere.x <= limitSpace and pHere.x >= -limitSpace and pHere.y >= -limitSpace and pHere.y <= limitSpace then linerun(AX,1,15) --减速 elseif pHere.x <= qiyiSpace and pHere.x >= -qiyiSpace and pHere.y >= -qiyiSpace and pHere.y <= qiyiSpace then linerun(AX,1,5) --减速(接近奇异点) else linerun(AX,1,80) --加速 end end elseif DI(behind) == ON then --X- publicwrite(0x00,1) --通知CPU#2灯慢闪 print("向后方向运行\n") if DI(safeSpace) == ON then --如果安全空间被触发 linerun(AX,1,30) --往反方向走 else pHere = getcart() if pHere.x <= limitSpace and pHere.x >= -limitSpace and pHere.y >= -limitSpace and pHere.y <= limitSpace then linerun(AX,-1,15) elseif pHere.x <= qiyiSpace and pHere.x >= -qiyiSpace and pHere.y >= -qiyiSpace and pHere.y <= qiyiSpace then linerun(AX,-1,5) else linerun(AX,-1,80) end end elseif DI(left) == ON then --Y+ publicwrite(0x00,1) --通知CPU#2灯慢闪 print("向左方向运行\n") if DI(safeSpace) == ON then --如果安全空间被触发 linerun(AY,-1,30) --往反方向走 else pHere = getcart() if pHere.x <= limitSpace and pHere.x >= -limitSpace and pHere.y >= -limitSpace and pHere.y <= limitSpace then linerun(AY,1,15) elseif pHere.x <= qiyiSpace and pHere.x >= -qiyiSpace and pHere.y >= -qiyiSpace and pHere.y <= qiyiSpace then linerun(AY,1,5) else linerun(AY,1,80) end end elseif DI(right) == ON then --Y- publicwrite(0x00,1) --通知CPU#2灯慢闪 print("向右方向运行\n") if DI(safeSpace) == ON then --如果安全空间被触发 linerun(AY,1,30) --往反方向走 else pHere = getcart() if pHere.x <= limitSpace and pHere.x >= -limitSpace and pHere.y >= -limitSpace and pHere.y <= limitSpace then linerun(AY,-1,15) elseif pHere.x <= qiyiSpace and pHere.x >= -qiyiSpace and pHere.y >= -qiyiSpace and pHere.y <= qiyiSpace then linerun(AY,-1,5) else linerun(AY,-1,80) end end elseif DI(down) == ON then --Z-- publicwrite(0x00,2) --通知CPU#2灯快闪 print("向下方向运行\n") sysrate(downSpeed) MovJ(J3,-120) --下去 DO(catch,ON) --手爪夹紧 Delay(500) MovJ(J3,8) --上来 sysrate(putPlace) Delay(10) MovP(putDollplace,"CP=80") --放娃娃位置 Delay(500) publicwrite(0x00,3) --通知CPU#2灯常亮 DO(catch,OFF) --手爪张开 sysrate(homeSpeed) Delay(20) MovP(standby,"CP=80") --回待机位置 elseif DI(0)==OFF and DI(1)==OFF and DI(2)==OFF and DI(3)==OFF and DI(4)==OFF then publicwrite(0x00,3) --通知CPU#2灯常亮 stoprun() --方向控制键和下去控制键都没信号时,停止运行 end --------------------------------------------------------------------------- end ---------------------------------------------------------------------------
LUA语言在机器人行业中的应用:抓娃娃机动作程序
猜你喜欢
转载自blog.csdn.net/iss_mk_gmbh/article/details/71512936
今日推荐
周排行