bdring
diff --git a/‎Grbl_Esp32/Custom/CoreXY.cpp‎
Lines changed: 52 additions & 19 deletions b/‎Grbl_Esp32/Custom/CoreXY.cpp‎
Lines changed: 52 additions & 19 deletions
diff --git a/‎Grbl_Esp32/src/Config.h‎
Lines changed: 2 additions & 7 deletions b/‎Grbl_Esp32/src/Config.h‎
Lines changed: 2 additions & 7 deletions
diff --git a/‎Grbl_Esp32/src/Defaults.h‎
Lines changed: 8 additions & 0 deletions b/‎Grbl_Esp32/src/Defaults.h‎
Lines changed: 8 additions & 0 deletions
diff --git a/‎Grbl_Esp32/src/Grbl.cpp‎
Lines changed: 4 additions & 5 deletions b/‎Grbl_Esp32/src/Grbl.cpp‎
Lines changed: 4 additions & 5 deletions
diff --git a/‎Grbl_Esp32/src/Grbl.h‎
Lines changed: 3 additions & 5 deletions b/‎Grbl_Esp32/src/Grbl.h‎
Lines changed: 3 additions & 5 deletions
diff --git a/‎Grbl_Esp32/src/Limits.cpp‎
Lines changed: 4 additions & 0 deletions b/‎Grbl_Esp32/src/Limits.cpp‎
Lines changed: 4 additions & 0 deletions
diff --git a/‎Grbl_Esp32/src/MotionControl.cpp‎
Lines changed: 8 additions & 6 deletions b/‎Grbl_Esp32/src/MotionControl.cpp‎
Lines changed: 8 additions & 6 deletions
diff --git a/‎Grbl_Esp32/src/Motors/Motors.cpp‎
Lines changed: 25 additions & 5 deletions b/‎Grbl_Esp32/src/Motors/Motors.cpp‎
Lines changed: 25 additions & 5 deletions
diff --git a/‎Grbl_Esp32/src/NutsBolts.cpp‎
Lines changed: 3 additions & 3 deletions b/‎Grbl_Esp32/src/NutsBolts.cpp‎
Lines changed: 3 additions & 3 deletions
diff --git a/‎Grbl_Esp32/src/NutsBolts.h‎
Lines changed: 6 additions & 3 deletions b/‎Grbl_Esp32/src/NutsBolts.h‎
Lines changed: 6 additions & 3 deletions
@@ -69,7 +69,8 @@ bool user_defined_homing(uint8_t cycle_mask) {
 
     // check for multi axis homing per cycle ($Homing/Cycle0=XY type)...not allowed in CoreXY
     bool setting_error = false;
-    for (int cycle = 0; cycle < 3; cycle++) {
+    auto n_axis        = number_axis->get();
+    for (int cycle = 0; cycle < n_axis; cycle++) {
         if (numberOfSetBits(homing_cycle[cycle]->get()) > 1) {
             grbl_msg_sendf(CLIENT_SERIAL,
                            MsgLevel::Info,
@@ -90,7 +91,7 @@ bool user_defined_homing(uint8_t cycle_mask) {
     pl_data->motion.systemMotion   = 1;
     pl_data->motion.noFeedOverride = 1;
 
-    uint8_t cycle_count = (cycle_mask == 0) ? 3 : 1;  // if we have a cycle_mask, we are only going to do one axis
+    uint8_t cycle_count = (cycle_mask == 0) ? n_axis : 1;  // if we have a cycle_mask, we are only going to do one axis
 
     AxisMask mask = 0;
     for (int cycle = 0; cycle < cycle_count; cycle++) {
@@ -105,7 +106,7 @@ bool user_defined_homing(uint8_t cycle_mask) {
 
         mask = motors_set_homing_mode(mask, true);  // non standard homing motors will do their own thing and get removed from the mask
 
-        for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) {
+        for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) {
             if (bit(axis) == mask) {
                 // setup for the homing of this axis
                 bool  approach       = true;
@@ -129,7 +130,9 @@ bool user_defined_homing(uint8_t cycle_mask) {
                         approach ? target[axis] = max_travel : target[axis] = -max_travel;
                     }
 
-                    target[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS);
+                    for (int axis = Z_AXIS; axis < n_axis; axis++) {
+                        target[axis] = system_convert_axis_steps_to_mpos(sys_position, axis);
+                    }
 
                     // convert back to motor steps
                     inverse_kinematics(target);
@@ -217,7 +220,10 @@ bool user_defined_homing(uint8_t cycle_mask) {
 
     last_cartesian[X_AXIS] = target[X_AXIS];
     last_cartesian[Y_AXIS] = target[Y_AXIS];
-    last_cartesian[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS);
+
+    for (int axis = Z_AXIS; axis < n_axis; axis++) {
+        last_cartesian[axis] = system_convert_axis_steps_to_mpos(sys_position, axis);
+    }
 
     // convert to motors
     inverse_kinematics(target);
@@ -239,15 +245,15 @@ bool user_defined_homing(uint8_t cycle_mask) {
 // This function is used by Grbl convert Cartesian to motors
 // this does not do any motion control
 void inverse_kinematics(float* position) {
-    float motors[3];
+    float motors[MAX_N_AXIS];
 
     motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS];
     motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS];
-    motors[Z_AXIS] = position[Z_AXIS];
 
-    position[0] = motors[0];
-    position[1] = motors[1];
-    position[2] = motors[2];
+    position[X_AXIS] = motors[X_AXIS];
+    position[Y_AXIS] = motors[Y_AXIS];
+
+    // Z and higher just pass through unchanged
 }
 
 // Inverse Kinematics calculates motor positions from real world cartesian positions
@@ -268,7 +274,11 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
 
     motors[X_AXIS] = geometry_factor * target[X_AXIS] + target[Y_AXIS];
     motors[Y_AXIS] = geometry_factor * target[X_AXIS] - target[Y_AXIS];
-    motors[Z_AXIS] = target[Z_AXIS];
+
+    auto n_axis = number_axis->get();
+    for (uint8_t axis = Z_AXIS; axis <= n_axis; axis++) {
+        motors[axis] = target[axis];
+    }
 
     float motor_distance = three_axis_dist(motors, last_motors);
 
@@ -283,24 +293,47 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
 
 // motors -> cartesian
 void forward_kinematics(float* position) {
-    float calc_fwd[MAX_N_AXIS];
+    float   calc_fwd[MAX_N_AXIS];
+    float   wco[MAX_N_AXIS];
+    float   print_position[N_AXIS];
+    int32_t current_position[N_AXIS];  // Copy current state of the system position variable
+
+    memcpy(current_position, sys_position, sizeof(sys_position));
+    system_convert_array_steps_to_mpos(print_position, current_position);
+
+    // determine the Work Coordinate Offsets for each axis
+    auto n_axis = number_axis->get();
+    for (int axis = 0; axis < n_axis; axis++) {
+        // Apply work coordinate offsets and tool length offset to current position.
+        wco[axis] = gc_state.coord_system[axis] + gc_state.coord_offset[axis];
+        if (axis == TOOL_LENGTH_OFFSET_AXIS) {
+            wco[axis] += gc_state.tool_length_offset;
+        }
+    }
 
+    // apply the forward kinemetics to the machine coordinates
     // https://corexy.com/theory.html
-    calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]);
-    calc_fwd[Y_AXIS] = 0.5 * (position[X_AXIS] - position[Y_AXIS]);
+    //calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]);
+    calc_fwd[X_AXIS] = ((0.5 * (print_position[X_AXIS] + print_position[Y_AXIS]) * geometry_factor) - wco[X_AXIS]);
+    calc_fwd[Y_AXIS] = ((0.5 * (print_position[X_AXIS] - print_position[Y_AXIS])) - wco[Y_AXIS]);
 
-    position[X_AXIS] = calc_fwd[X_AXIS];
-    position[Y_AXIS] = calc_fwd[Y_AXIS];
+    for (int axis = 0; axis < n_axis; axis++) {
+        if (axis > Y_AXIS) {  // for axes above Y there is no kinematics
+            calc_fwd[axis] = print_position[axis] - wco[axis];
+        }
+        position[axis] = calc_fwd[axis];  // this is the value returned to reporting
+    }
 }
 
 bool kinematics_pre_homing(uint8_t cycle_mask) {
     return false;
 }
 
 void kinematics_post_homing() {
-    gc_state.position[X_AXIS] = last_cartesian[X_AXIS];
-    gc_state.position[Y_AXIS] = last_cartesian[Y_AXIS];
-    gc_state.position[Z_AXIS] = last_cartesian[Z_AXIS];
+    auto n_axis = number_axis->get();
+    for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) {
+        gc_state.position[axis] = last_cartesian[axis];
+    }
 }
 
 // this is used used by Grbl soft limits to see if the range of the machine is exceeded.
 
@@ -258,11 +258,6 @@ static const uint8_t NHomingLocateCycle = 1;  // Integer (1-128)
 // previous tool path, as if nothing happened.
 #define ENABLE_SAFETY_DOOR_INPUT_PIN  // ESP32 Leave this enabled for now .. code for undefined not ready
 
-// After the safety door switch has been toggled and restored, this setting sets the power-up delay
-// between restoring the spindle and coolant and resuming the cycle.
-const double SAFETY_DOOR_SPINDLE_DELAY = 4.0;  // Float (seconds)
-const double SAFETY_DOOR_COOLANT_DELAY = 1.0;  // Float (seconds)
-
 // Inverts select limit pin states based on the following mask. This effects all limit pin functions,
 // such as hard limits and homing. However, this is different from overall invert limits setting.
 // This build option will invert only the limit pins defined here, and then the invert limits setting
@@ -582,8 +577,8 @@ const int DEBOUNCE_PERIOD = 32;  // in milliseconds default 32 microseconds
 // Configure options for the parking motion, if enabled.
 #define PARKING_AXIS Z_AXIS                      // Define which axis that performs the parking motion
 const double PARKING_TARGET            = -5.0;   // Parking axis target. In mm, as machine coordinate.
-const double PARKING_RATE              = 500.0;  // Parking fast rate after pull-out in mm/min.
-const double PARKING_PULLOUT_RATE      = 100.0;  // Pull-out/plunge slow feed rate in mm/min.
+const double PARKING_RATE              = 800.0;  // Parking fast rate after pull-out in mm/min.
+const double PARKING_PULLOUT_RATE      = 250.0;  // Pull-out/plunge slow feed rate in mm/min.
 const double PARKING_PULLOUT_INCREMENT = 5.0;    // Spindle pull-out and plunge distance in mm. Incremental distance.
 // Must be positive value or equal to zero.
 
 
@@ -42,6 +42,10 @@
 #    define DEFAULT_STEP_PULSE_MICROSECONDS 3  // $0
 #endif
 
+#ifndef DEFAULT_STEP_ENABLE_DELAY
+#    define DEFAULT_STEP_ENABLE_DELAY 0
+#endif
+
 #ifndef DEFAULT_STEPPER_IDLE_LOCK_TIME
 #    define DEFAULT_STEPPER_IDLE_LOCK_TIME 250  // $1 msec (0-254, 255 keeps steppers enabled)
 #endif
@@ -187,6 +191,10 @@
 #    define DEFAULT_SPINDLE_DELAY_SPINUP 0
 #endif
 
+#ifndef DEFAULT_COOLANT_DELAY_TURNON
+#    define DEFAULT_COOLANT_DELAY_TURNON 1.0
+#endif
+
 #ifndef DEFAULT_SPINDLE_DELAY_SPINDOWN
 #    define DEFAULT_SPINDLE_DELAY_SPINDOWN 0
 #endif
 
@@ -39,13 +39,10 @@ void grbl_init() {
 #endif
     settings_init();  // Load Grbl settings from non-volatile storage
     stepper_init();   // Configure stepper pins and interrupt timers
-    init_motors();
     system_ini();  // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files)
+    init_motors();
     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 definition in Grbl.cpp does nothing
     // Initialize system state.
 #ifdef FORCE_INITIALIZATION_ALARM
     // Force Grbl into an ALARM state upon a power-cycle or hard reset.
@@ -117,6 +114,8 @@ void run_once() {
     protocol_main_loop();
 }
 
+void __attribute__((weak)) machine_init() {}
+
 /*
   setup() and loop() in the Arduino .ino implements this control flow:
 
 
@@ -21,9 +21,8 @@
 */
 
 // Grbl versioning system
-
 const char* const GRBL_VERSION       = "1.3a";
-const char* const GRBL_VERSION_BUILD = "20210203";
+const char* const GRBL_VERSION_BUILD = "20210218";
 
 //#include <sdkconfig.h>
 #include <Arduino.h>
@@ -94,8 +93,7 @@ void run_once();
 // Called if USE_MACHINE_INIT is defined
 void machine_init();
 
-// Called if USE_CUSTOM_HOMING is defined
-bool user_defined_homing(uint8_t cycle_mask);
+bool user_defined_homing(uint8_t cycle_mask);  // weak definition in Limits.cpp
 
 // Called if USE_KINEMATICS is defined
 
@@ -106,7 +104,7 @@ uint8_t kinematic_limits_check(float* target);
 
 // Called if USE_FWD_KINEMATICS is defined
 void inverse_kinematics(float* position);  // used to return a converted value
-void forward_kinematics(float* position);
+void forward_kinematics(float* position);  // weak definition in Report.cpp
 
 // Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined
 void user_defined_macro(uint8_t index);
 
@@ -404,3 +404,7 @@ bool limitsCheckTravel(float* target) {
     }
     return false;
 }
+
+bool __attribute__((weak)) user_defined_homing(uint8_t cycle_mask) {
+    return false;
+}
@@ -184,8 +184,9 @@ void mc_arc(float*            target,
         float    cos_Ti;
         float    r_axisi;
         uint16_t i;
-        uint8_t  count = 0;
-        for (i = 1; i < segments; i++) {  // Increment (segments-1).
+        uint8_t  count             = 0;
+        float    original_feedrate = pl_data->feed_rate;  // Kinematics may alter the feedrate, so save an original copy
+        for (i = 1; i < segments; i++) {                  // Increment (segments-1).
             if (count < N_ARC_CORRECTION) {
                 // Apply vector rotation matrix. ~40 usec
                 r_axisi = r_axis0 * sin_T + r_axis1 * cos_T;
@@ -206,6 +207,7 @@ void mc_arc(float*            target,
             position[axis_1] = center_axis1 + r_axis1;
             position[axis_linear] += linear_per_segment;
 #ifdef USE_KINEMATICS
+            pl_data->feed_rate = original_feedrate;  // This restores the feedrate kinematics may have altered
             mc_line_kins(position, pl_data, previous_position);
             previous_position[axis_0]      = position[axis_0];
             previous_position[axis_1]      = position[axis_1];
@@ -225,11 +227,11 @@ void mc_arc(float*            target,
 
 // Execute dwell in seconds.
 void mc_dwell(float seconds) {
-    if (sys.state == State::CheckMode) {
+    if (seconds == 0 || sys.state == State::CheckMode) {
         return;
     }
     protocol_buffer_synchronize();
-    delay_sec(seconds, DELAY_MODE_DWELL);
+    delay_sec(seconds, DwellMode::Dwell);
 }
 
 // return true if the mask has exactly one bit set,
@@ -283,11 +285,11 @@ static bool axis_is_squared(uint8_t axis_mask) {
 // executing the homing cycle. This prevents incorrect buffered plans after homing.
 void mc_homing_cycle(uint8_t cycle_mask) {
     bool no_cycles_defined = true;
-#ifdef USE_CUSTOM_HOMING
+
     if (user_defined_homing(cycle_mask)) {
         return;
     }
-#endif
+
     // This give kinematics a chance to do something before normal homing
     // if it returns true, the homing is canceled.
 #ifdef USE_KINEMATICS
 
@@ -413,7 +413,19 @@ void           init_motors() {
 }
 
 void motors_set_disable(bool disable, uint8_t mask) {
-    static bool previous_state = true;
+    static bool    prev_disable = true;
+    static uint8_t prev_mask    = 0;
+
+    if ((disable == prev_disable) && (mask == prev_mask)) {
+        return;
+    }
+
+    prev_disable = disable;
+    prev_mask    = mask;
+
+    if (step_enable_invert->get()) {
+        disable = !disable;  // Apply pin invert.
+    }
 
     // now loop through all the motors to see if they can individually disable
     auto n_axis = number_axis->get();
@@ -425,11 +437,19 @@ void motors_set_disable(bool disable, uint8_t mask) {
         }
     }
 
-    // invert only inverts the global stepper disable pin.
-    if (step_enable_invert->get()) {
-        disable = !disable;  // Apply pin invert.
-    }
+    // global disable.
     digitalWrite(STEPPERS_DISABLE_PIN, disable);
+
+    // Add an optional delay for stepper drivers. that need time
+    // Some need time after the enable before they can step.
+    auto wait_disable_change = enable_delay_microseconds->get();
+    if (wait_disable_change != 0) {
+        auto disable_start_time = esp_timer_get_time() + wait_disable_change;
+
+        while ((esp_timer_get_time() - disable_start_time) < 0) {
+            NOP();
+        }
+    }
 }
 
 void motors_read_settings() {
 
@@ -112,15 +112,15 @@ void delay_ms(uint16_t ms) {
 }
 
 // Non-blocking delay function used for general operation and suspend features.
-void delay_sec(float seconds, uint8_t mode) {
+void delay_sec(float seconds, DwellMode mode) {
     uint16_t i = ceil(1000 / DWELL_TIME_STEP * seconds);
     while (i-- > 0) {
         if (sys.abort) {
             return;
         }
-        if (mode == DELAY_MODE_DWELL) {
+        if (mode == DwellMode::Dwell) {
             protocol_execute_realtime();
-        } else {  // DELAY_MODE_SYS_SUSPEND
+        } else {  // DwellMode::SysSuspend
             // Execute rt_system() only to avoid nesting suspend loops.
             protocol_exec_rt_system();
             if (sys.suspend.bit.restartRetract) {
 
@@ -25,6 +25,11 @@
 // #define false 0
 // #define true 1
 
+enum class DwellMode : uint8_t {
+    Dwell = 0,   // (Default: Must be zero)
+    SysSuspend = 1,  //G92.1 (Do not alter value)
+};
+
 const double SOME_LARGE_VALUE = 1.0E+38;
 
 // Axis array index values. Must start with 0 and be continuous.
@@ -57,8 +62,6 @@ static inline int toMotor2(int axis) {
 const double MM_PER_INCH = (25.40);
 const double INCH_PER_MM = (0.0393701);
 
-const int DELAY_MODE_DWELL       = 0;
-const int DELAY_MODE_SYS_SUSPEND = 1;
 
 // Useful macros
 #define clear_vector(a) memset(a, 0, sizeof(a))
@@ -87,7 +90,7 @@ const int DELAY_MODE_SYS_SUSPEND = 1;
 uint8_t read_float(const char* line, uint8_t* char_counter, float* float_ptr);
 
 // Non-blocking delay function used for general operation and suspend features.
-void delay_sec(float seconds, uint8_t mode);
+void delay_sec(float seconds, DwellMode mode);
 
 // Delays variable-defined milliseconds. Compiler compatibility fix for _delay_ms().
 void delay_ms(uint16_t ms);
Original file line number	Diff line number	Diff line change
`@@ -404,3 +404,7 @@ bool limitsCheckTravel(float* target) {`
`404`	`404`	`}`
`405`	`405`	`return false;`
`406`	`406`	`}`
	`407`	`+`
	`408`	`+bool __attribute__((weak)) user_defined_homing(uint8_t cycle_mask) {`
	`409`	`+ return false;`
	`410`	`+}`