diff --git a/Grbl_Esp32/Custom/4axis_xyxz.cpp b/Grbl_Esp32/Custom/4axis_xyxz.cpp index 85166a59..1883dd99 100644 --- a/Grbl_Esp32/Custom/4axis_xyxz.cpp +++ b/Grbl_Esp32/Custom/4axis_xyxz.cpp @@ -12,18 +12,22 @@ // second Z probe after tool change. Now we can compare //TODO parameterise this + //TODO correct ifdef/ifndef debugging messages or info messages //vTaskDelay (0.5 / portTICK_RATE_MS); // 0.5 Sec. */ -#define DEBUG 0; // do I want debug messages? yes/no +#include + +//#define DEBUG; // do I want debug messages? yes/no /* VARIABLES */ uint8_t AmountOfToolChanges; // Each new tool increases this by 1. Before first tool, it´s 0. -uint8_t currenttoolNo; -float firstZPos, newZPos; +uint8_t currenttoolNo, newtoolNo; +float firstZPos, newZPos, Zdiff; +static TaskHandle_t zProbeSyncTaskHandle = NULL; // Finite state machine and sequence of steps uint8_t tc_state; // tool change (tc) state machine @@ -33,8 +37,8 @@ uint8_t tc_state; // tool change (tc) state machine #define TOOLCHANGE_ZPROBE_1a 3 // Z probe #1. Send order to press the Z probe button #define TOOLCHANGE_ZPROBE_1b 4 // Z probe #1. After button press #define TOOLCHANGE_MANUAL 5 // Go to tool change position -#define TOOLCHANGE_ZPROBE_2 6 // Z probe #2. Send order to press the Z probe button -#define TOOLCHANGE_FINISH 90 // tool change finish. do some reporting, clean up, etc. +#define TOOLCHANGE_ZPROBE_2 6 // Z probe #2. Send order to press the Z probe button +#define TOOLCHANGE_FINISH 99 // tool change finish. do some reporting, clean up, etc. // declare functions float getLastZProbePos(); @@ -56,57 +60,54 @@ void machine_init() // Initialize state machine tc_state=TOOLCHANGE_IDLE; -} + + // 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 -#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) +// state machine +void zProbeSyncTask(void* pvParameters) { - // let´s start with the state machine - tc_state = TOOLCHANGE_INIT; + TickType_t xLastWakeTime; - // TODO - // if (new_tool == current_tool) - // return; + const TickType_t xProbeFrequency = 100; // in ticks + xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. //protocol_buffer_synchronize(); // wait for all previous moves to complete - grbl_send (CLIENT_ALL, "we started\r\n"); - do + for ( ;; ) { switch ( tc_state ) { + case TOOLCHANGE_INIT: // TODO set AmountOfToolChanges to 0 after job finish // Set amount of tool changes AmountOfToolChanges++; #ifdef DEBUG - grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_INIT. State=%d\r\n", tc_state); - 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", currenttoolNo, new_tool); + grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_INIT. State=%d\r", tc_state); + grbl_sendf (CLIENT_ALL, "This is the %d. tool change in this job\r", AmountOfToolChanges); + grbl_sendf (CLIENT_ALL, "Old tool is #%d (0 means unknown), new tool is #%d\r", currenttoolNo, newtoolNo); #endif - // Init - inputBuffer.push("G90 G94\r\n"); - inputBuffer.push("G17\r\n"); - inputBuffer.push("G21\r\n"); - // Switch off spindle - inputBuffer.push("M05\r\n"); + inputBuffer.push("M05\r"); tc_state = TOOLCHANGE_START; break; case TOOLCHANGE_START: #ifdef DEBUG - grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_START. State=%d\r\n", tc_state); + grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_START. State=%d\r", tc_state); #endif // Measure firstZPos only once. Then adjust G43.2 by comparing firstZPos and newZPos. @@ -119,35 +120,39 @@ void user_tool_change(uint8_t new_tool) // First Z Probe case TOOLCHANGE_ZPROBE_1a: #ifdef DEBUG - grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_1a. State=%d\r\n", tc_state); + grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_1a. State=%d\r", tc_state); #endif // Place spindle directly above button in X/Y and at high Z - inputBuffer.push("G53 G0 Z-5\r\n"); - inputBuffer.push("G53 G0 X-29 Y-410\r\n"); + inputBuffer.push("G53 G0 Z-5\r"); + inputBuffer.push("G53 G0 X-29 Y-410\r"); // Z probe - inputBuffer.push("G38.2 Z-100 F500\r\n"); + inputBuffer.push("G91 G38.2 Z-100 F500\r"); tc_state = 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", tc_state); + grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_1b. State=%d\r", tc_state); #endif // wait until we hit Z probe button // TODO Error handling. What happens in case the button is not pressed? - if ( probe_get_state() ) + if ( probe_get_state() ) { + #ifndef DEBUG + grbl_sendf(CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_1b. State=%d\r", tc_state); + #endif + if (AmountOfToolChanges == 1) firstZPos = getLastZProbePos(); // save Z pos for comparison later // hit the probe - grbl_sendf(CLIENT_ALL, "Button pressed first time. Z probe pos=%4.3f\r\n", firstZPos); + grbl_sendf(CLIENT_ALL, "Button pressed first time. Z probe pos=%4.3f\r", firstZPos); - inputBuffer.push("G53 G0 Z-5\r\n"); + inputBuffer.push("G53 G0 Z-5\r"); tc_state = TOOLCHANGE_MANUAL; } @@ -156,23 +161,23 @@ void user_tool_change(uint8_t new_tool) // go to manual tool change position case TOOLCHANGE_MANUAL: #ifdef DEBUG - grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_MANUAL. State=%d\r\n", tc_state); + grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_MANUAL. State=%d\r", tc_state); #endif if ( !probe_get_state() ) // button released now { // Go to tool change position - inputBuffer.push("G53 G0 X-5 Y-210\r\n"); + inputBuffer.push("G53 G0 X-5 Y-210\r"); // Hold - inputBuffer.push("M0\r\n"); + inputBuffer.push("M0\r"); // 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"); + inputBuffer.push("G53 G0 Z-5\r"); + inputBuffer.push("G53 G0 X-29 Y-410\r"); // Z probe, max. 50mm to press button, quick - inputBuffer.push("G91 G38.2 Z-100 F500\r\n"); + inputBuffer.push("G91 G38.2 Z-100 F500\r"); tc_state = TOOLCHANGE_ZPROBE_2; } @@ -181,7 +186,7 @@ void user_tool_change(uint8_t new_tool) case TOOLCHANGE_ZPROBE_2: // wait for button press #ifdef DEBUG - // grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_2. State=%d\r\n", tc_state); + // grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_2. State=%d\r", tc_state); #endif // TODO Error handling. What happens in case the button is not pressed? @@ -191,17 +196,17 @@ void user_tool_change(uint8_t new_tool) // hit the probe #ifdef DEBUG - grbl_sendf (CLIENT_ALL, "Button pressed second time. new Z probe pos=%4.3f\r\n", newZPos); + grbl_sendf (CLIENT_ALL, "Button pressed second time. new Z probe pos=%4.3f\r", newZPos); #endif // calculate and send out G43.1 adjustment char gcode_line[20]; - sprintf(gcode_line, "G43.1 Z%4.3f\r\n", newZPos-firstZPos); + sprintf(gcode_line, "G43.1 Z%4.3f\r", newZPos-firstZPos); inputBuffer.push(gcode_line); grbl_sendf (CLIENT_ALL, gcode_line); // go up - inputBuffer.push("G53 G0 Z-5\r\n"); + inputBuffer.push("G53 G0 Z-5\r"); tc_state = TOOLCHANGE_FINISH; } @@ -210,7 +215,7 @@ void user_tool_change(uint8_t new_tool) // That´s it case TOOLCHANGE_FINISH: #ifdef DEBUG - grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_FINISH. State=%d\r\n", tc_state); + grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_FINISH. State=%d\r", tc_state); #endif // button released, we lift up @@ -218,21 +223,40 @@ void user_tool_change(uint8_t new_tool) { //vTaskDelay (1 / portTICK_RATE_MS); // 1 sec. - grbl_send (CLIENT_ALL, "Tool change procedure finished.\r\n"); - grbl_send (CLIENT_ALL, "Go to current WCS origin after hold.\r\n"); + grbl_send (CLIENT_ALL, "Tool change procedure finished.\r"); + grbl_send (CLIENT_ALL, "Go to current WCS origin after hold.\r"); // go to current WCS origin. This could be G54, but also another one - inputBuffer.push("G0 X0 Y0\r\n"); - inputBuffer.push("G0 Z0\r\n"); + inputBuffer.push("G0 X0 Y0\r"); + inputBuffer.push("G0 Z0\r"); } tc_state = TOOLCHANGE_IDLE; break; } - - } while (tc_state != TOOLCHANGE_IDLE); + vTaskDelayUntil(&xLastWakeTime, xProbeFrequency); + } +} +#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) +{ + // let´s start with the state machine + + newtoolNo = new_tool; + tc_state = TOOLCHANGE_INIT; + + //TODO + // Nach Aufruf dieser Function wird gleich wieder zurcügkegeben in die aufrufende Function. + // Ziel: Erst return, wenn wirklich beendet (RTOS!) + return; } #endif diff --git a/Grbl_Esp32/Machines/4axis_xyxz.h b/Grbl_Esp32/Machines/4axis_xyxz.h index 86b0170f..35021032 100644 --- a/Grbl_Esp32/Machines/4axis_xyxz.h +++ b/Grbl_Esp32/Machines/4axis_xyxz.h @@ -2,15 +2,15 @@ * 4 Achsen CNC Fräse von Jens * Infos zur Benutzung einer Spindel mit Relais https://github.com/bdring/Grbl_Esp32/wiki/Spindle-Types */ - + #define MACHINE_NAME "MACHINE_ESP32 Jens XYZA" #ifdef N_AXIS - #undef N_AXIS +#undef N_AXIS #endif -// Möchte ich mit 3 oder 4 Achsen arbeiten? -// Beides ist eingerichtet. + // Möchte ich mit 3 oder 4 Achsen arbeiten? + // Beides ist eingerichtet. #define N_AXIS 3 #define CUSTOM_CODE_FILENAME "Custom/4axis_xyxz.cpp" @@ -27,10 +27,10 @@ #define STEPPERS_DISABLE_PIN GPIO_NUM_13 //ok -// Falls die 4. Achse genutzt wird + // Falls die 4. Achse genutzt wird #if (N_AXIS == 4) - #define A_STEP_PIN GPIO_NUM_25 - #define A_DIRECTION_PIN GPIO_NUM_22 +#define A_STEP_PIN GPIO_NUM_25 +#define A_DIRECTION_PIN GPIO_NUM_22 #endif #define SPINDLE_TYPE SPINDLE_TYPE_RELAY @@ -48,13 +48,13 @@ #define Y_LIMIT_PIN GPIO_NUM_4 //ok #define Z_LIMIT_PIN GPIO_NUM_16 //ok -// Falls die 4. Achse genutzt wird + // Falls die 4. Achse genutzt wird #if (N_AXIS == 4) - #define A_LIMIT_PIN GPIO_NUM_21 //ok - #define LIMIT_MASK B1111 +#define A_LIMIT_PIN GPIO_NUM_21 //ok +#define LIMIT_MASK B1111 #else // bei 3 Achsen - #define LIMIT_MASK B111 +#define LIMIT_MASK B111 #endif /* @@ -63,7 +63,7 @@ #define PROBE_PIN GPIO_NUM_35 //ok #define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_36 // needs external pullup #define CONTROL_RESET_PIN GPIO_NUM_34 // needs external pullup -// #define CONTROL_FEED_HOLD_PIN GPIO_NUM_35 // needs external pullup + // #define CONTROL_FEED_HOLD_PIN GPIO_NUM_35 // needs external pullup #define CONTROL_CYCLE_START_PIN GPIO_NUM_39 // needs external pullup /* Normally Grbl_ESP32 ignores tool changes. @@ -72,5 +72,8 @@ * it will call a function void user_tool_change(uint8_t new_tool) when it sees the M6 gcode command. */ + #define USE_MACHINE_INIT -#define USE_TOOL_CHANGE \ No newline at end of file +#define USE_TOOL_CHANGE + +void zProbeSyncTask(void* pvParameters); \ No newline at end of file