measure_script

exec(open('functions.py', encoding='utf-8').read())
import sys
import time

probeIndex = probeIndexTouch

#check machine in idle state
if d.getState() != State.Idle:
    print("Steuerung muß eingeschaltet sein.")
    exit(0)

# check Axis referenced
x=d.isAxisReferenced(Axis.X)
if Axis.X == 0:
   print("Achsen müssen referenziert sein!")
   sys.exit()

y=d.isAxisReferenced(Axis.Y)
if Axis.Y == 0:
   print("Achsen müssen referenziert sein!")
   sys.exit()

z=d.isAxisReferenced(Axis.Z)
if Axis.Z == 0:
   print("Achsen müssen referenziert sein!")
   sys.exit()

resetG68()

#search distance
searchDistance = searchDistance
toolSetterHeight=0 # Hier die Höhe des WLS eintragen oder 0 wenn 3d Taster benutzt wird
# the absolute position of the Z axis of the probe contact for the reference tool
refToolProbePos = -60
# approach velocity (units/min)
vel =1000
# probing velocity (units/min)
fastProbeVel = fastProbeSpeed
slowProbeVel = slowProbeSpeed
feedMeasurementVel = feedMeasurementSpeed
# lift up dist before fine probing
goUpDist = 3
# delay (seconds) before fine probing
fineProbingDelay = 0.2
# other options
moveX = True
moveY = True
checkFineProbingDiff = True
fineProbeMaxAllowedDiff = 0.1

#############################################
# Start Z Messung
#############################################
d.setSpindleState(SpindleState.OFF)
toolNr = d.getSpindleToolNumber()
if(toolNr == 0):
  sys.exit("Tool(0) has no tool lenght offset. Probing failed!")

# get current absolute position
pos = d.getPosition(CoordMode.Machine)

# start fast probing
pos[Axis.Z.value] = pos[Axis.Z.value]-searchDistance
probeResult = d.executeProbing(CoordMode.Machine, pos, probeIndex, fastProbeVel)
if(probeResult == False):
  sys.exit("fast probing failed!")
# get fast probe contact position
fastProbeFinishPos = d.getProbingPosition(CoordMode.Machine)

# lift-up Z
d.moveAxisIncremental(Axis.Z, goUpDist, vel)
# delay
time.sleep(fineProbingDelay)
# start fine probing
probeResult = d.executeProbing(CoordMode.Machine, pos, probeIndex, slowProbeVel)
if(probeResult == False):
  sys.exit("slow probing failed!")
# get fine probe contact position
probeFinishPos = d.getProbingPosition(CoordMode.Machine)

# check diff between fast and fine probing
probeDiff = abs(fastProbeFinishPos[Axis.Z.value] - probeFinishPos[Axis.Z.value])
if(probeDiff > fineProbeMaxAllowedDiff and checkFineProbingDiff == True):
  errMsg = "ERROR: fine probing difference limit exceeded! (diff: {:.3f})".format(probeDiff)
  sys.exit( errMsg)

d.setAxisProgPosition(Axis.Z,0+toolSetterHeight)
d.moveAxisIncremental(Axis.Z,0+zSafeHeight, feedMeasurementVel)

#Message Box Start Rechteckmessung
msg.info("Z Tiefenmessung erfolgreich beendet. Bitte Messtaster mittig über dem zu messenden Teil positionieren")



###
# first Measurement in Yplus
###
d.moveAxisIncremental(Axis.Y,-((yLength/2)+xyClearance),feedMeasurementVel)
d.moveAxisIncremental(Axis.X,((xLength/2)-edgeOffset),feedMeasurementVel)

posZ=d.getPosition(CoordMode.Machine)
posZ[Axis.Z.value] = posZ[Axis.Z.value]-(zSafeHeight-zMeasureDeep)

#execute positioning the z axis down, when touch probe is active breake
zStartPos = d.executeProbing(CoordMode.Machine, posZ,probeIndex ,feedMeasurementVel)
if(zStartPos==True):
    sys.exit("Z movement fail, please position the probe once again!!")
probePoint1=probeYPlus()


###
# second Measurement in Yplus
###

d.moveAxisIncremental(Axis.Y,-(xyClearance),feedMeasurementVel)
d.moveAxisIncremental(Axis.Z,(zSafeHeight-zMeasureDeep),feedMeasurementVel)
d.moveAxisIncremental(Axis.X,-(xLength-(2*edgeOffset)),feedMeasurementVel)
d.moveAxisIncremental(Axis.Z,-(zSafeHeight-zMeasureDeep),feedMeasurementVel)
probePoint2=probeYPlus()

d.moveAxisIncremental(Axis.Y,-(xyClearance),feedMeasurementVel)
d.moveAxisIncremental(Axis.Z,(zSafeHeight-zMeasureDeep),feedMeasurementVel)

###
# Measurement in Xplus
###

d.moveAxisIncremental(Axis.X,-(edgeOffset+xyClearance),feedMeasurementVel)
d.moveAxisIncremental(Axis.Y,(xyClearance+yLength/2),feedMeasurementVel)
d.moveAxisIncremental(Axis.Z,-(zSafeHeight-zMeasureDeep),feedMeasurementVel)
probePoint3=probeXPlus()

d.moveAxisIncremental(Axis.X,-(xyClearance),feedMeasurementVel)
d.moveAxisIncremental(Axis.Z,(zSafeHeight-zMeasureDeep),feedMeasurementVel)

###
# Measurement in Xminus
###

d.moveAxisIncremental(Axis.X,(xLength+(2*xyClearance)),feedMeasurementVel)
d.moveAxisIncremental(Axis.Z,-(zSafeHeight-zMeasureDeep),feedMeasurementVel)
probePoint4=probeXMinus()

d.moveAxisIncremental(Axis.X,(xyClearance),feedMeasurementVel)
d.moveAxisIncremental(Axis.Z,(zSafeHeight-zMeasureDeep),feedMeasurementVel)

###
#Measurement in Yminus
###

d.moveAxisIncremental(Axis.X,-(xLength/2+xyClearance),feedMeasurementVel)
d.moveAxisIncremental(Axis.Y,((yLength/2)+xyClearance),feedMeasurementVel)
d.moveAxisIncremental(Axis.Z,-(zSafeHeight-zMeasureDeep),feedMeasurementVel)
probePoint5=probeYMinus()

d.moveAxisIncremental(Axis.Y,(xyClearance),feedMeasurementVel)
d.moveAxisIncremental(Axis.Z,(zSafeHeight-zMeasureDeep),feedMeasurementVel)

rotationAngle=rotationAngle(probePoint1,probePoint2)

def rectangle_center_zeropointX(p1,p2):
    distance=(p2[0]-p1[0])
    midpoint_x = p1[0]+(distance/2)

    return midpoint_x


def rectangle_center_zeropointY(point1, point2, point3):
    #equilateral triangle
    sidec = math.sqrt((point2[0] - point1[0])**2 + (point1[1] - point2[1])**2)
    sidea = math.sqrt((point2[0] - point3[0])**2 + (point2[1] - point3[1])**2)
    sideb = math.sqrt((point3[0] - point1[0])**2 + (point3[1] - point1[1])**2)

    if sidea == sideb:
        height = math.sqrt(sidec/2)

    #calculate hypotenuses
    sidep = math.sqrt(sidea**2 / sidec)
    sideq = math.sqrt(sideb**2 / sidec)

    #right triangle
    height = math.sqrt(sidea**2 - sidep**2)

    midpoint_y = height/2

    return midpoint_y

xZeroPoint=rectangle_center_zeropointX(probePoint3, probePoint4)
yZeroPoint=rectangle_center_zeropointY(probePoint2, probePoint1, probePoint5)

setAxisProgX(xZeroPoint)
setAxisProgY(yZeroPoint)

endPos=pos = d.getPosition(CoordMode.Program)
endPos[0]=0
endPos[1]=0
d.moveToPosition( CoordMode.Program, endPos,feedMeasurementVel)

print("Messung erfolgreich beendet.")
distanceX=midpoint_x*2
distanceY=midpoint_y*2
print(f"Das Maße betragen für X: {distanceX:.3f}mm x Y: {distanceY:.3f}mm")