pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow

Grbl_Esp32/Custom/4axis_xyxz.cpp

@@ -3,28 +3,10 @@
 	Part of Grbl_ESP32
 */
 
-uint8_t AmountOfToolChanges;
-
-#ifdef USE_MACHINE_INIT
-/*
-machine_init() is called when Grbl_ESP32 first starts. You can use it to do any
-special things your machine needs at startup.
-*/
-void machine_init()
-{
-    // First call means AmountOfToolChanges=0.
-    AmountOfToolChanges=0;
-
-    grbl_send(CLIENT_ALL, "machine_init");
-}
-#endif
-
-#ifdef USE_TOOL_CHANGE
-/*
-  user_tool_change() is called when tool change gcode is received,
-  to perform appropriate actions for your machine.
-*/
+uint8_t AmountOfToolChanges; //  Each new tool increases this by 1. Before first tool, it<69>s 0.
+float ZPosOld, ZPosNew;
 
+// return current Z machine position
 float getCurrentZPos()
 {
     int32_t current_position[N_AXIS]; // copy of current location
@@ -36,6 +18,7 @@ float getCurrentZPos()
     return m_pos[Z_AXIS];
 }
 
+// return last Z probe machine position
 float getLastZProbePos()
 {
     float m_pos[N_AXIS];   // machine position in mm
@@ -47,6 +30,7 @@ float getLastZProbePos()
     return m_pos[Z_AXIS];
 }
 
+// return the stored G54/Z value
 float getG54Zvalue()
 {
     float coord_data[N_AXIS];
@@ -56,6 +40,28 @@ float getG54Zvalue()
     return coord_data[Z_AXIS];
 }
 
+#ifdef USE_MACHINE_INIT
+/*
+    machine_init() is called when Grbl_ESP32 first starts. You can use it to do any
+    special things your machine needs at startup.
+
+    Prerequisite: add "#define USE_MACHINE_INIT" to your machine.h file
+*/
+void machine_init()
+{
+    // We start with no tool changes yet
+    AmountOfToolChanges=0;
+    ZPosOld=0;
+ }
+#endif
+
+#ifdef USE_TOOL_CHANGE
+/*
+    user_tool_change() is called when tool change gcode is received,
+    to perform appropriate actions for your machine.
+
+    Prerequisite: add "#define USE_TOOL_CHANGE" to your machine.h file
+*/
 void user_tool_change(uint8_t new_tool)
 {
     // Variables
@@ -63,80 +69,105 @@ void user_tool_change(uint8_t new_tool)
     float value1, value2, diff;
 
     // Start of function
-    // AmountOfToolChanges=AmountOfToolChanges+1 each time.
+    // Increase AmountOfToolChanges every time by 1
 
     AmountOfToolChanges++;
 
-    sprintf(temp, "Tool change amount=%d\r\n", AmountOfToolChanges);
+    /*
+        Prerequisites
+        - First tool properly mounted in spindle
+        - G54 is set. G54/Z touches the work piece surface
+        - Reminder: set Autodesk Fusion 360 raw material origin to G54
+
+        What happens with every call to this function
+
+        + Switch off spindle
+        + Move to tool change position                          (not for the first call)
+        - Hold                                                  (not for the first call)
+
+        - Manual tool change                                    (not for the first call)
+
+        - Proceed by un-hold                                    (not for the first call)
+        - Move to Z probe button
+        - Quick and then slow Z probe for better precision
+        - Save Z probe position to "NEW"
+        - Calculate delta between "NEW" and "OLD"
+        - Call G43.1 with that delta
+        - Copy Z probe position "NEW" to "OLD"
+    
+        - Go to G54
+        - Hold
+
+        - Switch on spindle
+        - Proceed with job
+    */
+
+    // Report tool change amount. During first call it<69>s 0.
+    sprintf(temp, "Tool change #=%d\r\n", AmountOfToolChanges);
     grbl_send(CLIENT_ALL, temp);
 
     // Init. Safe start block. G54, XY plane, mm mode, relative addressing mode
     inputBuffer.push("G54\r\n");
     inputBuffer.push("G17 G21 G91\r\n");
+    inputBuffer.push("G0 F100\r\n");
+    protocol_buffer_synchronize();
 
     // Switch off spindle
     inputBuffer.push("M05\r\n");
+    protocol_buffer_synchronize();
 
-    /*
-    // Set speed
-    inputBuffer.push("F100\r\n");
+    // Go to tool change position
+    inputBuffer.push("G53 G0 Z-5\r\n");
+    inputBuffer.push("G53 G0 X-5 Y-210\r\n");
+    protocol_buffer_synchronize();
+
+    // Hold
+    inputBuffer.push("M0\r\n");
+    protocol_buffer_synchronize();
+
+     /*
+       do some reporting
+    */
+    //current Z pos. Should be, where the button is pressed
+    sprintf(temp, "(1) z pos current at hold=%4.3f\r\n", getCurrentZPos());
+    grbl_send(CLIENT_ALL, temp);
 
     // Place spindle directly above button in X/Y and a few mm above Z
-    inputBuffer.push("G53 G0 Z-5 F500\r\n");
-    inputBuffer.push("G53 G0 X-29 Y-410 F100\r\n");
-    inputBuffer.push("G53 G0 Z-60 F100\r\n");
+    inputBuffer.push("G53 G0 X-29 Y-410\r\n");
+    inputBuffer.push("G53 G0 Z-60 \r\n");
+    protocol_buffer_synchronize();
 
-    // Z probe, max. 50mm to find button
+    // Z probe, max. 50mm to press button
     inputBuffer.push("G38.2 Z-50 F250\r\n");
+    protocol_buffer_synchronize();
 
-    // Raise spindle a little bit
+    // Raise spindle a little bit for slower try to increase accuracy
     inputBuffer.push("G0 Z1.5\r\n");
+    protocol_buffer_synchronize();
 
     // Z probe again
     inputBuffer.push("G38.2 Z-2 F30\r\n");
-    */
-
-
     protocol_buffer_synchronize(); // wait for all previous moves to complete
 
-
-    // Z Probe Pos.
-    value1=getCurrentZPos();
-    sprintf(temp, "z probe pos=%4.3f\r\n", value1);
-    grbl_send(CLIENT_ALL, temp); // send the report
-
-
-    // Raise spindle a little bit
-    inputBuffer.push("G0 Z1.5\r\n");
-    protocol_buffer_synchronize(); // wait for all previous moves to complete
-
-    // Z Probe Pos.
-    value2=getCurrentZPos();
-    sprintf(temp, "z probe pos neu=%4.3f\r\n",  value2);
-    grbl_send(CLIENT_ALL, temp); // send the report
-
-    diff = value2 - value1;
-    sprintf(temp, "diff=%4.3f mm\r\n",  diff);
-    grbl_send(CLIENT_ALL, temp); // send the report
-
-    
-
-    //sprintf(temp, "z pos=%4.3f\r\n", getCurrentZPos());
-    //grbl_send(CLIENT_ALL, temp); // send the report
-
-    //sprintf(temp, "z G54=%4.3f\r\n", getG54Zvalue());
-    //grbl_send(CLIENT_ALL, temp); // send the report
-
-
-  
-    
-
     // Clean up
     inputBuffer.push("G90\r\n");
-
-
     protocol_buffer_synchronize(); // wait for all previous moves to complete
 
+    /*
+       do some reporting
+    */
+    //current Z pos. Should be, where the button is pressed
+    sprintf(temp, "(2) z pos current=%4.3f\r\n", getCurrentZPos());
+    grbl_send(CLIENT_ALL, temp);
+
+    //Z probe position. Should be identical to getCurrentZPos() here
+    sprintf(temp, "(3) z probe pos=%4.3f\r\n", getLastZProbePos());
+    grbl_send(CLIENT_ALL, temp);
+    
+    //get G54/Z position, as manually configured by jogging. WORKS!
+    sprintf(temp, "(4) z G54 pos=%4.3f\r\n", getG54Zvalue());
+    grbl_send(CLIENT_ALL, temp);
+
     return;
 }
 #endif