--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
---------------------------------------------------------------------------