Think I found it.
Edited this line:
double ZOriginalPos = exec.GetZpos(); // Get the current machine coordinates
To
double ZOriginalPos = exec.GetZmachpos(); // Get the current machine coordinates
And them removed these lines so the program start running at safe z height:
exec.Code ("G00 Z" + ZOriginalPos);
while(exec.IsMoving()){}
exec.Wait(100);
Works so far. But if it is right I'm not sure
Code:
//M6 Macro for automatic tool measuring. First the machine moves to the tool change position, then to the fixed plate
//to measure the new tool length
double ToolChangeX = 10; //Tool change X position (all in machine coordinates)
double ToolChangeY = 2.5; //Tool change Y position
double ToolChangeZ = -0.1; //Tool change Z position
double FixedPlateX = -368.99; //Fixed plate X position (machine coordinate)
double FixedPlateY = 363.99; //Fixed plate Y position
double CaFixedPlateZ = -100; //Fixed plate ca. Z position
double SafeZ = -0.1; //Safe Z in machine coordinates
double ZRetractHeight = 20; //Height above Z0 to which probe will retract
double CoarseRate = 500; //Feedrate for initial probing
double FineRate = 10; //Feedrate for fine probing
double Zmin = -125; //maximum probing distance
double PlateDifference = exec.GetCpos(); //Fetches the plate difference from the C axis DRO
// G91 Sets incremental mode,
// G90 Sets absolute distance mode
// G53 Sets move in absolute coordinate for that line
int originaldistancemode = exec.actualdistmode; // remember the distance mode
int originalmodalmode = exec.actualmodal; // remember the modal mode
exec.Stopspin(); //Turn off spindle
exec.Code ("G91"); //sets incremental mode for the whole macro
double XOriginalPos = exec.GetXmachpos(); // Get the current machine coordinates
double YOriginalPos = exec.GetYmachpos(); // Get the current machine coordinates
double ZOriginalPos = exec.GetZpos(); // Get the current machine coordinates
exec.Code ("G53 G00 Z" + ToolChangeZ); //Moves to tool change position
while (exec.IsMoving ()){}
exec.Wait (200);
exec.Code ("G53 G00 X" + ToolChangeX+ " Y" + ToolChangeY); //Moves to tool change position
while (exec.IsMoving ()){}
exec.Wait (200);
MessageBox.Show ("Press OK when toolchange is complete");
exec.Wait (200);
exec.Code ("G53 G00 X" + FixedPlateX + " Y" + FixedPlateY); //Move to Fixed Plate Position
while(exec.IsMoving()){}
exec.Wait(200);
exec.Code("G53Z" + CaFixedPlateZ); //Retracts Z for move to plate
while(exec.IsMoving()){}
exec.Wait(200);
exec.Code("G31 Zmin + "F" + CoarseRate); // Probe Z quickly to get rough height
while(exec.IsMoving()){}
exec.Wait(200);
exec.Code ("G00 Z" + 0.4); // Retract 0.2mm above the plate
while(exec.IsMoving()){}
exec.Wait(100);
exec.Code("G31 Zmin + "F" + FineRate); // Probe Z slowly for better resolution
while(exec.IsMoving()){}
exec.Wait(200);
//because we stopped at the fixed plate, we now want to update our DRO to the value of the plate difference
exec.mainform.sumoffsetcontrol1.G54.newCzinput(PlateDifference); // Set G54 to new Zzero
exec.mainform.sumoffsetcontrol1.G55.newCzinput(PlateDifference); // Set G55 to new Zzero
exec.mainform.sumoffsetcontrol1.G56.newCzinput(PlateDifference); // Set G56 to new Zzero
exec.mainform.sumoffsetcontrol1.G57.newCzinput(PlateDifference); // Set G57 to new Zzero
exec.mainform.sumoffsetcontrol1.G58.newCzinput(PlateDifference); // Set G58 to new Zzero
exec.mainform.sumoffsetcontrol1.G59.newCzinput(PlateDifference); // Set G59 to new Zzero
exec.Code ("G53 G00 Z" + SafeZ); //retracts Z
while(exec.IsMoving()){}
exec.Wait(100);
exec.Code ("G53 G00 X" + XOriginalPos + " Y" + YOriginalPos); //moves to original XY position
while(exec.IsMoving()){}
exec.Wait(100);
exec.Code ("G90"); //returns machine to absolute for Z move and rest of code
//exec.Code ("G00 Z" + ZOriginalPos);
//while(exec.IsMoving()){}
//exec.Wait(100);
exec.Code("Griginaldistancemode); // Set system back to the original distance mode
exec.Code("Griginalmodalmode); // Set system back to the original distance mode