diff --git a/Grbl_Esp32/Custom/atc.cpp b/Grbl_Esp32/Custom/atc.cpp index 2880fba1..0d446b50 100644 --- a/Grbl_Esp32/Custom/atc.cpp +++ b/Grbl_Esp32/Custom/atc.cpp @@ -20,8 +20,7 @@ */ -const int TOOL_COUNT = 4; - +const int TOOL_COUNT = 4; const float TOOL_GRAB_TIME = 0.75; // seconds. How long it takes to grab a tool // temporary struct....should maybe be a class @@ -36,7 +35,7 @@ void go_above_tool(uint8_t tool_num); void return_tool(uint8_t tool_num); void atc_tool_setter(); bool set_ATC_open(bool open); -void gc_exec_linef(const char* format, bool sync_before, ...); +void gc_exec_linef(const char* format, bool sync_after, ...); uint8_t current_tool = 0; @@ -68,7 +67,6 @@ void machine_init() { } bool user_tool_change(uint8_t new_tool) { - char gcode_line[80]; bool spindle_was_on = false; uint64_t spindle_spin_delay; // milliseconds float saved_mpos[MAX_N_AXIS] = {}; @@ -87,9 +85,7 @@ bool user_tool_change(uint8_t new_tool) { // is spindle on? Turn it off and determine when the spin down should be done. if (gc_state.modal.spindle != SpindleState::Disable) { spindle_was_on = true; - sprintf(gcode_line, "M5\r"); // - gc_execute_line(gcode_line, CLIENT_INPUT); - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "ATC: %s", gcode_line); + gc_exec_linef("M5", false); spindle_spin_delay = esp_timer_get_time() + (spindle_delay_spindown->get() * 1000.0); // When has spindle stopped } @@ -104,7 +100,6 @@ bool user_tool_change(uint8_t new_tool) { // TODO Check for G91...might not matter in G53 go_above_tool(new_tool); - protocol_buffer_synchronize(); // wait for motion to complete // if spindle was on has the spindle down period completed? If not wait. if (spindle_was_on) { @@ -114,42 +109,26 @@ bool user_tool_change(uint8_t new_tool) { } } - // open ATC - set_ATC_open(true); + set_ATC_open(true); // open ATC - // drop down to tool - sprintf(gcode_line, "G53G0Z%0.3f\r", tool[new_tool].mpos[Z_AXIS]); // - gc_execute_line(gcode_line, CLIENT_INPUT); - protocol_buffer_synchronize(); // wait for it + gc_exec_linef("G53G0Z%0.3f", true, tool[new_tool].mpos[Z_AXIS]); // drop down to tool - // Close ATC - set_ATC_open(false); + set_ATC_open(false); // Close ATC - // wait for grab to complete and settle - sprintf(gcode_line, "G4P%0.2f\r", TOOL_GRAB_TIME); // - gc_execute_line(gcode_line, CLIENT_INPUT); - - // raise to top of Z - sprintf(gcode_line, "G53G0Z-1\r"); // - gc_execute_line(gcode_line, CLIENT_INPUT); - - // move to X Y in front of tool - sprintf(gcode_line, "G53G0X%0.3fY%0.3f\r", tool[new_tool].mpos[X_AXIS], tool[new_tool].mpos[Y_AXIS] - 20.0); // - gc_execute_line(gcode_line, CLIENT_INPUT); + gc_exec_linef("G4P%0.2f", true, TOOL_GRAB_TIME); // wait for grab to complete and settle + gc_exec_linef("G53G0Z-1", false); // raise to top of Z + gc_exec_linef("G53G0X%0.3fY%0.3f", false, tool[new_tool].mpos[X_AXIS], tool[new_tool].mpos[Y_AXIS] - 20.0); // move to X Y in front of tool current_tool = new_tool; atc_tool_setter(); - // is spindle on? + // was spindle on? if (spindle_was_on) { - sprintf(gcode_line, "M3\r"); // - gc_execute_line(gcode_line, CLIENT_INPUT); + gc_exec_linef("M3", false); } - // return to saved mpos - sprintf(gcode_line, "G53G0X%0.3fY%0.3fZ%0.3f\r", saved_mpos[X_AXIS], saved_mpos[Y_AXIS], saved_mpos[Z_AXIS]); // - gc_execute_line(gcode_line, CLIENT_INPUT); + gc_exec_linef("G53G0X%0.3fY%0.3fZ%0.3f", false, , saved_mpos[X_AXIS], saved_mpos[Y_AXIS], saved_mpos[Z_AXIS]); // return to saved mpos // TODO wait for spinup @@ -159,83 +138,44 @@ bool user_tool_change(uint8_t new_tool) { // ============= Local functions ================== void go_above_tool(uint8_t tool_num) { - char gcode_line[80]; - - //sprintf(gcode_line, "G53G0Z-1"); - //gc_execute_line(gcode_line, CLIENT_INPUT); - gc_exec_linef("G53G0Z-1", false); // raise up gc_exec_linef("G53G0X%0.3fY%0.3f", false, tool[tool_num].mpos[X_AXIS], tool[tool_num].mpos[Y_AXIS] - 20.0); // move in front of tool - gc_exec_linef("G53G0Y%0.3f", false, tool[tool_num].mpos[Y_AXIS]); // Move over tool - - // move in front - //sprintf(gcode_line, "G53G0X%0.3fY%0.3f\r", tool[tool_num].mpos[X_AXIS], tool[tool_num].mpos[Y_AXIS] - 20.0); // - //gc_execute_line(gcode_line, CLIENT_INPUT); - - // Move over tool - // sprintf(gcode_line, "G53G0Y%0.3f\r", tool[tool_num].mpos[Y_AXIS]); // - // gc_execute_line(gcode_line, CLIENT_INPUT); + gc_exec_linef("G53G0Y%0.3f", true, tool[tool_num].mpos[Y_AXIS]); // Move over tool } void return_tool(uint8_t tool_num) { - char gcode_line[80]; - if (tool_num == 0) return; go_above_tool(tool_num); - // drop down to tool - sprintf(gcode_line, "G53G0Z%0.3f\r", tool[tool_num].mpos[Z_AXIS]); // - gc_execute_line(gcode_line, CLIENT_INPUT); + gc_exec_linef("G53G0Z%0.3f", true, tool[tool_num].mpos[Z_AXIS]); // drop down to tool + set_ATC_open(true); // open ATC + gc_exec_linef("G4P0.5", false); // wait + gc_exec_linef("G53G0Z-1", true); // raise up + set_ATC_open(false); // close ATC - // release tool - // open ATC - protocol_buffer_synchronize(); // wait for it - set_ATC_open(true); - - // wait - sprintf(gcode_line, "G4P0.5\r"); // - gc_execute_line(gcode_line, CLIENT_INPUT); - - // raise up - sprintf(gcode_line, "G53G0Z-1"); - gc_execute_line(gcode_line, CLIENT_INPUT); - - // close ATC - protocol_buffer_synchronize(); // wait for it - set_ATC_open(false); - - // move forward - sprintf(gcode_line, "G53G0X%0.3fY%0.3f\r", tool[tool_num].mpos[X_AXIS], tool[tool_num].mpos[Y_AXIS] - 20.0); // - gc_execute_line(gcode_line, CLIENT_INPUT); + gc_exec_linef("G53G0X%0.3fY%0.3f", true, tool[tool_num].mpos[X_AXIS], tool[tool_num].mpos[Y_AXIS] - 20.0); // move forward } void atc_tool_setter() { - char gcode_line[80]; float print_position[MAX_N_AXIS]; go_above_tool(0); - // probe - sprintf(gcode_line, "G38.2F%0.3fZ%0.3f\r", 300.0, -15); // - gc_execute_line(gcode_line, CLIENT_INPUT); - protocol_buffer_synchronize(); + // TODO Fast Probe...Slow Probe? - system_convert_array_steps_to_mpos(print_position, sys_probe_position); + gc_exec_linef("G38.2F%0.3fZ%0.3f", true, 300.0, -15); // probe // TODO was probe successful? - // Get the Z height ... - tool[current_tool].offset[Z_AXIS] = print_position[Z_AXIS]; + system_convert_array_steps_to_mpos(print_position, sys_probe_position); + tool[current_tool].offset[Z_AXIS] = print_position[Z_AXIS]; // Get the Z height ... - // raise up - sprintf(gcode_line, "G53G0Z-1"); - gc_execute_line(gcode_line, CLIENT_INPUT); + gc_exec_linef("G53G0Z-1", false); // raise up // move forward - sprintf(gcode_line, "G53G0X%0.3fY%0.3f\r", tool[0].mpos[X_AXIS], tool[0].mpos[Y_AXIS] - 20.0); // - gc_execute_line(gcode_line, CLIENT_INPUT); + gc_exec_linef("G53G0X%0.3fY%0.3f", false, tool[0].mpos[X_AXIS], tool[0].mpos[Y_AXIS] - 20.0); } bool set_ATC_open(bool open) { @@ -250,13 +190,10 @@ bool set_ATC_open(bool open) { /* Formats and sends a gcode line - char* format = a printf style string like "G0X%0.3fY0.3f" -bool sync_before. true = protocol_buffer_synchronize before gcode - */ -void gc_exec_linef(const char* format, bool sync_before, ...) { +void gc_exec_linef(const char* format, bool sync_after, ...) { char loc_buf[100]; char* temp = loc_buf; va_list arg; @@ -272,14 +209,14 @@ void gc_exec_linef(const char* format, bool sync_before, ...) { } } len = vsnprintf(temp, len + 1, format, arg); - // if (sync_before) { - // protocol_buffer_synchronize(); - // } gc_execute_line(temp, CLIENT_INPUT); - grbl_sendf(CLIENT_SERIAL, "[ATC GCode:%s]\r\n", temp); + //grbl_sendf(CLIENT_SERIAL, "[ATC GCode:%s]\r\n", temp); va_end(arg); if (temp != loc_buf) { delete[] temp; } + if (sync_after) { + protocol_buffer_synchronize(); + } }