'-------------- Device Params -----------------------
Params
CS.DEC = 300
CS.VTHRESH = 10
End Params

'-------------- Define (dim) Global Variables --------

Dim indexdistance as integer
Dim indexspeed as integer
Dim jogspeed as integer
Dim starthome as integer
Dim startmove as integer
Dim moveselection as integer
Dim startmoveselected as integer
Dim homefound as integer
Dim startjogpos as integer
Dim startjogneg as integer

MBInfo
$MBMap32(5000, indexdistance) 'Modbus registers 5000 and 5001 for indexdistance from HMI
$MBMap32(5002, indexspeed) 'Modbus registers 5002 and 5003 for indexspeed from HMI
$MBMap32(5004, jogspeed) 'Modbus registers 5004 and 5005 for jogspeed from HMI
$MBMap32(5006, starthome) 'Modbus register 5006 for starthome button from HMI
$MBMap32(5008, startmove) 'Modbus register 5007 for startmove button from HMI
$MBMap32(5010, moveselection) 'Modbus register 5008 for startmove button from HMI
$MBMap32(5012, startmoveselected) 'Modbus register 5009 for startmove button from HMI
$MBMap32(5014, homefound) 'Modbus register 5010 for homestatus indicator in HMI
$MBMap32(5016, startjogpos) 'Modbus register 5011 for startjogpos button from HMI
$MBMap32(5018, startjogneg) 'Modbus register 5012 for startjogneg button from HMI
End
'-------------- Main Program -------------------------
'Home switch on Input 2
Main
DRV.SWENABLE = 0
starthome = 0
homefound = 0
startmove = 0
startjogpos = 0
startjogneg = 0
startmoveselected = 1
indexdistance = 360
indexspeed = 60
jogspeed = 60
Move.Acc = 1000  ' Acceleration (drive units)
Move.Dec = 1000  ' Deceleration (drive units)

'Enable Interrupts
INTR.HWLS.POSSTATE = 1
INTR.HWLS.NEGSTATE = 1
Intr.DRV.FAULTS = 1
INTR.DRV.HWENABLE = 1

DRV.SWENABLE = 1
While DRV.ACTIVE = 0 : Wend 'wait for hardware and software enable

While 1 = 1 'start continuous loop
Print "Start Loop"

'Homing uses Input 2 for the position capture, and uses the negative limit switch (interrupt) in the homing routine.
If homefound = 0 Then 'If axis has not been homed, then wait for starthome button
	While starthome = 0 : Wend 'starthome HMI button
	CAP0.MODE = 0 'standard position capture
	CAP0.TRIGGER = 1 'Use input 2 for position capture
	CAP0.EDGE = 1 'positive edge
	CAP0.EVENT = 0 'no pre-conditions must be met
	CAP0.EN = 1 'Start looking for the capture trigger
	MOVE.RUNSPEED = 60 'deg/s
	MOVE.DIR = 1 'negative direction
	MOVE.GOVEL 'start moving
	While CAP0.STATE = 0 : Wend 'wait for the capture to trigger
	Print "cap"
	MOVE.RUNSPEED = 0
	MOVE.GOVEL 'stop moving, zero speed
	While MOVE.MOVING = 1 : Wend 'wait for motion to stop
	MOVE.POSCOMMAND = (PL.FB -CAP0.PLFB ) 'set position to the difference between the actual position and captured position
	Pause (.2)
	MOVE.RUNSPEED = 10 'deg/s
	MOVE.TARGETPOS = 0
	MOVE.GOABS 'move back to the captured position
	Pause (1) 'this is the end of the homing routine
	homefound = 1 'set homefound tag
	startmove = 0 'reset startmove tag in case the button was pressed prior to homing
End If

While DRV.HWENABLE = 0  : Wend

'Indexing Move
If startmove = 1 Then 'start "Cut to Length" indexing move
startmove = 0 'reset startmove tag
MOVE.ABORT
Move.RunSpeed = indexspeed 'index speed in integer position units per sec (from HMI)
MOVE.RELATIVEDIST = indexdistance 'index distance in integer position units (from HMI)
Move.GoRel
End If
While MOVE.MOVING = 1 : Wend

If startmoveselected = 1 Then
MOVE.ABORT
'Predefined Move Selection
If moveselection = 1 Then
Move.RunSpeed = 3600 'deg/s
Move.TargetPos = 0
Move.GoAbs
ElseIf moveselection = 2 Then
Move.RunSpeed = 360 'deg/s
MOVE.RELATIVEDIST = 90 'degrees
MOVE.GOREL
ElseIf moveselection = 3 Then
Move.RunSpeed = 360 'deg/s
MOVE.RELATIVEDIST = 180 'degrees
MOVE.GOREL
ElseIf moveselection = 4 Then
Move.RunSpeed = 360 'deg/s
MOVE.RELATIVEDIST = 270 'degrees
MOVE.GOREL
ElseIf moveselection = 5 Then
Move.RunSpeed = 360 'deg/s
MOVE.RELATIVEDIST = 360 'degrees
MOVE.GOREL
Else
MOVE.ABORT
End If
startmoveselected = 0
End If
While MOVE.MOVING = 1 : Wend

If startjogpos = 1 Then
MOVE.ABORT
Print "starting Jog"
startjogpos = 0 'reset tag
Move.RunSpeed = jogspeed 'from HMI
Move.Dir = 0 'positive
Move.Govel
Print "Jogging pos"
ElseIf startjogneg = 1 Then
MOVE.ABORT
Print "starting Jog"
startjogneg = 0 'reset tag
Move.RunSpeed = jogspeed 'from HMI
Move.Dir = 1 'negative
Move.Govel
Print "Jogging neg"
End If

Wend 'return to "While 1 = 1" loop
End Main

'-------------- Subroutines and Functions ------------

'-------------- Interrupt Routines -------------------
Interrupt HWLS.NEGSTATE
	If homefound = 0 Then 'look for home in the other direction
	While MOVE.MOVING = 1 : Wend
		MOVE.DIR = 0 'positive direction
		MOVE.RUNSPEED = 50 'mm/s
		MOVE.GOVEL 'start moving
		While DIN2.STATE = 0 : Wend 'wait for home switch to turn on
		While DIN2.STATE = 1 : Wend 'wait for home switch to turn off (pass the home switch)
		Pause (0.5)
		MOVE.RUNSPEED = 0
		MOVE.GOVEL 'stop moving, zero speed
		While MOVE.MOVING = 1 : Wend
		CAP0.EN = 1 'Start looking for the capture trigger
		MOVE.DIR = 1 'negative direction
		MOVE.RUNSPEED = 50 'mm/s
		MOVE.GOVEL 'start moving
	ElseIf homefound = 1 Then
		Restart
	End If
    INTR.HWLS.NEGSTATE = 1
End Interrupt

Interrupt HWLS.POSSTATE
	Restart
End Interrupt

Interrupt DRV.HWENABLE
	Restart
End Interrupt

Interrupt DRV.FAULTS
	Restart
End Interrupt