'Example program smartmotor3.src  2008-06-05  Ralph Stirling
'I/O:
'	port A: output, READY signal to PLC
'	port B: input, TRIGGER from PLC
'	port C: input, HOME from sensor
'	port D: input, POS1 from PLC
'	port E: input, POS2 from PLC
'	port F: input, POS3 from PLC
'F E D
'0 0 0	home
'0 0 1	abs. move #1
'0 1 0	rel. move
'0 1 1	abs. move #2
'1 1 1	constant torque move, testing for broken wire

' Setup servo parameters
KP=400
KI=30
KD=200
F

' Setup input & outputs
UAO	' READY signal to PLC
UBI	' TRIGGER signal from PLC
UCI	' HOME signal from sensor
UDI	' POS1 signal from PLC
UEI	' POS2 signal from PLC
UFI	' POS3 signal from PLC

ZS	' clear status word, including limit bits

WHILE 1	' loop forever
	UA=1			' let PLC know we're ready
	WHILE UBI==0 LOOP	' wait for trigger from PLC

	d=UDI			'xx1 or xx0
	e=UEI*2			'x1x or x0x
	f=UFI*4			'1xx or 0xx
	g=d+e
	g=g+f			'all bits together

	' homing move?
	IF g==0
		UA=0		' tell PLC we are moving

		V=32212/4	' 1/4 rev/sec
		A=80		' 10 rev/sec^2

		D=2000		' 1 rev positive move
		G
		WHILE Bt
			IF UCI==0	' got home signal?
				O=0	' set origin
				S	' stop motion
				BREAK	' get out of the loop
			ENDIF
		LOOP

	' position #1 move?
	ELSEIF g==1

		UA=0		' not READY now

		V=1*32212	' 1 rev/sec (60 RPM)
		A=80		' 10 rev/sec^2

		P=500		' 90 degrees
		G
		TWAIT		' wait for done

	' relative move?
	ELSEIF g==2
		UA=0
		V=1*32212	' 1 rev/sec
		A=80

		D=10*2000	' 10 turns
		G
		TWAIT

	' constant torque move?
	ELSEIF g==7
		UA=0
		AMPS=500	' 50% max. current as safety
		MT		' torque mode
		T=200		' 20% torque, 200 max vel
		b=@P+20000	' current position + 10 turns
		G
		WHILE Bt
			IF @P>b		' finished all turns
				X	' slow down and stop
				BREAK
			ELSEIF @V>190	' almost max velocity
				S	' stop now, as wire broken
				BREAK
			ENDIF
		LOOP
		MP	' go back to position mode

	ENDIF

	WHILE UBI==1 LOOP	' be sure PLC saw our UA=0
	UA=1			' back to READY
LOOP				' loop back to start
END

