diff --git a/Grbl_Esp32/Custom/atari_1020.cpp b/Grbl_Esp32/Custom/atari_1020.cpp
index 5661ae6d..cf0fd270 100644
--- a/Grbl_Esp32/Custom/atari_1020.cpp
+++ b/Grbl_Esp32/Custom/atari_1020.cpp
@@ -212,16 +212,16 @@ void calc_solenoid(float penZ) {
 	A tool (pen) change is done by bumping the carriage against the right edge 3 times per
 	position change. Pen 1-4 is valid range.
 */
-void user_tool_change(uint8_t new_tool) {
+bool user_tool_change(uint8_t new_tool) {
     uint8_t move_count;
     char    gcode_line[20];
     protocol_buffer_synchronize();  // wait for all previous moves to complete
     if ((new_tool < 1) || (new_tool > MAX_PEN_NUMBER)) {
         grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Requested Pen#%d is out of 1-4 range", new_tool);
-        return;
+        return false;
     }
     if (new_tool == current_tool)
-        return;
+        return true;
     if (new_tool > current_tool)
         move_count = BUMPS_PER_PEN_CHANGE * (new_tool - current_tool);
     else
@@ -235,6 +235,8 @@ void user_tool_change(uint8_t new_tool) {
     }
     current_tool = new_tool;
     grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Change to Pen#%d", current_tool);
+    
+    return true;
 }
 
 // move from current tool to next tool....
diff --git a/Grbl_Esp32/Custom/atc.cpp b/Grbl_Esp32/Custom/atc.cpp
index 32f13f09..30c87348 100644
--- a/Grbl_Esp32/Custom/atc.cpp
+++ b/Grbl_Esp32/Custom/atc.cpp
@@ -18,19 +18,19 @@
  If you zero a tool on the work piece, all tools will use the delta determined by the 
  toolsetter to.
 
- 
-
 */
 
-#define TOOL_GRAB_TIME 0.75  // seconds
+const int TOOL_COUNT = 4;
 
-// temporary struct....should be a class
+const float TOOL_GRAB_TIME = 0.75;  // seconds. How long it takes to grab a tool
+
+// temporary struct....should maybe be a class
 typedef struct {
     float mpos[MAX_N_AXIS];    // the pickup location in machine coords
     float offset[MAX_N_AXIS];  // the offset from the zero'd tool
 } tool_t;
 
-tool_t tool[5];  // one setter, plus 4 tools
+tool_t tool[TOOL_COUNT + 1];  // one setter, plus 4 tools
 
 void go_above_tool(uint8_t tool_num);
 void return_tool(uint8_t tool_num);
@@ -60,29 +60,36 @@ void machine_init() {
     tool[3].mpos[X_AXIS] = 95.0;
     tool[3].mpos[Y_AXIS] = 130.0;
     tool[3].mpos[Z_AXIS] = -20.0;
+
+    tool[4].mpos[X_AXIS] = 125.0;
+    tool[4].mpos[Y_AXIS] = 130.0;
+    tool[4].mpos[Z_AXIS] = -20.0;
 }
 
-/*
-
-*/
-void user_tool_change(uint8_t new_tool) {
+bool user_tool_change(uint8_t new_tool) {
     char     gcode_line[80];
-    bool     spindle_on = false;
-    uint64_t spindle_delay;
+    bool     spindle_was_on = false;
+    uint64_t spindle_spin_delay;            // milliseconds
     float    saved_mpos[MAX_N_AXIS] = {};
 
-    // save current position
-    system_convert_array_steps_to_mpos(saved_mpos, sys_position);
+    if (new_tool == current_tool)
+        return true;
 
-    protocol_buffer_synchronize();  // wait for all previous moves to complete
+    if (new_tool > TOOL_COUNT) {
+        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "ATC Tool out of range:%d", new_tool);
+        return false;
+    }
+
+    protocol_buffer_synchronize();  // wait for all previous moves to complete    
+    system_convert_array_steps_to_mpos(saved_mpos, sys_position);  // save current position so we can return
 
     // is spindle on? Turn it off and determine when the spin down should be done.
     if (gc_state.modal.spindle != SpindleState::Disable) {
-        spindle_on = true;
+        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);
-        spindle_delay = esp_timer_get_time() + (spindle_delay_spinup->get() * 1000.0);  // When has spindle stopped
+        spindle_spin_delay = esp_timer_get_time() + (spindle_delay_spindown->get() * 1000.0);  // When has spindle stopped
     }
 
     return_tool(current_tool);  // does nothing if current tool is 0
@@ -90,7 +97,7 @@ void user_tool_change(uint8_t new_tool) {
     // if changing to tool 0...we are done.
     if (new_tool == 0) {
         current_tool = new_tool;
-        return;
+        return true;
     }
 
     // TODO Check for G91...might not matter in G53
@@ -99,10 +106,10 @@ void user_tool_change(uint8_t 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_on) {
+    if (spindle_was_on) {
         uint64_t current_time = esp_timer_get_time();
-        if (current_time < spindle_delay) {
-            vTaskDelay(spindle_delay - current_time);
+        if (current_time < spindle_spin_delay) {
+            vTaskDelay(spindle_spin_delay - current_time);
         }
     }
 
@@ -134,7 +141,7 @@ void user_tool_change(uint8_t new_tool) {
     atc_tool_setter();
 
     // is spindle on?
-    if (spindle_on) {
+    if (spindle_was_on) {
         sprintf(gcode_line, "M3\r");  //
         gc_execute_line(gcode_line, CLIENT_INPUT);
     }
@@ -144,13 +151,8 @@ void user_tool_change(uint8_t new_tool) {
     gc_execute_line(gcode_line, CLIENT_INPUT);
 
     // TODO wait for spinup
-}
 
-// Polar coaster has macro buttons, this handles those button pushes.
-void user_defined_macro(uint8_t index) {}
-
-void user_m30() {
-    grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "ATC M30");
+    return true;
 }
 
 // ============= Local functions ==================
diff --git a/Grbl_Esp32/Custom/custom_code_template.cpp b/Grbl_Esp32/Custom/custom_code_template.cpp
index 0560de47..eda7e2c3 100644
--- a/Grbl_Esp32/Custom/custom_code_template.cpp
+++ b/Grbl_Esp32/Custom/custom_code_template.cpp
@@ -46,13 +46,11 @@ enabled with USE_ defines in Machines/my_machine.h
 
 */
 
-#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() {}
-#endif
 
 #ifdef USE_CUSTOM_HOMING
 /*
@@ -125,7 +123,7 @@ void forward_kinematics(float* position) {
   user_tool_change() is called when tool change gcode is received,
   to perform appropriate actions for your machine.
 */
-void user_tool_change(uint8_t new_tool) {}
+bool user_tool_change(uint8_t new_tool) {}
 
 #if defined(MACRO_BUTTON_0_PIN) || defined(MACRO_BUTTON_1_PIN) || defined(MACRO_BUTTON_2_PIN)
 /*
diff --git a/Grbl_Esp32/Custom/mpcnc_laser_module.cpp b/Grbl_Esp32/Custom/mpcnc_laser_module.cpp
index e65ce629..52b816c4 100644
--- a/Grbl_Esp32/Custom/mpcnc_laser_module.cpp
+++ b/Grbl_Esp32/Custom/mpcnc_laser_module.cpp
@@ -21,7 +21,6 @@
 	along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-#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.
@@ -32,4 +31,3 @@ void machine_init() {
     pinMode(LVL_SHIFT_ENABLE, OUTPUT);
     digitalWrite(LVL_SHIFT_ENABLE, HIGH);
 }
-#endif
diff --git a/Grbl_Esp32/src/Error.cpp b/Grbl_Esp32/src/Error.cpp
index 149b0665..4d5c7687 100644
--- a/Grbl_Esp32/src/Error.cpp
+++ b/Grbl_Esp32/src/Error.cpp
@@ -61,6 +61,7 @@ std::map<Error, const char*> ErrorNames = {
     { Error::GcodeG43DynamicAxisError, "Gcode G43 dynamic axis error" },
     { Error::GcodeMaxValueExceeded, "Gcode max value exceeded" },
     { Error::PParamMaxExceeded, "P param max exceeded" },
+    { Error::ToolChangeError, "Tool change failed" },
     { Error::SdFailedMount, "SD failed mount" },
     { Error::SdFailedRead, "SD failed read" },
     { Error::SdFailedOpenDir, "SD failed to open directory" },
diff --git a/Grbl_Esp32/src/Error.h b/Grbl_Esp32/src/Error.h
index 09a54030..da3acf38 100644
--- a/Grbl_Esp32/src/Error.h
+++ b/Grbl_Esp32/src/Error.h
@@ -64,6 +64,7 @@ enum class Error : uint8_t {
     GcodeG43DynamicAxisError    = 37,
     GcodeMaxValueExceeded       = 38,
     PParamMaxExceeded           = 39,
+    ToolChangeError             = 40,
     SdFailedMount               = 60,  // SD Failed to mount
     SdFailedRead                = 61,  // SD Failed to read file
     SdFailedOpenDir             = 62,  // SD card failed to open directory
diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp
index 324cec17..d780e092 100644
--- a/Grbl_Esp32/src/GCode.cpp
+++ b/Grbl_Esp32/src/GCode.cpp
@@ -1352,7 +1352,9 @@ Error gc_execute_line(char* line, uint8_t client) {
     //	gc_state.tool = gc_block.values.t;
     // [6. Change tool ]: NOT SUPPORTED
     if (gc_block.modal.tool_change == ToolChange::Enable) {
-        user_tool_change(gc_state.tool);  // (weak)   should be user defined
+        if (!user_tool_change(gc_state.tool)) {  // (weak)   should be user defined
+            FAIL(Error::ToolChangeError);
+        }
     }
     // [7. Spindle control ]:
     if (gc_state.modal.spindle != gc_block.modal.spindle) {
@@ -1599,7 +1601,7 @@ Error gc_execute_line(char* line, uint8_t client) {
     return Error::Ok;
 }
 
-__attribute__((weak)) void user_tool_change(uint8_t new_tool) {}
+__attribute__((weak)) bool user_tool_change(uint8_t new_tool) {}
 
 /*
   Not supported:
diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp
index 7a29bc4f..ef8e98a5 100644
--- a/Grbl_Esp32/src/Grbl.cpp
+++ b/Grbl_Esp32/src/Grbl.cpp
@@ -43,9 +43,8 @@ void grbl_init() {
     system_ini();  // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files)
     memset(sys_position, 0, sizeof(sys_position));  // Clear machine position.
 
-#ifdef USE_MACHINE_INIT
-    machine_init();  // user supplied function for special initialization
-#endif
+    machine_init();  // (weak) should be user defined
+
     // Initialize system state.
 #ifdef FORCE_INITIALIZATION_ALARM
     // Force Grbl into an ALARM state upon a power-cycle or hard reset.
@@ -117,6 +116,8 @@ void run_once() {
     protocol_main_loop();
 }
 
+__attribute__((weak)) void machine_init() {}
+
 /*
   setup() and loop() in the Arduino .ino implements this control flow:
 
diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h
index 9d3d7d7e..7b01498e 100644
--- a/Grbl_Esp32/src/Grbl.h
+++ b/Grbl_Esp32/src/Grbl.h
@@ -92,7 +92,6 @@ const char* const GRBL_VERSION_BUILD = "20201101";
 void grbl_init();
 void run_once();
 
-// Called if USE_MACHINE_INIT is defined
 void machine_init();
 
 // Called if USE_CUSTOM_HOMING is defined
@@ -115,4 +114,4 @@ void user_defined_macro(uint8_t index);
 // Called if USE_M30 is defined
 void user_m30();
 
-void user_tool_change(uint8_t new_tool);  // weak
+bool user_tool_change(uint8_t new_tool);  // weak
diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_ATC.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_ATC.h
index 22df5b29..bd9701ee 100644
--- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_ATC.h
+++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_ATC.h
@@ -28,9 +28,6 @@
 
 #define N_AXIS 3
 
-#define USE_MACHINE_INIT
-#define USE_M30 // use the user defined end of program
-
 // === Special Features
 
 // I2S (steppers & other output-only pins)
diff --git a/Grbl_Esp32/src/Machines/atari_1020.h b/Grbl_Esp32/src/Machines/atari_1020.h
index c1aafb33..1c225c16 100644
--- a/Grbl_Esp32/src/Machines/atari_1020.h
+++ b/Grbl_Esp32/src/Machines/atari_1020.h
@@ -140,7 +140,6 @@
 #define ATARI_HOMING_ATTEMPTS 13
 
 // tells grbl we have some special functions to call
-#define USE_MACHINE_INIT
 #define USE_CUSTOM_HOMING
 #define ATARI_TOOL_CHANGE_Z 5.0
 #define USE_M30 // use the user defined end of program
diff --git a/Grbl_Esp32/src/Machines/midtbot.h b/Grbl_Esp32/src/Machines/midtbot.h
index 41e35c2a..2fd6991e 100644
--- a/Grbl_Esp32/src/Machines/midtbot.h
+++ b/Grbl_Esp32/src/Machines/midtbot.h
@@ -32,7 +32,6 @@
 #define MIDTBOT             // applies the geometry correction to the kinematics 
 #define USE_KINEMATICS      // there are kinematic equations for this machine
 #define USE_FWD_KINEMATICS  // report in cartesian
-#define USE_MACHINE_INIT    // There is some custom initialization for this machine
 #define USE_CUSTOM_HOMING
 
 #define SPINDLE_TYPE    SpindleType::NONE
diff --git a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h
index f7b02d29..0d78c90d 100644
--- a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h
+++ b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h
@@ -30,7 +30,6 @@
 #define MACHINE_NAME "MPCNC_V1P2 with Laser Module"
 
 // The laser module fires without a low signal. This keeps the enable on
-#define USE_MACHINE_INIT
 #define LVL_SHIFT_ENABLE        GPIO_NUM_32
 #define CUSTOM_CODE_FILENAME    "Custom/mpcnc_laser_module.cpp"
 
diff --git a/Grbl_Esp32/src/Machines/tapster_3.h b/Grbl_Esp32/src/Machines/tapster_3.h
index b4de2b1c..5896f682 100644
--- a/Grbl_Esp32/src/Machines/tapster_3.h
+++ b/Grbl_Esp32/src/Machines/tapster_3.h
@@ -27,7 +27,6 @@
 
 #define USE_KINEMATICS				// there are kinematic equations for this machine
 #define USE_FWD_KINEMATICS          // report in cartesian
-#define USE_MACHINE_INIT			// There is some custom initialization for this machine
 	
 // ================== Delta Geometry ===========================
 
diff --git a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h
index 6f88a816..88975f93 100644
--- a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h
+++ b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h
@@ -27,7 +27,6 @@
 #define USE_RMT_STEPS              // Use the RMT periferal to generate step pulses
 #define USE_TRINAMIC               // some Trinamic motors are used on this machine
 #define USE_MACHINE_TRINAMIC_INIT  // there is a machine specific setup for the drivers
-#define USE_MACHINE_INIT           // There is some custom initialization for this machine
 
 #define SEGMENT_LENGTH 0.5  // segment length in mm
 #define KIN_ANGLE_CALC_OK 0
@@ -43,7 +42,6 @@
 
 #define USE_KINEMATICS      // there are kinematic equations for this machine
 #define USE_FWD_KINEMATICS  // report in cartesian
-#define USE_MACHINE_INIT    // There is some custom initialization for this machine
 
 // ================== Delta Geometry ===========================