' 2010 Rectangular Pocket Probing Script
' 3-1-11
' 8-28-2011 External round or square probe code.  
'           Eric Sanders.
'           Based upon original internal probing code by .....
' 11/18/17 - Remove GetVar
Dim FRate1, FRate2, ProbingFeedRate
Dim DMax, Clearance, EdgeLength
Dim XStart, YStart, ZStart
Dim PlateOffset, XOffset, YOffset
Dim ZeroYN
Dim ToolNo
Dim ToolD
Dim ProbeD, ProbeDOffset
Dim ProbeLengthX, ProbeLengthY
Dim XHit, X1, X2
Dim YHit, Y1, Y2
Dim XPosCenter, YPosCenter, XCenter, YCenter
Dim XScale, YScale, ZScale
Dim CurrentFeed,CurrentAbsInc,CurrentGmode
Dim SafeZHeight
Dim DirText, WaitTime

Sub Main()
   WaitTime = 100' Pause time for moves
   Call GetUserProbingInputs() 'Set our user defined variables
   Call GetCurrentMachineState() 'Save our current machine state
   Call CheckForErrors() 'Check for errors.

'Set All Axis' Scale to 1
   Call SetOEMDRO(59,1)
   Call SetOEMDRO(60,1)
   Call SetOEMDRO(61,1)
   Sleep(250)

   Code "G90" ' Go to absolute mode
   Sleep(100)


   
'=======================================
'Positive Y Probing
'=======================================
   ProbeLengthY = YStart + DMax	
   ProbeDOffset = -1 * ProbeD/4
   DirText = "Forward"
   Call ProbeYAxis()
   Y1 = YHit + ProbeD/2  'Tool position at probe hit.
   Code "G0 Y" & (YHit - ProbeD/4)
   Call WaitForMove()

'=======================================
'Move to the rear side
'=======================================
   Message "Positioning Probe to Rear of Part.........."
   Call GotoZHeight()
   Code "G0 Y" & (Y1 + EdgeLength + Clearance)
   Call WaitForMove()
   Call GotoZZero()

'=======================================
'Negative Y Probing
'=======================================
   ProbeLengthY = YStart-DMax
   ProbeDOffset = ProbeD/4
   DirText = "Back"

   Call ProbeYAxis()

   Y2 = YHit - ProbeD/2 'Tool position at probe hit.
   YPosCenter = (Y1+Y2)/2 ' Calculate our Y Center
   
   Code "G0 Y" & (YHit + ProbeD/4) ' Back off part
   Call WaitForMove()
   
   Call GotoZHeight()

     'Set our Y Axis Center Text.  Clip to .0000x
   YCenter = CStr(YPosCenter-YOffset)
   LabelLength = InStr(YCenter,".") + 5
   YCenter=Left(YCenter, LabelLength)

   Message "Y Center Found.........."
   Sleep(100)

'=======================================
'Move to the right side
'=======================================
   Message "Positioning Probe to Right Side of Part.........."
   Code "G0 X" & (XStart +  EdgeLength/2 + Clearance) & " Y" & YPosCenter
   Call WaitForMove()
   Call GotoZZero()

'=======================================
'Negative X Probing
'=======================================
   ProbeLengthX = XStart - DMax
   ProbeDOffset = ProbeD/4
   DirText = "Right"

   Call ProbeXAxis()
   X1 = XHit - ProbeD/2 'Tool position at probe hit.

   Code "G0 X" & (XHit + ProbeD/4)
   Call WaitForMove()
   
   Call GotoZHeight()

'=======================================
'Move to the left side
'=======================================
   Message "Positioning Probe to Left Side of Part.........."
   Code "G0 X" & (X1 - EdgeLength - Clearance) 
   Call WaitForMove()
   Call GotoZZero()

'=======================================
'Positive X Probing
'=======================================
   ProbeLengthX = XStart + DMax
   ProbeDOffset = -1 * ProbeD/4
   DirText = "Left"

   Call ProbeXAxis()
   X2 = XHit + ProbeD/2 'Tool position at probe hit.

   XPosCenter = (X1+X2)/2

   Code "G0 X" & (XHit - ProbeD/4) 
   Call WaitForMove()
   Call GotoZHeight()
   Code "G0 Y" & (YPosCenter) & " G0 X" & (XPosCenter) 
   Call WaitForMove()

   If ZeroYN = 1 Then
      Call WaitForMove()
      SetOEMDRO(800,-1 * XOffset) 'set our DRO to out offset values
      SetOEMDRO(801,-1 * YOffset)
      Sleep(100)
   End If

   XCenter = CStr(XPosCenter - XOffset)
   LabelLength = InStr(XCenter,".") + 5
   XCenter=Left(XCenter, LabelLength)
   
   Message "X Center Found.........."
   Sleep(150)

   SetUserLabel (9,XCenter)
   SetUserLabel (10,YCenter)

   Call ReturnToInitialState()
   Sleep(100)
   Message "Center Found"
End Sub

'======================================= 
'Check For Error Routine
'======================================= 
Sub CheckforErrors()
   If GetOemLED(16)<>0 Then ' Check for Machine Coordinates
     Message "Please change to working coordinates"
     Call ReturnToInitialState() 'just in case
     Exit Sub ' Exit if in Machine Coordinates
   End If
   If GetOemLED(825)<>0 Then
      Message "Probe Grounded - Check connection and try again"
      Call ReturnToInitialState() 'just in case
      Sleep(250)
      Exit Sub ' Exit if probe is tripped
   End If
End Sub
 
'======================================= 
'Return Machine to initial state
'======================================= 
Sub ReturnToInitialState()
   If CurrentAbsInc = 0 Then 'if G91 was in effect before then return to it
      Code "G91"
   End If
   If CurrentGMode = 0 Then 'if G0 was in effect before then return to it
      Code "G0"
   End If

   Code "F" & CurrentFeed ' Reset to original feed rate

   Call SetOEMDRO(59,XScale)
   Call SetOEMDRO(60,YScale)
   Call SetOEMDRO(61,ZScale)
End Sub 

'======================================= 
'Save Current Machine State
'======================================= 
Sub GetCurrentMachineState()
   CurrentAbsInc = GetOemLED(48) 'Get the current G90/G91 state
   CurrentGmode = GetOemDRO(819) 'Get the current G0/G1 state
   CurrentFeed = GetOemDRO(818)  ' Get the current feedrate to return to later
   XStart = GetOEMDRO(800)
   YStart = GetOEMDRO(801)
   ZStart = GetOEMDRO(802)

   'Get Axis Scale factors
   XScale = GetOEMDRO(59)
   YScale = GetOEMDRO(60)
   ZScale = GetOEMDRO(61)

 '  If(Abs(ZStart) > .00005) Then
 '    Message "Please Zero the Z Axis at Probing Depth"
 '    Call ReturnToInitialState() 'just in case
 '    Exit Sub ' Exit if in Machine Coordinates
   
'   End if
   

   If GetOEMLED(1871) Then
      ZeroYN=1
   Else
      ZeroYN=0
   End If
End Sub

'======================================= 
'Get User Variables
'======================================= 
Sub GetUserProbingInputs()
   FRate1 = Abs(GetUserDRO(1821))
   FRate2 = Abs(GetUserDRO(1822))
   DMax = Abs(GetUserDRO(1823))
   PlateOffset = GetUserDRO(1824)
   ToolNo = GetCurrentTool()
   ToolD = GetToolParam(ToolNo,1)
   If GetUserDRO(1829) = 0 then	
   	ProbeD = ToolD
	Else 
	ProbeD = GetUserDRO(1829)
   End If
   Clearance = Abs(GetUserDRO(1825))
   XOffset = GetUserDRO(1826)
   YOffset = GetUserDRO(1827)
   EdgeLength = Abs(GetUserDRO(1828))
   SafeZHeight = Abs(GetUserDRO(1830))

End Sub

Sub ProbeYAxis()
   Message "Probing " & DirText & " for Y Center....."
   Sleep(250)  'Pause 1 second
   
   ProbingFeedRate = Frate1
   Call ProbeY()'Probe For Positive Y Feed1
   

   If FRate2<>0 Then 'If user has secondary feed rate entered, reprobe for more accuracy
      Message "Slow Probing " & DirText & " for Y position....."
      Code "G0 Y" & YHit + ProbeDOffset ' back off quarter probe diameter
      Call WaitForMove()
      ProbingFeedRate = Frate2
      Call ProbeY()'Probe For Positive Y Feed2
   End If
 End Sub

Sub ProbeY()
      Code "F" & ProbingFeedRate ' Set our new feedrate
      Sleep(100)
      Code "G31 Y" & ProbeLengthY 'Reprobe at new feedrate
      Call WaitForMove()
      YHit = GetOEMDRO(801)
End Sub

Sub ProbeXAxis()
   Message "Probing " & DirText & " for X Center....."
   Sleep(250)  'Pause 1/4 second
   
   ProbingFeedRate = Frate1
   Call ProbeX()

   If FRate2<>0 Then
      Code "G0 X" & XHit + ProbeDOffset
      Call WaitForMove()
      Message "Slow Probing " & DirText & " for X Position....."
      ProbingFeedRate = Frate2
      Call ProbeX()
   End If
End Sub

Sub ProbeX()
   Code "F" & ProbingFeedRate & " G31 X" & ProbeLengthX
   Call WaitForMove()
   XHit = GetOEMDRO(800)
End Sub
   
Sub GotoZHeight()
   Code "G0 Z" & (SafeZHeight + ZStart)
   Call WaitForMove()
End Sub    

Sub GotoZZero()
   Code "G0 Z" & ZStart
   Call WaitForMove()
End Sub    
       
Sub WaitForMove()
   While IsMoving()
   Wend
   Sleep(WaitTime)  'Pause 1/4 second
End Sub