'*******************************************************ECAT总线初始化

global CONST BUS_TYPE = 0				'总线类型。用于上位机区分当前模式
global CONST MAX_AXISNUM = 16			'最大轴数
global CONST Bus_Slot	= 0				'槽位号0
global CONST Bus_AxisStart	 = 0	    '总线轴起始轴号
global CONST Bus_IoStart = 16	    	'总线IO起始编号

global Bus_InitStatus			'总线初始化完成状态
Bus_InitStatus = -1
global  Bus_TotalAxisnum		'检查扫描的总轴数
global ecatscan_num
ecatscan_num = vr(50)
ecatscan_num = ecatscan_num+1
vr(50) = ecatscan_num
?"执行次数 "ecatscan_num
VRSTRING (1000+ ecatscan_num*10,10) = TIME$
'modbus_string(100+ ecatscan_num*10,10) = TIME$
?"执行时间"VRSTRING (1000+ ecatscan_num*10,10)

if vr(50)>300 then 
	?"执行次数超过300 清零 "
	CLEAR() 
endif


global dim startPos
global dim endPos
global dim g_cmd
global dim taskNum
global dim restTaskMark
global dim opNum
startPos = 0
endPos = 0
g_cmd = 0
taskNum = 2
opNum = 0
restTaskMark = 0


RAPIDSTOP(2)					'所有轴立即停止
delay(3000)				'延时3S等待驱动器上电
SLOT_STOP(Bus_Slot)			'总线停止
SYSTEM_ZSET=1+2^7
delay(100)
'**********************初始化ECAT总线 
Ecat_Init()

end

global sub Ecat_Init()
	local temp_axis,drive_vender,drive_device,drive_alias
	'RAPIDSTOP(2)					'所有轴立即停止
	for i=0 to MAX_AXISNUM - 1		'初始化还原轴类型					
		AXIS_ENABLE(i) = 0			'所有轴使能关闭
		atype(i) = 0
		DELAY(10)					'防止所有驱动器全部同时切换使能导致瞬间电流过大
	next
	
	'REG_INPUTS=$3210  'R0-R3分别对应输入口0，1，2，3
	Bus_InitStatus = -1
	Bus_TotalAxisnum = 0	
	'SLOT_STOP(Bus_Slot)			'总线停止
	delay(200)
	SLOT_SCAN(Bus_Slot)			'开始扫描
	if return then 
	
		?"总线扫描成功","连接设备数："NODE_COUNT(Bus_Slot)
		?
		?"开始映射轴号"
		for i=0 to NODE_COUNT(Bus_Slot)-1						'遍历总线下所有从站节点
			drive_vender = NODE_INFO(Bus_Slot,i,0)			'读取驱动器厂商
			drive_device = NODE_INFO(Bus_Slot,i,1)			'读取设备编号
			drive_alias = NODE_INFO(Bus_Slot,i,3)				'读取设备拨码ID
			if NODE_AXIS_COUNT(Bus_Slot,i) <>0 then			'判断当前节点是否有电机
				for j=0 to NODE_AXIS_COUNT(Bus_Slot,i)-1
					temp_axis = Bus_AxisStart + Bus_TotalAxisnum
				  ?"temp_axis:"temp_axis
					base(temp_axis)
					AXIS_ADDRESS = (Bus_Slot<<16)+ Bus_TotalAxisnum + 1	'映射轴号,槽位间隔65536
					ATYPE=65									'设置控制模式 65-位置 66-速度 67-转矩
					if  drive_vender = $168 and drive_device= $b then	'鸣志 自定义PDO
						DRIVE_PROFILE = 12	
						'Set_MZ_PDO(i)
'					elseif temp_axis=0 then 
'						DRIVE_PROFILE =  12
'						REG_INPUTS=$1010  'R0-R3分别对应输入口0，1，2，3
'					elseif drive_vender = $100000 then 
'						DRIVE_PROFILE =  12
'						REG_INPUTS=$1010  'R0-R3分别对应输入口0，1，2，3
					else
						DRIVE_PROFILE = 4						'PDO带驱动器输入输出对象字典
					endif
					disable_group(temp_axis)					'每轴单独分组
					DRIVE_IO = 128 + (temp_axis)*32			'映射驱动器上的IO状态
					'REV_IN = 128 + (temp_axis)*32
					FWD_IN = -1
					DATUM_IN = 130 + (temp_axis)*32
					REV_IN = 128 + (temp_axis)*32
					INVERT_IN(128 + (temp_axis)*32,ON)					
					'INVERT_IN(129 + (temp_axis)*32,ON)
					INVERT_IN(130 + (temp_axis)*32,OFF)
					Bus_TotalAxisnum=Bus_TotalAxisnum+1			'总轴数+1
				next
			else												'IO扩展模块
				NODE_IO(Bus_Slot, i) = Bus_IoStart + 32*i		'总线连接先连总线IO模块
			endif
		next
	
		?"轴号映射完成","连接总轴数："Bus_TotalAxisnum
		wa 2000
	
		SLOT_START(Bus_Slot)									'启动总线
		if return then 
			?"总线开启成功"
		
			?"开始清除驱动器错误(根据驱动器数据字典设置)"
'			for i= Bus_AxisStart to Bus_AxisStart + Bus_TotalAxisnum - 1 
'				
'				DRIVE_CONTROLWORD(i)=128						'根据驱动器数据字典
'				wa 100
'				DRIVE_CONTROLWORD(i)=6
'				wa 100
'				DRIVE_CONTROLWORD(i)=15
'				wa 100
'			next
			
			for i=0 to MAX_AXISNUM - 1
				DRIVE_CLEAR(0)axis(i)
				delay(10)
			next
			
			?"驱动器错误清除完成"
			wa 100

			?"清除控制器错误"
			datum(0)
			'RIVE_CLEAR(0)
			
			?"控制器错误清除完成"
			wa 100
			wdog=1
			delay(30)				
			?"轴使能准备"
			for i= Bus_AxisStart to Bus_AxisStart + Bus_TotalAxisnum - 1
				base(i)
				AXIS_ENABLE=1
				delay(10)
				if i=0 then 
				     REG_INPUTS=0    '将R0-R3都对应输入口0
				endif
			next
													'使能总开关
			Bus_InitStatus  = 1
			?"轴使能完成"
			watch_sub()				
		else
			?"总线开启失败"
			Bus_InitStatus = 0
		endif
	
	else
		?"总线扫描失败"
		Bus_InitStatus = 0
	endif
end sub

'*********************************************鸣志电气PDO**********************************************
'部分特殊品牌可能需要手动配置，大部分只需要通过DRIVE_PROFILE设置自动配置相应的POD参数即可
'******************************************************************************************************
global sub Set_MZ_PDO(iNode)
	TRACE "鸣志 自定义pdo"
	SDO_WRITE (Bus_Slot, iNode, $1c12, 0 ,5 ,0)					'禁用PDO,禁用后才可以修改内容
	wa(50)
	SDO_WRITE (Bus_Slot, iNode, $1c13, 0 ,5 ,0)
	wa(50)
	
	SDO_WRITE (Bus_Slot, iNode, $1600, $0 ,5 ,0)					'RxPDO 配置对应参数
	SDO_WRITE (Bus_Slot, iNode, $1600, $1 ,7 ,$60400010)			'控制字
	SDO_WRITE (Bus_Slot, iNode, $1600, $2 ,7 ,$607a0020)			'目标位置
	SDO_WRITE (Bus_Slot, iNode, $1600, $3 ,7 ,$60fe0120)			'驱动器IO输出
	SDO_WRITE (Bus_Slot, iNode, $1600, $0 ,5 ,3)
	
	SDO_WRITE (Bus_Slot, iNode, $1a00, $0 ,5 ,0)					'TxPDO 
	SDO_WRITE (Bus_Slot, iNode, $1a00, $1 ,7 ,$60410010)			'状态字
	SDO_WRITE (Bus_Slot, iNode, $1a00, $2 ,7 ,$60640020)			'反馈位置
	SDO_WRITE (Bus_Slot, iNode, $1a00, $3 ,7 ,$60fd0020)			'驱动器IO输入
	SDO_WRITE (Bus_Slot, iNode, $1a00, $0 ,5 ,3)		
	
	SDO_WRITE (Bus_Slot, iNode, $1c12, 1 ,6 ,$1600)				'pdo分配对象			
	wa(50)
	SDO_WRITE (Bus_Slot, iNode, $1c13, 1 ,6 ,$1a00)				'
	wa(50)
	SDO_WRITE (Bus_Slot, iNode, $1c12, 0 ,5 ,1)					'启用PDO
	wa(50)
	SDO_WRITE (Bus_Slot, iNode, $1c13, 0 ,5 ,1)
	
	SDO_WRITE (Bus_Slot, iNode, $1C32, $1 ,6 ,2)					'设置DC同步模式
	SDO_WRITE (Bus_Slot, iNode, $1C33, $1 ,6 ,2)
	
	DRIVE_PROFILE = -1												'使用驱动缺省PDO配置		
end sub




global sub watch_sub()
	while 1	
		if g_cmd=1 then
				?"获得个cmd为1的信号，opNum taskNum startPos endPos mpos(0)分别为"opNum,taskNum,startPos,endPos,mpos(0)
				g_cmd=0
				stoptask taskNum
		        delay(1)
				runtask taskNum,run_sub(opNum,startPos,endPos)
				if taskNum<27 then
					taskNum=taskNum+1
					?"taskNum 加1"
				else 
					taskNum=2
					?"taskNum 重置为2"
				endif
		endif
		if restTaskMark=1 then 
			restTaskMark=0
		    for j=2 to 28
		       stoptask j
			next
		    ?"restTaskMark 重置为0"			
		endif	
	    if SCAN_EVENT(in(14))<0 then       '急停的输入io
            RAPIDSTOP(2)					'所有轴立即停止
		    ?"急停按钮已按下，所有轴立即停止"
		endif
	wend
end sub

global sub run_sub(op_Num,start_Pos,end_Pos)
    ?"启动"
	    wait until mpos(0)>start_Pos                                '大于第一个位置 op开
	    op(op_Num,on)
	    ?"op_Num 置为on  "op_Num,start_Pos,mpos(0),op(op_Num)
	    wait until mpos(0)>end_Pos                              '大于下一个位置 op关
        op(op_Num,off)
        ?"op_Num 置为 off  "op_Num,end_Pos,mpos(0),op(op_Num)	
end sub

'global sub run_sub2()
'    ?"启动"
'	for i=0 to 98
'	     wait until mpos(0)>g_data(i)                               '大于第一个位置 op开 10ms后关
'		 op(0,on)
'		 delay(10)
'		 op(0,off)
'    next
'end sub