'(M3332) Get Angle of Workpiece

Angle = GetOemDro(118)

pi=4*Atn(1)

CurrentToolDiameter = GetOemDRO(43)
X = GetUserDro (1053)
Y = GetUserDro (1054)
Z = GetUserDro (1055)

CurrentFeed = GetOemDRO(818) 'Get the current feedrate to return to later
CurrentAbsInc = GetOemLED(48) 'Get the current G90/G91 state
CurrentGmode = GetOemDRO(819) 'Get the current G0/G1 state
CurrentToolDiameter = GetOemDRO(43) 'gets the current tool diameter
PlateThickness = GetUserDRO(1051)
ToolLength = GetOemDRO(42)

MoveXDistance = ((X * 1.1) + CurrentToolDiameter)
MoveYDistance = ((Y * 1.1) + CurrentToolDiameter)
MoveDistance = .5 + CurrentToolDiameter
MoveZDistance = .5

If GetOemLed (825) <> 0 Then 'Check to see if the probe is already grounded or faulty
    Call ProbeGrounded()
    Exit Sub
Else

'Get top of part

'DoOEMButton (1010) 'zero the Z axis so the probe move will start from here
Code "G4 P4" ' this delay gives me time to get from computer to hold probe in place
Code "G91 G31Z-2.0 F4" 'probing move, can set the feed rate here as well as how far to move
While IsMoving() 'wait while it happens
Wend
'Call SetDro (2, PlateThickness)

ZProbePos = GetVar(2002) 'get the axact point the probe was hit
'Code "G0 Z" &PlateThickness
'While IsMoving ()
'Wend
Code "G1 G91 F40 Z" &MoveZDistance 'put the Z retract height you want here
While IsMoving ()
Wend

'Code "(Z axis is now zeroed)" 'puts this message in the status bar
Code "F" &CurrentFeed 'Returns to prior feed rate


'Call SetDro (2,GetDro(2)-Z)

'Probe The X sides

    XCurrent = GetDro(0)
    YCurrent = GetDro(1)

    Code "G4 P1" 'Pause 1 second to give time to position probe plate
    While IsMoving ()
    Wend
    Call SetDro (0,0.000)
    While IsMoving ()
    Wend
    Call SetDro (1,0.000)
    Code "F4" 'slow feed rate to 4 ipm


Rem Probe Y Face Side

    Code "G90 G0 X" &-MoveDistance
    Code "G4 P0.25" 
    Code "G91 G0 Z" &-MoveZDistance-.2
    Code "G4 P0.25" 
    XNew = Xcurrent + 3
    Code "G31 X" &XNew 'Probe X1,Y1 (P1) location
    While IsMoving() 'wait for the move to finish
    Wend
    X1 = GetVar(2000) 'get the probe touch location
    Y1=0 ' Should always be zero
    Y2 = Y-0.5 '1/2 inch back from end of block

    Code "G91 G0 X-.25"
    Code "G4 P0.25" 
    Code "G91 G0 Y" &Y2
    Code "G4 P0.25" 
    Code "G31 X" &XNew 'Probe X2,Y2 (P2) location
    While IsMoving() 'wait for the move to finish
    Wend
    X2 = GetVar(2000) 'get the probe touch location
    'Y2= Block length - 1/2 inch

    Code "G91 G0 Z" &MoveZDistance+.2
    Code "G4 P0.25" 
    Code "G91 G0 Y" &-Y2 'Go back to the first spot
    Code "G4 P0.25" 
    Code "G91 G0 X" &CurrentToolDiameter+.1 'rapid move back to start point
    Code "G4 P0.25" 

Rem Probe X Face Side

    Code "G90 G0 Y" &-MoveDistance
    Code "G4 P0.25" 
    Code "G91 G0 Z" &-MoveZDistance-.2
    Code "G4 P0.25" 
    YNew = YCurrent + 3
    Code "G31 Y" &YNew 'Probe X3,Y3 (P3) location
    While IsMoving()
    Wend
    Y3 = GetVar(2001)   'get the probe location 
    'X3=0
    X4 = X-0.5 '1/2 inch back from end of block

    Code "G91 G0 Y-.25"
    Code "G4 P0.25" 
    Code "G91 G0 X" &X4
    Code "G4 P0.25" 
    Code "G31 Y" &YNew 'Probe X4,Y4 (P4) location
    While IsMoving() 'wait for the move to finish
    Wend
    Y4 = GetVar(2001) 'get the probe touch location
    'X4= Block Width - 1/2 inch

    Code "G90 G0 Z" &MoveZDistance+1    'Back up and clear
    Code "G4 P0.25" 

t1= (x4-x3)*(y1-y3)-(y4-y3)*(x1-x3)
b1= (y4-y3)*(x2-x1)-(x4-x3)*(y2-y1)
ua=t1/b1

t2= (x2-x1)*(y1-y3)-(y2-y1)*(x1-x3)
b2= (y4-y3)*(x2-x1)-(x4-x3)*(y2-y1)
ub=t2/b2

XH=x1+ua*(x2-x1) ' X Home  distance away
YH=y1+ua*(y2-y1) ' Y Home  distance away


If X>Y Then 
   'LongestSide=X
   a=(Y4-Y3)
   b=(X4-X3)
   v=a/b
   NewAngle=Atn(v)*(180/pi)
Else
   'LongestSide=Y
   a=(X2-X1)
   b=(Y2-Y1)
   v=b/a
   NewAngle=Atn(v)*(180/pi)
   If NewAngle > 0 Then
     NewAngle=NewAngle-90
   Else
     NewAngle=NewAngle+90
   End If
End If

Rem move To the Block Lower Corner

    Code "G90 G0 Y" &YH + CurrentToolDiameter/2
    Code "G90 G0 X" &XH + CurrentToolDiameter/2
    While IsMoving ()
    Wend
    Call SetDro (1,0.000)
    While IsMoving ()
    Wend
    Code "G4 P0.25" 
    Call SetDro (0,0.000)
    Call SetDro (1,0.000)

    Call SetOemDro (118,NewAngle) ' now you can rotate the block


'Call SetDro (2,Z + PlateThickness)

    Code "G4 P0.25" 

    Code "F" &CurrentFeed 'restore starting feed rate
	Call ReturnG90G91State()
	Exit Sub
End If
 
Sub ProbeGrounded()
	Code "(Probe plate is grounded, check connection and try again)"
	Call ReturnG90G91State()
End Sub

Sub ReturnG90G91State()
	If CurrentAbsInc = 0 Then 'if G91 was in effect before then return to it
		Code "G91"
	Else
		Code "G90"
	End If
	If CurrentGMode = 0 Then 'if G0 was in effect before then return to it
		Code "G0"
	Else
		Code "G1"
	End If
End Sub