Option Explicit		' always require the declaration of varables before they are used - it's good programming practice for Basic 

' This is some sample code for a 6 turret tool changer
' revision history:
' rev 1:	initial code sample for an MSM customer
' rev 2:	removed use of UserDRO in favor of just using the turret angle to calculate the current tool position
' rev 3: 	Added logic to handle tool #0 ( = "no tool mounted")
' rev 4: 	removed activate and deactivate turret stop; changed rotation motions to be G91 to control rotation direction; 
'		added test for good position at start of tool change; changed to G5x WC sine can't do G91 in G53 with mach.
' rev 5:	reversed turet rotaion direction - see Turret Angle Pictures.ppt for direction pictures 
' Rev 6: 	added 360 rollover check for rotation values; reset WC each time turret is positioned to remove lost steps; fixed bug in G91 rotation amount calculation
' rev 7: 	fixed typo in string 
' rev 8: 	fixed bug in desired turrent angle case statement from reversing turret rotation direction; added update of MSM tool change msg line.
' rev 9: 	add mod op to MC To WC compare; added wait for ref operation to avoid timing holes
' rev 10: 	added mod op to current rotation optimization test
' rev 11:	fixed debug test in DebugMsg 
' rev 12:	Fixed numerous typos in comments (no code changes)

' The assumed hardware model is:
' To change tools, the turret is turned CW to just past the desired tool position rotation angle and then the turret is turned CCW into a stop.
' Turret is controlled as a rotary axis in mach, e.g. move 360 degrees = 1 full turn.
' note that negative rotation angles are CW and positive rotaiton angels are CCW
' 
' some assumption about the mach config setup:
'	in the mach general config dialog are some options for how a rotary axis is handled by mach
'	the options apply to all rotary axes - mach does not support different settings for different rotary axes
'
'	"Rot 360 rollover" - this should be ON. Whenever we go over 360 degrees, we want the current position to be modulo 360.
'	"ang Short Rot on G0" - this should be OFF. We always want mach going the direction we command and don't want mach to try to help 
'					by optimizing the physical travel direction!
'	"Rotational Soft Limits" - should be OFF. We don't want soft limits applied to the turret rotation angle.
'
' Note that the home position is set to 0 in machine coordinates by the mach homing routines.
' we do all tool change movements in machine (G53) coordinates as all turret postions are in "the turret's machine coordinates"
' however we have to do this in a bit of an odd manner.... since we want to control the direction of rotation of the turret, we want to use G91
' incremential rotations, however, mach will not allow G91 with G53... so we use the current WC instead of G53. We get away with this as 
' we check to see that G53=G5x position on entry to the tool chnage routine (if it is not we abort the tool change).

' ****************************
' this code  is intended to be called as part of the mach M6 sequence, and we are also writing this code for the MSM+Mach environment, so this will be the 
' M6ATC.m1s file and not M6Start etc.

' define configuration constants
Const Debug = True			' true => emit msgbox debug dialogs; false = run without making debug msgs
Const SimulateHome = False	' true => simulate Home aciton; false => call mach to do home action

Const TurretAxis = "A"		' axis letter for the turret axis
Const RefTurretBitValue = 8	' must be set to match axis letter
							' the value = 2^x where X is the axis number and mach numbers axes as 0-5 for x,y,z,a,b,c  
Const RotationExtra = 20	' value is in degrees. this is the amount that the turret should go past (CW) the desired position before 
							' reversing onto the stop. I picked this value from the logic of:
							' 1) we know that the tool positions are at every 60 degrees, so we wan to stay < 30 so that we stay on this side 
							' of the mid angle between positions.  So 20 seemed like a good number to use. 

Const MaxToolNumber = 6			' max tool number for the turret

' define some Mach magic DRO vlaues we use
Const MachCurrentToolNumberDRO		= 824
Const MachAAxisZeroOEMBtn 			= 1011	' OEM button number to zero A axis
Const MachResetButton				= 1021
Const TurretMCRotationAngle 		= 86	' A Axis Machine Coordinate OEM DRO 
Const TurretWCRotationAngle 		= 803	' A Axis Work Coordinate OEM DRO 
Const HomeRefAOEMBtn 				= 1025	' reference A axis button

' define variables
Dim OldToolNumber As Integer
Dim NewToolNumber As Integer
Dim RotationAngle As Double

' **************** this is the entry point into the routine *****************
Call Main()
Exit Sub

Sub Main()
	On Error GoTo TurretError		' set up an error trap for unexpected events
	DebugMsg("M6ATC Lathe called")
	Call PositionTurret()
	On Error GoTo 0					' cancel the error handler
	Exit Sub

TurretError:
	' unexpected error while positioning the turret
	MsgBox "Unexpected Turret error, resetting turret position"
	Call HomeTurret()	' let's get set back to the home position
	Exit Sub
End Sub

Sub PositionTurret()
	' this is the main routine
	
	Dim CurrentTurretPosition As Integer
	Dim CurrentTurretAngle As Double
	Dim DesiredTurretAngle As Double
	Dim G91Angle As Double
		
	MSMToolChangeStatusMsg("Positioning Turret...")
	' check that MC = WC for the turret
	If Gcn(GetOEMDRO(TurretMCRotationAngle) mod 360) <> Gcn(GetOEMDRO(TurretWCRotationAngle) mod 360) Then
		' whoops, there is some type of offset applied to the turret axis - abort tool change
		' we need WC=MC for the turret in order to do G91 moves in WC as a work around for G91 in G53 mode
		MsgBox "Turret MC <> WC error! " & Chr(13) & _
		"MC = " & Gcn(GetOEMDRO(TurretMCRotationAngle)) & Chr(13) & _
		"WC = " & Gcn(GetOEMDRO(TurretWCRotationAngle) & Chr(13) & _
		"Attempting to recover turret position")

		Call HomeTurret	' get us to a known zero angle home position
		CurrentTurretAngle = GetOEMDRO(TurretWCRotationAngle)	' get new position after home operation
	End If
		
	OldToolNumber = CInt(GetOEMDRO(MachCurrentToolNumberDRO))	' Cint conversion for safety incase DRO has a non-integer value in it
	' get next tool - the use of GetSeleccted Tool() could be dangerous as Mach has bugs that can cause that call no not always return a good value...
	NewToolnumber = GetSelectedTool()
	DebugMsg("Old Tool # = " & OldToolNumber & ", New Tool # = " & NewToolNumber)

	CurrentTurretAngle = GetOEMDRO(TurretWCRotationAngle)
	DebugMsg("Current Turret Angle = " & CurrentTurretAngle)
	If CurrentTurretAngle > 360 Then 
		DebugMsg("Warning CurrentTurretAngle > 360, check Mach 'Rot 360 Rollover' option setting")
		' code assumes the mach config option "rot 360 rollover" is set, so it shuold be impossoble for us to get a value > 360.
		' we'll see if we can recoiver by forcing the position back to a mod 360 value
		CurrentTurretAngle = CurrentTurretAngle Mod 360
		SetOEMDRO(TurretWCRotationAngle, CurrentTurretAngle)	' reset the Turret angle DRO to new value
		DebugMsg("CurrentTurretAngle reset to " & CurrentTurretAngle)
	End If
	
	' check that the angle of the turret matches a known tool position
	Select Case CurrentTurretAngle
	Case 0, 60, 120, 180, 240, 300, 360
		' these are Ok values, do nothing
	Case Else
		' we are not at a good tool position to start!
		DebugMsg("not at known tool position, homing turret")
		Call HomeTurret	' get us to a known position
		CurrentTurretAngle = GetOEMDRO(TurretWCRotationAngle)	' get new position after home operation
	End Select
		
	' ok, we need to move turret to the desired tool position, find desired position
	Select Case NewToolNumber
	Case 0 
		' mach wants to unmount all tools
		' ok- the question is what do we want to do in this case?
		' we do not have a tool turret position that is always empty, so we can't change to a postition that corresponds to "no tool mounted".
		' so we just exit, leaving the turret at whatever position it is already in.
		DebugMsg("Mouting tool #0, so doing nothing with turret")
		Exit Sub
	Case 1
		DesiredTurretAngle = 360
	Case 2
		DesiredTurretAngle = 60
	Case 3
		DesiredTurretAngle = 120
	Case 4
		DesiredTurretAngle = 180
	Case 5
		DesiredTurretAngle = 240
	Case 6
		DesiredTurretAngle = 300
	Case Else
		MsgBox "Out Of Range Tool# (" & NewToolNumber & ") requested!"
		DoOEMButton(MachResetButton)	' reset mach - continuing the gcode program when we can't mount the requested tool
										' could cause a machine crash.
		Exit Sub
	End Select
	DebugMsg("DesiredTurretAngle = " & DesiredTurretAngle)

	If DesiredTurretAngle mod 360 = CurrentTurretAngle mod 360 Then			        
		' we don't have to do anything, we are already positioned at the correct tool.
		' all is fine, exit without doing a turret action
		DebugMsg("No Turret rotation needed, already at desired position")
		MSMToolChangeStatusMsg("Turret in Position")
		Exit Sub
	End If
	
	If CurrentTurretAngle > DesiredTurretAngle Then
		G91Angle = ((360 - (CurrentTurretAngle - DesiredTurretAngle) ) Mod 360)  + RotationExtra
	Else
		G91Angle = ((DesiredTurretAngle - CurrentTurretAngle) Mod 360)  + RotationExtra
	End If
	DebugMsg("at angle " & CurrentTurretAngle & ", going to " & DesiredTurretAngle & ", G91Angle = " & G91Angle)
	
	DebugMsg("about to do G0 G91 " & TurretAxis & GCN(G91Angle) & " move")
	code "G0 G91" & TurretAxis & G91Angle
	
	Call MotionWait()
	
	' note the following assumes that reversing to exactly the sesired position is Good enough
	' if we need to reverse (CCW) further and let the stepper loose steps against the stop, then the entire routine will need more work
	' thi is becuase the lost steps of the stepper will cause mach to think the axis is at a different angle than it actually is, and that will change the 
	' angle we get from reading the MC axis value etc.... If that complexity is needed, we would have to change the approach assumed in this routine.
	DebugMsg("about to do G0 G91 " & TurretAxis & GCN(-1*RotationExtra)) & " move" 
	Code "G0 G91 " & TurretAxis & GCN(-1*RotationExtra)
	Call MotionWait()
	
	' we are now at position and physically on the stop.
	' OK, getting on to the pawl, probably caused the machine to loose steps, so the actual angle position of the turret is not the 
	' same angle as where mach thinks the turret is positioned - it is off by the amount of lost steps.
	' so we reset the WC rotation value to be the desired angle, this removes the lost steps position error.
	SetOEMDRO(TurretWCRotationAngle, DesiredTurretAngle)
	DebugMsg("Set Turret WC angle to " & DesiredTurretAngle)
	
	' If this were an M6 script, we would do the next line, but we have commented it out as this is for use with MSM and MSM does this for us 
	' as part of the MSM M6 logic.
	'SetCurrentTool(NewToolNumber)
	
	' we are all done with the tool change, exit 
	DebugMsg("tool Change finished OK")
	MSMToolChangeStatusMsg("Turret in Position")
	Exit Sub	
End Sub



Sub HomeTurret()
	' small routine to home the turret to tool position #1
	'	we can't use a general input signal and try to look at it from a mach script - 
	'	the scripts are much to slow to see the electrical home signal change. This is why mach has built in homing routines. 
	'	we let mach do the home oepration and return to us when the turret is at the home position
	'
	' The turret has a steppermotor and an optical slot that goes active when the turret is at the home postion (0 degrees)
	'	electrically, this input signal must be configured as the rotary Axis home switch input.
	' 	we need the home position to be exactly when the Home signal goes active as we want that position to be 0 degrees
	'	therefore, we have to configure the mach homing options as follows:
	'		Home Ofset = 0		we do not want the 0 degree position shifted from the input signal active position.
	'		Auto Zero = On (checked) 	we want mach to set the axis 0 location in machine coords to 0 when the turret is homed
	
	MSMToolChangeStatusMsg("Homing Turret")
	DebugMsg("about to reference Turret to home position")
	If SimulateHome Then	' just a little debug code for testing routine on a PC that does not have the turret....
		' this lets us fake it - we just set the machine coord axis back to 0 as if we had homed the machine
		DebugMsg("doing debug simulation of turret home actions")
		Code "G90 G0 G53 A360"
		Call MotionWait()
		DoOEMBUtton(MachAAxisZeroOEMBtn)
		
	Else
		DebugMsg("about to ask mach to home Turret")
		DoOEMButton( HomeRefAOEMBtn )
		Call MachRefWait()
		
		' OK, we are homed and therefore we are at turret angle = 0 degrees
		
		' move past the home position as if we had just rotated to here
		DebugMsg("about to move past home position by RotationExtra amount")
		code "G0 G90 G53 " & TurretAxis & 0 + RotationExtra	
		Call MotionWait()

		DebugMsg("about to rotate back on to pawl")
		Code "G0 G53 " & TurretAxis & 0
		Call MotionWait()
		Code "G90"
		
		' reset the WC offset to = the MC now that we are homed
		SetOEMDRO(TurretWCRotationAngle, 0)
		DebugMSg("reset Turret WC to 0")
		
	End If
	MSMToolChangeStatusMsg("Turret Homed")
	' we are now at 0 degrees, physically on the stop.
	Exit Sub
End Sub

	
Function GCN(ByVal VBNum) As String
	' GCodeNumber: a small util to take a VB double, format it and return it as a string
	' this is needed so that all numbers put into strings that will be sent to Code will not 
	' have things of the form: X0.123456e-9   - which is not legal gcode format
	GCN = Format(VBNum, "##0.")		' 6 sig digits to right of decimal point should be enough 
	Exit Sub
End Function

Sub MotionWait
	' just a little utility routine to wait for moiton to complete
	While IsMoving()
		Sleep 50		' to keep from locking up the CPU in a hard wait on earlier versions of mach.
	Wend
	Exit Sub
End Sub

Sub DebugMsg(ByRef S As String)
	' a utility routine to conditionally put out debug messages....
	' you could add writing a msg to a file etc here - this just does a simple msgbox display
	if Debug then 
		MsgBox "Turret: " & s
	end if
	Exit Sub
End Sub

sub MSMToolChangeStatusMsg(ByVal S as String)
	Const MSMToolChangeUserLabel			= 171	' an internal MSM Userlabel number, this # could change in the future... 
													' UserLabel # OK as of MSM 2.0.0

	SetUserLabel(MSMToolChangeUserLabel, S)
	exit sub
end sub

Sub MachRefWait()
	' ok the following is a mach hack....
	' (relevant to mach 3.x.x)
	'
	' it turns out that Mach reference operations do not wait for the action to be complete before returning to the calling script. 
	' Yet, the calls are commanly used as if they are blocking calls (even though they turn out not to be) - 1024.set also uses them as if they are blocking
	' Add to this the fact that there is no built in API to wait in a script for a reference operation to be complete...
	' and we have a timing hole that the script can fall into.
	'
	' But we can trick mach by inserting a short dwell into the Gcode queue, 
	' the dwell will show up internally to mach as a gcode movement that the IsMoving() API ***will*** wait on,
	' AND (this is key) mach will ***not*** start executing the dwell command until the reference op is done.... 
	' thus we can force a wait until the ref (and dwell) operation is completed.
	'
	' The dwell time amount is not important.  I used P1 for either 1ms or 1 second - the time unit depends on the mach config setting.
	' Either way the added delay will not be noticed as part of the overall homing sequence time. 
	Code("G4 P1")
	While IsMoving() 
		sleep 10
	wend
	exit sub
end sub