''========================================================================================
'  Created From: C:\Perl\site\lib\MyMach3Macros\Probing\ZMinusEdgeFind.m3m
'  Includes: 
'       C:\Perl\site\lib\MyMach3Macros\Common.def
'       C:\Perl\site\lib\MyMach3Macros\Probing\Probing.def
''========================================================================================

''========================================================================================
''	 Included Definitions:
''========================================================================================
AAxis =                         3       ' Symbol for argument passing
CenterFindLED1 =		1115	' Center Finder Macro Mapped to Shuttle Pro Macro1
LFCornerFindLED1 = 		1105	' LF Corner Finder Macro Mapped to Shuttle Pro Macro1
LFViseAlignLED1 = 		1109	' LF Vise Align Macro Mapped to Shuttle Pro Macro1
LRCornerFindLED1 = 		1107	' LR Corner Finder Macro Mapped to Shuttle Pro Macro1
LRViseAlignLED1 = 		1111	' LR Vise Align Macro Mapped to Shuttle Pro Macro1
MachAbsoluteModeLED = 		48	' Absolute Coordinate Mode LED
MachFeedrateDRO = 		818	' Current Feedrate DRO
MachProbeLED = 			825	' OEM code for probe input LED
MachXAxisMultiModeOEMDRO = 	800	' X Axis Multi-Mode OEM DRO 
MachXAxisOEMDRO = 		178	' X Axis OEM DRO 
MachXTouchVar = 		2000	' OEM code for Var that holds X touch position
Minus =                         -1      ' Symbol for argument passing
Plus =                          1       ' Symbol for argument passing
Prim_EdgeFind_NoEdgeFound =     2
Prim_EdgeFind_ProbeActive =     1
Prim_EdgeFind_Success =         0
ProbeBackOffDistanceDRO =	1101	' Back-off distance, after probing in X/Y
ProbeDiameterDRO =		1000	' Touch probe diameter
ProbeDistanceDRO =		1102	' Max. Distance to move when probing
ProbeFeedRateDRO = 		1103	' Feedrate to use when probing
ProbeSlowBackoffDistance =      0.025   ' Back off distance before doing slow probe
ProbeSlowFeed =                 1.0     ' Feedrate for slow probe
ProbeSlowMargin =               0.025   ' Overshoot allowance for slow probe
ProbeZAxisFeed =                5.0     ' Feedrate used only for probing Z axis
RFCornerFindLED1 = 		1106	' RF Corner Finder Macro Mapped to Shuttle Pro Macro1
RFViseAlignLED1 = 		1110	' RF Vise Align Macro Mapped to Shuttle Pro Macro1
RRCornerFindLED1 = 		1108	' RR Corner Finder Macro Mapped to Shuttle Pro Macro1
RRViseAlignLED1 = 		1112	' RR Vise Align Macro Mapped to Shuttle Pro Macro1
Success =                       0       ' Success Return Value
TouchPlateThicknessDRO = 	1001	' Z Touch plate thickness
ViseWidthDRO =			1100	' Vise width, for vise alignment macros
XAxis =                         0       ' Symbol for argument passing
XMidPointFindLED1 = 		1113	' X Axis Mid-Point Finder Macro Mapped to Shuttle Pro Macro1
XMinusEdgeFindLED1 = 		1101	' X- Edge Finder Macro Mapped to Shuttle Pro Macro1
XPlusEdgeFindLED1 = 		1100	' X+ Edge Finder Macro Mapped to Shuttle Pro Macro1
YMidPointFindLED1 = 		1114	' Y Axis Mid-Point Finder Macro Mapped to Shuttle Pro Macro1
YMinusEdgeFindLED1 = 		1103	' Y- Edge Finder Macro Mapped to Shuttle Pro Macro1
YPlusEdgeFindLED1 = 		1102	' Y+ Edge Finder Macro Mapped to Shuttle Pro Macro1
ZAxis =                         2       ' Symbol for argument passing
ZMinusEdgeFindLED1 = 		1104	' Z- Edge Find/Tool Length Setting Macro Mapped to Shuttle Pro Macro1
ZProbeBackOffDistanceDRO = 	1104	' Back-off distance, after probing in Z
ZeroDROOnProbeLED =             1201    ' Zero DRO on successful probe operation when "lit"
''========================================================================================

''
'   Button-Callable Z Axis Minus Edge Finder Macro
''

'#filename "M954.m1s"
'#include "Common.def"
'#include "Probing.def"

''========================================================================================
''	Start of Macro Code
''========================================================================================

    ' Set our LED
    SetUSerLED(XPlusEdgeFindLED1, 0)
    SetUSerLED(XMinusEdgeFindLED1, 0)
    SetUSerLED(YPlusEdgeFindLED1, 0)
    SetUSerLED(YMinusEdgeFindLED1, 0)
    SetUSerLED(ZMinusEdgeFindLED1, 1)
    SetUSerLED(LFCornerFindLED1, 0)
    SetUSerLED(RFCornerFindLED1, 0)
    SetUSerLED(LRCornerFindLED1, 0)
    SetUSerLED(RRCornerFindLED1, 0)
    SetUSerLED(LFViseAlignLED1, 0)
    SetUSerLED(RFViseAlignLED1, 0)
    SetUSerLED(LRViseAlignLED1, 0)
    SetUSerLED(RRViseAlignLED1, 0)
    SetUSerLED(XMidPointFindLED1, 0)
    SetUSerLED(YMidPointFindLED1, 0)
    SetUSerLED(CenterFindLED1, 0)

    ZeroDRO = GetUserLED(ZeroDROOnProbeLED)
    ReturnVal = Prim_EdgeFind (ZAxis, Minus, ZeroDRO)
    
    ShowProbeResult (ReturnVal)
''========================================================================================
''	 Included Subroutines:
''========================================================================================
Sub DoProbe (ByVal Command As String)
    Code "G31 " & Command
    WaitUntilStopped
End Sub

Sub DoRapid (ByVal Command As String)
    Code "G0 " & Command
    WaitUntilStopped
End Sub

Sub GetProbeParameters ( _
    ProbeRadius As Double, _
    ProbeDistance As Double, _
    ProbeFeed As Double, _
    BackoffDistance As Double, _
    ZTouchPlateThickness As Double, _
    ZProbeFeed As Double, _
    ZBackoffDistance As Double, _
    ViseWidth As Double _
    )
    
    ''========================================================================================
    ''      This subroutine gets the parameters that control operation of the probing macros.
    ''      By default, these are all obtained mostly from on-screen DROs, but they can be
    ''      hard-wired by simply modifying this one subroutine.
    ''
    ''      ProbeRadius             = Radius of probe tip for X/Y
    ''      ProbeDistance           = Maximum distance to probe for all axes
    ''      ProbeFeed As Double     = Feedrate for fast probing in X/Y
    ''      BackoffDistance         = Distance to back-off from touch point for X/Y
    ''      ZTouchPlateThickness    = Thickness of touchplate for Z/A
    ''      ZProbeFeed As Double    = Feedrate for fast probing in Z/A
    ''      ZBackoffDistance        = Distance to back-off from touch point for Z/A
    ''      ViseWidth               = Width of vise jaw, for vise alignment
    ''========================================================================================

    ' Radius of touch probe
    ProbeRadius = GetUserDRO(ProbeDiameterDRO)/2
    ' Maximum distance to probe
    ProbeDistance = GetUserDRO(ProbeDistanceDRO)
    ' Feedrate to use for probing
    ProbeFeed = GetUserDRO(ProbeFeedRateDRO)
    ' Distance to back off when done
    BackoffDistance = GetUserDRO(ProbeBackOffDistanceDRO)
    ' Thickness of Z/A axis touchplate
    ZTouchPlateThickness = GetUserDRO(TouchPlateThicknessDRO)
    ' Feedrate to use for probing Z/A only
    ZProbeFeed = ProbeZAxisFeed
    ' Distance to back off when done probing Z/A only
    ZBackoffDistance = GetUserDRO(ZProbeBackOffDistanceDRO)
    ' Vise jaw width for vise alignment
    ViseWidth = GetUserDRO(ViseWidthDRO)                    
End Sub

Sub WaitUntilStopped
    ' Wait until machine is stopped
    While IsMoving ()
        Sleep 10
    Wend
End Sub
''========================================================================================

''========================================================================================
''	 Included Functions:
''========================================================================================
Function Prim_EdgeFind ( _
    ByVal ProbeAxis As Integer, _
    ByVal ProbeDirection As Integer, _
    ByVal ZeroDRO As Integer _
    ) As Integer

    ' Get some probing parameters        
    Dim ProbeRadius As Double
    Dim ProbeDistance As Double
    Dim ProbeFeed As Double
    Dim BackoffDistance As Double
    Dim ZTouchPlateThickness As Double
    Dim ZProbeFeed As Double
    Dim ZBackoffDistance As Double
    Dim ViseWidth As Double

    GetProbeParameters ProbeRadius, ProbeDistance, ProbeFeed, BackoffDistance, _
                       ZTouchPlateThickness, ZProbeFeed, ZBackoffDistance, ViseWidth
    
    ' Assume not doing Z or A axis
    TouchPlateThickness = 0                             
    
    ' Fudge some things if this is Z axis
    If (ProbeAxis = ZAxis) OR (ProbeAxis = AAxis) Then
        ' Probe radius is zero when doing Z/A
        ProbeRadius = 0
        ' Z/A touchplate thickness replaces ProbeRadius
        TouchPlateThickness = ZTouchPlateThickness
        ' Feedrate to use for probing Z/A only
        ProbeFeed = ZProbeFeed
        ' Distance to back off when done
        BackoffDistance = ZBackOffDistance              
    End If

    ' Define some constants
    If ProbeAxis < AAxis Then
        ' Axis to probe
        AxisName = Chr(Asc("X") + ProbeAxis) 		
    Else
        ' Axis to probe
        AxisName = Chr(Asc("A") + ProbeAxis - 3)
    End If
    ' Axis DRO for reading axis position (this one always gives work position)
    AxisReadDRO = MachXAxisOEMDRO + ProbeAxis
    ' Axis DRO for writing (this one always write work position)
    AxisWriteDRO = MachXAxisMultiModeOEMDRO + ProbeAxis
    ' Touch point Var
    TouchVar = MachXTouchVar + ProbeAxis
    
    If ProbeDirection = Plus Then
        Direction = "+"
    Else
        Direction = "-"
    End If

    ' Do nothing if probe is already grounded
    If (GetOemLed(MachProbeLED) = 0) Then
    
        ' Save current feedrate
        CurrentFeed = GetOemDRO(MachFeedrateDRO)

        ' Save current coordinate mode
        AbsMode = GetOemLED(MachAbsoluteModeLED)

        ' Set absolute coordinate mode
        Code "G90"

        ' Show what we're doing
        Message "Probing " & AxisName & Direction & " At Fast Speed..."

        ' Set target of probe move
        StartPos = GetOEMDRO(AxisReadDRO)
        ProbeTarget = StartPos + (ProbeDistance * ProbeDirection)

        ' Do the touch move at specified feedrate
        DoProbe AxisName & ProbeTarget & " F" & ProbeFeed

        ' Get the position where probe input triggered
        TouchPos = GetVar(TouchVar)
            
        ' Did we find an edge, or run out of room?
        If Abs(TouchPos - ProbeTarget) < 0.001 Then
    
            ' Ran out of room, so return to start position
            DoRapid AxisName & StartPos
                
            ' Tell user we failed
            Prim_EdgeFind = Prim_EdgeFind_NoEdgeFound

        Else
            ' Show what we're doing
            Message "Probing " & AxisName & Direction & " At Slow Speed..."
            
            ' Go back to touch position, plus a bit more
            DoRapid AxisName & (TouchPos - (ProbeSlowBackoffDistance * ProbeDirection))

            ' Set target of probe move
            ProbeTarget = (TouchPos + (ProbeSlowMargin * ProbeDirection))
            
            ' Repeat the touch move at slow feedrate, for better accuracy
            DoProbe AxisName & ProbeTarget & " F" & ProbeSlowFeed
                    
            ' Get the position where probe input triggered
            TouchPos = GetVar(TouchVar)
            
            ' Did we find an edge, or run out of room?
            If Abs(TouchPos - ProbeTarget) < 0.001 Then
        
                ' Ran out of room, so return to start position
                DoRapid AxisName & StartPos
                    
                ' Tell user we failed
                Prim_EdgeFind = Prim_EdgeFind_NoEdgeFound
                
            Else                
                
                ' Go back to touch position
                DoRapid AxisName & TouchPos
    
                If ZeroDRO > 0 Then
                    StartPos = 0 - ((ProbeRadius + TouchPlateThickness) * ProbeDirection)
                    SetOEMDRO(AxisWriteDRO, StartPos)
                Else
                    StartPos = TouchPos
                End If
    
                ' Now back off
                DoRapid AxisName & (StartPos - (BackoffDistance * ProbeDirection))
    
                ' Return success
                Prim_EdgeFind = Prim_EdgeFind_Success

            End If
            
        End If

        ' Reset feedrate to original value
        Code "F" & CurrentFeed

        ' Reset coordinate mode to original value
        If AbsMode = 0 Then
            Code "G91"
        End If
    
    Else

        ' Tell user what happened
        Prim_EdgeFind = Prim_EdgeFind_ProbeActive

    End If 

    ' Clear the message line
    Message ""

End Function

Function ShowProbeResult (ByVal Result As Integer) As Integer
    If Result = Prim_EdgeFind_ProbeActive Then
        Message "Probe is stuck active, check connection and try again"
    ElseIf Result = Prim_EdgeFind_NoEdgeFound Then
        Message "No Edge Found"
    ElseIf Result = Prim_EdgeFind_Success Then
        Message "Probe Successful!"
    End If
    ShowProbeResult = Result
End Function
''========================================================================================