running with state machine, repeatable results in test environment

Grbl_Esp32/Custom/4axis_xyxz.cpp

@@ -3,44 +3,44 @@
 	Part of Grbl_ESP32
 */
 
+#define DEBUG 0; // do I want debug messages? yes/no
+
+/*
+   VARIABLES
+*/
 uint8_t AmountOfToolChanges; //  Each new tool increases this by 1. Before first tool, it<69>s 0.
-float ZPosOld, ZPosNew;
+uint8_t current_toolNo, new_toolNo;
+float firstZPos, newZPos, Zdiff;
 static TaskHandle_t zProbeSyncTaskHandle = 0;
 
+// Finite state machine.
+uint8_t state; // final state machine
 
-// return current Z machine position
+#define TOOLCHANGE_IDLE       0 // initial state. tool change switched off. set during machine_init()
-float getCurrentZPos()
+#define TOOLCHANGE_INIT       1 // do some reporting at first. tool change number x and so on
-{
+#define TOOLCHANGE_START      2 // tool change function called by G code "T[1..n] M06". Not triggered by G38.2 or ESP3D probe function :-)
-    int32_t current_position[N_AXIS]; // copy of current location
-    float m_pos[N_AXIS];   // machine position in mm
 
-    memcpy(current_position, sys_position, sizeof(sys_position)); // get current position in step
+// first Z probe before tool change
-    system_convert_array_steps_to_mpos(m_pos, sys_position); // convert to millimeters
+#define TOOLCHANGE_ZPROBE_1a  3 // Z probe #1 (quick). Touch down and order to press the button
+#define TOOLCHANGE_ZPROBE_1b  4 // Z probe #1 (quick). Wait for button press.
 
-    return m_pos[Z_AXIS];
+#define TOOLCHANGE_MANUAL     5 // go to tool change position
-}
 
-// return last Z probe machine position
+// second Z probe after tool change. Now we can compare
-float getLastZProbePos()
+#define TOOLCHANGE_ZPROBE_2a  6 // Z probe #2 (slow ). Touch down and order to press the button
-{
+#define TOOLCHANGE_ZPROBE_2b  7 // Z probe #2 (slow ). Wait for button press.
-    float m_pos[N_AXIS];   // machine position in mm
-    char output[200];
 
-    //memcpy(current_position, sys_position, sizeof(sys_position)); // get current position in step
+#define TOOLCHANGE_FINISH    99 // tool change finish. do some reporting, clean up, etc.
-    system_convert_array_steps_to_mpos(m_pos, sys_probe_position); // convert to millimeters
 
-    return m_pos[Z_AXIS];
+// declare functions
-}
+float getCurrentZPos();
+float getLastZProbePos();
+float getG54Zvalue();
+int8_t getState();
+bool checkState(uint8_t checkState);
+void setState(int8_t newState);
+void setState(int8_t newState, uint8_t newtool);
 
-// return the stored G54/Z value
-float getG54Zvalue()
-{
-    float coord_data[N_AXIS];
-
-    settings_read_coord_data(0, coord_data); //0=G54
-
-    return coord_data[Z_AXIS];
-}
 
 #ifdef USE_MACHINE_INIT
 /*
@@ -53,49 +53,233 @@ void machine_init()
 {
     // We start with no tool changes yet
     AmountOfToolChanges=0;
-    ZPosOld=0;
 
-    xTaskCreatePinnedToCore(zProbeSyncTask,   // task
+    // unknown at the beginning. But this will change, if the first tool is loaded
-                            "zProbeSyncTask", // name for task
+    current_toolNo = 0;
-                            4096,				// size of task stack
+
-                            NULL,				// parameters
+    // Initialize state machine
-                            1,					// priority
+    state=TOOLCHANGE_IDLE;
-                            &zProbeSyncTaskHandle,
+
-                            0 // core
+    // TODO this task runs permanently. Alternative?
+    xTaskCreatePinnedToCore(zProbeSyncTask,         // task
+                            "zProbeSyncTask",       // name for task
+                            4096,				    // size of task stack
+                            NULL,				    // parameters
+                            1,					    // priority
+                            &zProbeSyncTaskHandle,  // handle
+                            0                       // core
                            );
  }
 #endif
 
-void zProbeSyncTask(void* pvParameters) {
+// return current state machine status
-    int32_t sys_POS_steps[N_AXIS], sys_PROBE_steps[N_AXIS];
+int8_t getState()
-    float sys_POS[N_AXIS], sys_PROBEPOS[N_AXIS];
+{
-    float m_pos[N_AXIS];			  // machine position in mm
+    return state;
-    TickType_t xLastWakeTime;
+}
-    char temp[200];
 
-    const TickType_t xProbeFrequency = ZPROBE_TASK_FREQ; // in ticks (typically ms)
+// check, if given state is the active
+bool checkState(uint8_t checkState)
+{
+    if (state == checkState) return 1;
+    else return 0;
+}
+
+// setnew state machine status
+void setState(int8_t newState)
+{
+    state = newState;
+}
+
+// setnew state machine status
+void setState(int8_t newState, uint8_t newtool)
+{
+    setState(newState);
+    new_toolNo = newtool;
+}
+
+// state machine
+void zProbeSyncTask(void* pvParameters)
+{
+    TickType_t xLastWakeTime;
+
+    const TickType_t xProbeFrequency = 100; // in ticks (typically ms)
     xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time.
 
-    while (true) {
+    for( ;; )
-        // don't ever return from this or the task dies
+    {
-       
+        switch ( getState() )
-        memcpy(sys_POS_steps, sys_position, sizeof(sys_position));
-        memcpy(sys_PROBE_steps, sys_probe_position, sizeof(sys_probe_position));
-
-        system_convert_array_steps_to_mpos(sys_POS, sys_POS_steps);
-        system_convert_array_steps_to_mpos(sys_PROBEPOS, sys_PROBE_steps);
-
-        //sprintf(temp, "lprobegestate=%d\r\n", probe_get_state());
-        //grbl_send(CLIENT_ALL, temp);
-
-        if ( (probe_get_state()==1) )
         {
-            grbl_send(CLIENT_ALL, "Button pressed !");
+            //while running
 
-            sprintf(temp, "syspos/Z=%4.3f, probepos/Z=%4.3f\r\n", sys_POS[Z_AXIS], sys_PROBEPOS[Z_AXIS]);
+            case TOOLCHANGE_INIT:
+                #ifdef DEBUG
+                    grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_INIT. State=%d\r\n", getState());
+                #else  
+                    grbl_sendf (CLIENT_ALL, "Tool change procedure started.\r\n");
+                #endif
+    
+                // TODO set AmountOfToolChanges to 0 after job finish
+                AmountOfToolChanges++;             // Report tool change amount.
+
+                #ifdef DEBUG
+                    grbl_sendf (CLIENT_ALL, "This is the %d. tool change in this job\r\n", AmountOfToolChanges);
+                    grbl_sendf (CLIENT_ALL, "Old tool is #%d (0 means unknown), new tool is #%d\r\n", current_toolNo, new_toolNo);
+                #endif
+                
+                // Init. Safe start block. G54, XY plane, mm mode, relative addressing mode
+                inputBuffer.push("G49\r\n");
+                //inputBuffer.push("G54\r\n");
+                inputBuffer.push("G53\r\n");
+                inputBuffer.push("G17 G21 G90\r\n");
+                inputBuffer.push("G0 F100\r\n");
+
+                // Switch off spindle
+                inputBuffer.push("M05\r\n");
+
+                setState (TOOLCHANGE_START);
+                break;
+            
+            case TOOLCHANGE_START:
+                #ifdef DEBUG
+                    grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_START. State=%d\r\n", getState());
+                #endif
+
+                // nur beim ersten Mal die L<>nge des Fr<46>sers messen. Mit dem Wert alle Weiteren vergleichen und G43.2 berechnen.
+                if (AmountOfToolChanges == 1)
+                    setState(TOOLCHANGE_ZPROBE_1a);
+                else
+                    setState(TOOLCHANGE_MANUAL);
+                break;
+
+            // First Z Probe 
+            case TOOLCHANGE_ZPROBE_1a:
+                #ifdef DEBUG
+                    grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_1a. State=%d\r\n", getState());
+                #endif
+
+                // Place spindle directly above button in X/Y and a few mm above Z
+                inputBuffer.push("G53 G0 Z-5\r\n");
+                inputBuffer.push("G53 G0 X-29 Y-410\r\n");
+
+                // Z probe, max. 50mm to press button, quick
+                inputBuffer.push("G91 G38.2 Z-110 F500\r\n");
+                inputBuffer.push("G4 P1.0\r\n");
+
+                setState(TOOLCHANGE_ZPROBE_1b);
+                break;
+
+            case TOOLCHANGE_ZPROBE_1b: // wait for button press
+                #ifdef DEBUG
+                    // grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_1b. State=%d\r\n", getState());
+                #endif
+
+                // TODO Error handling. What happens in case the button is not pressed?
+                if ( probe_get_state() )
+                {
+
+                    // TODO neue Tooll<6C>nge immer gegen das erste Tool, richtig?
+                    if (AmountOfToolChanges == 1)
+                        firstZPos = getLastZProbePos(); // save Z pos for comparison later
+
+                    // hit the probe
+                    #ifdef DEBUG
+                        grbl_sendf(CLIENT_ALL, "Button pressed first time, quickly. Z probe pos=%4.3f\r\n", firstZPos);
+                    #endif
+
+                    inputBuffer.push("G91 G0 Z2\r\n");
+                    vTaskDelay (xProbeFrequency * 20); // wait until button is released
+                    setState (TOOLCHANGE_MANUAL);
+                }
+                break;
+
+            // go to manual tool change position
+            case TOOLCHANGE_MANUAL:
+                #ifdef DEBUG
+                    grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_MANUAL. State=%d\r\n", getState());
+                #endif
+
+                // Go to tool change position
+                inputBuffer.push("G53 G0 Z-5\r\n");
+                inputBuffer.push("G53 G0 X-5 Y-210\r\n");
+
+                // Hold
+                inputBuffer.push("M0\r\n");
+
+                setState (TOOLCHANGE_ZPROBE_2a);
+                break;
+
+            case TOOLCHANGE_ZPROBE_2a:
+                #ifdef DEBUG
+                    grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_2a. State=%d\r\n", getState());
+                #endif
+
+                // Place spindle directly above button in X/Y and a few mm above Z
+                inputBuffer.push("G53 G0 Z-5\r\n");
+                inputBuffer.push("G53 G0 X-29 Y-410\r\n");
+
+                // Z probe, max. 50mm to press button, quick
+                inputBuffer.push("G91 G38.2 Z-110 F500\r\n");
+                inputBuffer.push("G4 P2.0\r\n");
+
+                setState(TOOLCHANGE_ZPROBE_2b);
+                break;
+
+            case TOOLCHANGE_ZPROBE_2b: // wait for button press
+                #ifdef DEBUG
+                    // grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_2b. State=%d\r\n", getState());
+                #endif
+
+                // TODO Error handling. What happens in case the button is not pressed?
+                if ( probe_get_state() )
+                {
+                    newZPos = getLastZProbePos(); // save Z pos for comparison later
+
+                    // hit the probe
+                    #ifdef DEBUG
+                        grbl_sendf (CLIENT_ALL, "Button pressed second time, slowly. new Z probe pos=%4.3f\r\n", newZPos);
+                     #endif
+
+                    // send out G43.1 adjustment
+                    char gcode_line[20];
+
+                    sprintf(gcode_line, "G54 G43.1 Z%4.3f\r\n", newZPos-firstZPos);
+
+                    inputBuffer.push(gcode_line);
+                    grbl_sendf (CLIENT_ALL, gcode_line);
+
+                    inputBuffer.push("G91 G0 Z2\r\n");
+                    setState (TOOLCHANGE_FINISH);
+                }
+                break;
+
+            // That<61>s it
+            case TOOLCHANGE_FINISH:
+                #ifdef DEBUG
+                    grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_FINISH. State=%d\r\n", getState());
+                #endif
+
+                grbl_send (CLIENT_ALL, "Tool change procedure finished.\r\n");
+                grbl_send (CLIENT_ALL, "Go to G54 after hold.\r\n");
+                
+                // Hold
+                //inputBuffer.push("M0\r\n");
+
+                // Clean up
+                //inputBuffer.push("G90\r\n");
+
+                // Go to G54
+                // TODO G54 needs to be set before, otherwise bad things could happen
+                inputBuffer.push("G53 G0 Z-5\r\n");
+
+                inputBuffer.push("G54");
+                inputBuffer.push("G90");
+                inputBuffer.push("G0 X0 Y0\r\n");
+                inputBuffer.push("G0 Z0\r\n");
+
+                setState (TOOLCHANGE_IDLE);
+                break;
 
-            //sprintf(temp, "last probe/Z =%4.3f\r\n", ZPosNew);
-            grbl_send(CLIENT_ALL, temp);
         }
 
         vTaskDelayUntil(&xLastWakeTime, xProbeFrequency);
@@ -111,14 +295,7 @@ void zProbeSyncTask(void* pvParameters) {
 */
 void user_tool_change(uint8_t new_tool)
 {
-    // Variables
+    setState (TOOLCHANGE_INIT, new_tool); // let<65>s start with the state machine
-    char temp[200];
-    float value1, value2, diff;
-
-    // Start of function
-    // Increase AmountOfToolChanges every time by 1
-
-    AmountOfToolChanges++;
 
     /*
         Prerequisites
@@ -149,73 +326,41 @@ void user_tool_change(uint8_t new_tool)
         - 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();
-
-    // 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 X-29 Y-410\r\n");
-    inputBuffer.push("G53 G0 Z-60 \r\n");
-    protocol_buffer_synchronize();
-
-    // Z probe, max. 50mm to press button
-    inputBuffer.push("G38.2 Z-50 F250\r\n");
-    protocol_buffer_synchronize();
-
-    // 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");
-    inputBuffer.push("G4 P2.0\r\n");
-    protocol_buffer_synchronize(); // wait for all previous moves to complete
-
-    // 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
+#endif
+
+// return current Z machine position
+float getCurrentZPos()
+{
+    int32_t current_position[N_AXIS]; // copy of current location
+    float m_pos[N_AXIS];   // machine position in mm
+
+    memcpy(current_position, sys_position, sizeof(sys_position)); // get current position in step
+    system_convert_array_steps_to_mpos(m_pos, sys_position); // convert to millimeters
+
+    return m_pos[Z_AXIS];
+}
+
+// return last Z probe machine position
+float getLastZProbePos()
+{
+    int32_t lastZPosition[N_AXIS]; // copy of current location
+    float m_pos[N_AXIS];   // machine position in mm
+    char output[200];
+
+    memcpy(lastZPosition, sys_probe_position, sizeof(sys_probe_position)); // get current position in step
+    system_convert_array_steps_to_mpos(m_pos, lastZPosition); // convert to millimeters
+
+    return m_pos[Z_AXIS];
+}
+
+// return the stored G54/Z value
+float getG54Zvalue()
+{
+    float coord_data[N_AXIS];
+
+    settings_read_coord_data(0, coord_data); //0=G54
+
+    return coord_data[Z_AXIS];
+}

Grbl_Esp32/Machines/4axis_xyxz.h

@@ -4,7 +4,6 @@
  */
  
 #define MACHINE_NAME            "MACHINE_ESP32 Jens XYZA"
-#define ZPROBE_TASK_FREQ 1000 // this is milliseconds
 
 #ifdef N_AXIS
         #undef N_AXIS