ECHO
'Univwersal Traverse and Takup teachable winding demo.
ADDR=1
EIGN(W,0)
'This program scans for traverse point switches physically
'and then their locaiotn becomes the traverse points in software.

'Set up paramters
'homing: (used in C100)
	vv=175000	'Home Speed
	aa=100		'Home Accel
	hh=1000		'Home offset
	ee=270		'Home error limit
	rr=-1		'Home direciton
	tt=200		'Home torque hold value
	a=1			'Motor address used in homing routine

'Paramterts for C500 universal traversing subroutine

'Hard coded traverse points: (after homing)
ll=1000		'low-side 	traverse point
hh=40000	'high-side 	traverse point

ttt=0	'=0 : use hard coded values above for traverse points 
		'=1	: use switches to find values for travers points	


ff=1		'Set to 1 for external encoder, 2 for virtual axis testing

lll=1000	'low-side 	lead-in distance
hhh=1000	'high-side	lead-in distance
uuu=1		'lead-in based off of 1 for slave, 0 for mater counts
 
ddd=1000	'dwell at each traverse point

mm=1000		'Gearing Multiplier
dd=2000		'Gearing Divisor
h=0	'homing not done yet, this will be set to 1 when homing is done
'Call C100 for homing
'Yu must home before traversing will work.
'Call C500 for traversing

END



C500	'universal subroutine for absolute traversing mode 
IF h!=1 RETURN ENDIF	'do not run unless homed first
IF ttt==1	'scan inputs 4 and 5 for traverse point locations
	MP PT=0 G TWAIT
	VT=vv ADT=aa G	'scan for traverse point switches
	WHILE IN(4)==1  LOOP
	lll=PA	'assign low point location
	WHILE IN(5)==1 LOOP
	hhh=PA	'assign high point location
	X TWAIT
ENDIF	
	SRC(ff)	'Master Encoder Source
	MFCTP(-1,0)	'Set direciton to start
	MFL(lll,uuu)'lead in on low side, distances to accend into MFR		
	MFH(hhh,uuu)'lead in on hi  side, distances to accend into MFR		
	MFLTP=ll	'Low Side traverse point
	MFHTP=hh	'High Side traverse point
	MFMUL=mm	'Gear Ratio Multiplier
	MFDIV=dd	'Gear Ratio Divisor	
	MFSDC(ddd,2)'Dwell at traverse point	
	MFR			'Set to Gear mode at pre-defined Gear Ratio
	G			'Go
RETURN
'================================
C100	'HOME any AXIS with address "a" from this local motor.
	IF a==4 | a==6 RETURN ENDIF
	ZS:a					'Clear any faults prior to homing
    kk=KP:a KP:a=1000 F:a
	k=KI:a KI:a=0 F:a		'Capture KI tuning gain value
	O:a=0					'SET ORIGIN TO ZERO TO CHECK FOR RACKING
	VT:a=vv*rr           	'Set speed 
	ADT:a=aa             	'set accel 
	MV:a 					'Set to Velocity Mode
	G:a           			'Go
	T:a=tt*rr				'Set Torque for holdng against hard stop
	MT:a					'Bufer in Torque Mode
	WHILE 1              	'while not stopped, loop
		qq=EA:a
		IF ABS(qq)>ee BREAK ENDIF 'Exit loop if ee exceeded
	LOOP
	MTB						'Dynamically brake
	G:a WAIT=50				'Hold in Torque Modefor 50 msec's
	o=hh*rr             	'calculate home position
	O:a=o               	'Set Origin to home offset
	MP:a PT:a=0 G:a   		'go to zero
	WHILE B(0,2):a==1 LOOP  'Wait for move to complete
	KI:a=k F:a				'Restore prior KI tuning gain value
    KP:a=kk F:a 
	h=1
RETURN