'-------------- Device Params -----------------------
Params
	DRV.OPMODE = 2 'position operation mode
	DRV.CMDSOURCE = 5 'command source = AKD BASIC TG
	UNIT.PROTARY = 3 '16 bit position units, 65536 counts/rev
	UNIT.VROTARY = 0 'velocity units = rpm
	UNIT.ACCROTARY = 0 'acceleration units = rpm/sec
	UNIT.PIN = 10000
	UNIT.POUT = 1
	PL.MODPEN = 1'Enable slave modulo
	PL.MODP2 = 10000'Set slave modulo value
	FB2.ENCRES = 8000 'set slave gearing input resolution
	EXTENCODER.POSMODULO = 8000 ' Set master modulo value
End Params

'-------------- Define (dim) Global Variables --------
Dim homefound as integer
Dim targetposition as integer
Dim speed as integer 'speed of initial move to sprayposition
Dim accel as integer 'acceleration for all moves
Dim decel as integer 'deceleration for all moves
dim m, s, nextpos as float 
dim i as integer 
const npoints = 21

MBInfo
End
'-------------- Main Program -------------------------
Main
DOUT2.STATEU = 0
DRV.SWENABLE = 0

'Enable Interrupts
INTR.HWLS.POSSTATE = 1
INTR.HWLS.NEGSTATE = 1 'used for homing if home input not reached first
Intr.DRV.FAULTS = 1
'INTR.DISABLE = 1
INTR.DRV.HWENABLE = 1

'move to absolute zero
Retry1:
While DRV.HWENABLE = 0  : Wend 'wait for hardware enable
DRV.SWENABLE = 1 'enable drive
Move.Acc        = 1000  ' Acceleration (drive units)
Move.Dec        = 1000  ' Deceleration (drive units)
Move.RunSpeed   = 500  ' Runspeed (drive units)
Move.TargetPos  = 0   ' Target position (drive units)
Move.GoAbs
While MOVE.MOVING = 1 : Wend 'wait for motion to stop
If PL.FB < -10 or PL.FB > 10 Then
	GoTo Retry1
End If
'MOVE.POSCOMMAND = 0 'set slave (AKD BASIC) position to 0

DRV.SWENABLE = 0

'find the EEO index pulse to start learning data points
CAP1.MODE = 0 'standard position capture
CAP1.TRIGGER = 9 'Use input 4 for position capture
CAP1.EDGE = 1 'positive edge
CAP1.EVENT = 0 'no pre-conditions must be met
CAP1.EN = 1 'Start looking for the capture trigger
Print "waiting for cap1 - learning mode"
While CAP1.STATE = 0 : Wend 'wait for the capture to trigger
EXTENCODER.POSITION = 0 'set Master position to 0
DOUT2.STATEU = 1 'turn on output 2 to indicate ready to learn positions
DOUT2.STATEU = 0
DOUT2.STATEU = 1
DOUT2.STATEU = 0
DOUT2.STATEU = 1
DOUT2.STATEU = 0
CAM.CREATE (2, npoints)
	CAM.ADDPOINT(0,0)
	nextpos = 0
	for i = 1 to npoints-2
		nextpos = nextpos + 8000/(npoints-1)
		while EXTENCODER.POSITION < nextpos :Wend 'wait for nextpos increment
		'nextpos = EXTENCODER.POSITION
		'm = EXTENCODER.POSITION 'master encoder position
		DOUT2.STATEU = 1
		s = PL.FB 'learn current slave position
		DOUT2.STATEU = 0
		CAM.ADDPOINT(nextpos,s)
		Print i
		Print nextpos
		Print s
	next i
	Cam.AddPoint(8000,10000)
end CAM.CREATE
Print "done"
DOUT2.STATEU = 0 'turn off output 2 to indicate done learning
CAM.MASTER = 2 'Cam Master = External Encoder

DRV.SWENABLE = 1 'enable the motor
Print "Motor Enabled, wait 2 sec" 
Pause (2)

'move to absolute zero
Retry2:
While DRV.HWENABLE = 0  : Wend 'wait for hardware enable
DRV.SWENABLE = 1 'enable drive
Move.Acc        = 1000  ' Acceleration (drive units)
Move.Dec        = 1000  ' Deceleration (drive units)
Move.RunSpeed   = 500  ' Runspeed (drive units)
Move.TargetPos  = 0   ' Target position (drive units)
Move.GoAbs
While MOVE.MOVING = 1 : Wend 'wait for motion to stop
If PL.FB < -10 or PL.FB > 10 Then
	GoTo Retry2
End If


'Wait to start the cam until seeing the EEO index pulse
CAP1.MODE = 0 'standard position capture
CAP1.TRIGGER = 9 'Use input 4 for position capture
CAP1.EDGE = 1 'positive edge
CAP1.EVENT = 0 'no pre-conditions must be met
CAP1.EN = 1 'Start looking for the capture trigger
Print "waiting for cap1"
While CAP1.STATE = 0 : Wend 'wait for the capture to trigger
EXTENCODER.POSITION = 0 'set Master position to 0
CAM.ACTIVATE = 2 'activate cam #1
Print "CAM Enabled" 

While 1 = 1 : wend 'loops continuously

End Main

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

'-------------- Interrupt Routines -------------------
Interrupt HWLS.NEGSTATE
	If homefound = 0 Then 'move to the positive side of the home switch
		While MOVE.MOVING = 1 : Wend 'wait to stop moving
		MOVE.DIR = 0 'positive direction
		MOVE.RUNSPEED = 50 'mm/s
		MOVE.GOVEL 'start moving
		While DIN4.STATE = 0 : Wend 'wait to get on the home switch
		While DIN4.STATE = 1 : Wend 'find positive edge of home switch
		Pause (0.5)
		MOVE.RUNSPEED = 0
		MOVE.GOVEL 'stop moving, zero speed
		While MOVE.MOVING = 1 : Wend 'wait to stop moving
		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
		Print "restarting neg EOT"
		Restart
	End If
    INTR.HWLS.NEGSTATE = 1
End Interrupt

Interrupt HWLS.POSSTATE
	Print "restarting pos EOT"
	Restart
End Interrupt

Interrupt DRV.HWENABLE
	Print "restarting HW en"
	Restart
    Intr.DRV.HWENABLE = 1
End Interrupt

'Interrupt disable
'	Print "restarting"
'	Restart
'	INTR.DISABLE = 1
'End Interrupt

Interrupt DRV.FAULTS
	Print "restarting fault"
	Restart
End Interrupt