diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp
index e98dfab9..9de92f49 100644
--- a/Grbl_Esp32/src/GCode.cpp
+++ b/Grbl_Esp32/src/GCode.cpp
@@ -1598,7 +1598,7 @@ Error gc_execute_line(char* line, uint8_t client) {
             if (sys.state != State::CheckMode) {
                 coords[gc_state.modal.coord_select]->get(gc_state.coord_system);
                 system_flag_wco_change();  // Set to refresh immediately just in case something altered.
-                config->_spindle->set_state(SpindleState::Disable, 0);
+                config->_spindle->spinDown();
 
                 config->_coolant->off();
             }
diff --git a/Grbl_Esp32/src/MachineConfig.cpp b/Grbl_Esp32/src/MachineConfig.cpp
index 364fe58c..99da84e9 100644
--- a/Grbl_Esp32/src/MachineConfig.cpp
+++ b/Grbl_Esp32/src/MachineConfig.cpp
@@ -200,7 +200,7 @@ uint8_t Axes::set_homing_mode(uint8_t homing_mask, bool isHoming) {
     return can_home;
 }
 
-void Axes::step(uint8_t step_mask, uint8_t dir_mask) {
+void IRAM_ATTR Axes::step(uint8_t step_mask, uint8_t dir_mask) {
     auto n_axis = _numberAxis;
     //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "motors_set_direction_pins:0x%02X", onMask);
 
@@ -238,7 +238,7 @@ void Axes::step(uint8_t step_mask, uint8_t dir_mask) {
     }
 }
 // Turn all stepper pins off
-void Axes::unstep() {
+void IRAM_ATTR Axes::unstep() {
     auto n_axis = _numberAxis;
     for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
         for (uint8_t gang_index = 0; gang_index < Axis::MAX_NUMBER_GANGED; gang_index++) {
diff --git a/Grbl_Esp32/src/Motors/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp
index e01742b1..376dcda0 100644
--- a/Grbl_Esp32/src/Motors/StandardStepper.cpp
+++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp
@@ -104,7 +104,7 @@ namespace Motors {
                        reportAxisLimitsMsg(axis_index()));
     }
 
-    void StandardStepper::step() {
+    void IRAM_ATTR StandardStepper::step() {
         if (config->_stepType == ST_RMT) {
             RMT.conf_ch[_rmt_chan_num].conf1.mem_rd_rst = 1;
             RMT.conf_ch[_rmt_chan_num].conf1.tx_start   = 1;
@@ -113,13 +113,13 @@ namespace Motors {
         }
     }
 
-    void StandardStepper::unstep() {
+    void IRAM_ATTR StandardStepper::unstep() {
         if (config->_stepType != ST_RMT) {
             _step_pin.off();
         }
     }
 
-    void StandardStepper::set_direction(bool dir) { _dir_pin.write(dir); }
+    void IRAM_ATTR StandardStepper::set_direction(bool dir) { _dir_pin.write(dir); }
 
     void StandardStepper::set_disable(bool disable) { _disable_pin.write(disable); }
 
diff --git a/Grbl_Esp32/src/Motors/UnipolarMotor.cpp b/Grbl_Esp32/src/Motors/UnipolarMotor.cpp
index 8528d9c7..4e83836b 100644
--- a/Grbl_Esp32/src/Motors/UnipolarMotor.cpp
+++ b/Grbl_Esp32/src/Motors/UnipolarMotor.cpp
@@ -32,9 +32,9 @@ namespace Motors {
         _enabled = !disable;
     }
 
-    void UnipolarMotor::set_direction(bool dir) { _dir = dir; }
+    void IRAM_ATTR UnipolarMotor::set_direction(bool dir) { _dir = dir; }
 
-    void UnipolarMotor::step() {
+    void IRAM_ATTR UnipolarMotor::step() {
         uint8_t _phase[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };  // temporary phase values...all start as off
         uint8_t phase_max;
 
diff --git a/Grbl_Esp32/src/NutsBolts.cpp b/Grbl_Esp32/src/NutsBolts.cpp
index dde77a92..d4e2ad0a 100644
--- a/Grbl_Esp32/src/NutsBolts.cpp
+++ b/Grbl_Esp32/src/NutsBolts.cpp
@@ -195,10 +195,6 @@ float map_float(float x, float in_min, float in_max, float out_min, float out_ma
     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
-uint32_t map_uint32_t(uint32_t x, uint32_t in_min, uint32_t in_max, uint32_t out_min, uint32_t out_max) {
-    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-}
-
 float constrain_float(float in, float min, float max) {  // DrawBot_Badge
     if (in < min) {
         return min;
diff --git a/Grbl_Esp32/src/NutsBolts.h b/Grbl_Esp32/src/NutsBolts.h
index ec134081..189b8492 100644
--- a/Grbl_Esp32/src/NutsBolts.h
+++ b/Grbl_Esp32/src/NutsBolts.h
@@ -105,12 +105,11 @@ float convert_delta_vector_to_unit_vector(float* vector);
 float limit_acceleration_by_axis_maximum(float* unit_vec);
 float limit_rate_by_axis_maximum(float* unit_vec);
 
-float    mapConstrain(float x, float in_min, float in_max, float out_min, float out_max);
-float    map_float(float x, float in_min, float in_max, float out_min, float out_max);
-uint32_t map_uint32_t(uint32_t x, uint32_t in_min, uint32_t in_max, uint32_t out_min, uint32_t out_max);
-float    constrain_float(float in, float min, float max);
-bool     char_is_numeric(char value);
-char*    trim(char* value);
+float mapConstrain(float x, float in_min, float in_max, float out_min, float out_max);
+float map_float(float x, float in_min, float in_max, float out_min, float out_max);
+float constrain_float(float in, float min, float max);
+bool  char_is_numeric(char value);
+char* trim(char* value);
 
 int numberOfSetBits(uint32_t i);
 
diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp
index 7a8cd8df..fe216a7d 100644
--- a/Grbl_Esp32/src/Protocol.cpp
+++ b/Grbl_Esp32/src/Protocol.cpp
@@ -603,7 +603,7 @@ static void protocol_execute_overrides() {
     if (sys_rt_exec_accessory_override.bit.spindleOvrStop) {
         sys_rt_exec_accessory_override.bit.spindleOvrStop = false;
         // Spindle stop override allowed only while in HOLD state.
-        // NOTE: Report counters are set in spindle_set_state() when spindle stop is executed.
+        // NOTE: Report counters are set in spindle->spinDown()
         if (sys.state == State::Hold) {
             if (sys.spindle_stop_ovr.value == 0) {
                 sys.spindle_stop_ovr.bit.initiate = true;
@@ -925,7 +925,7 @@ void protocol_exec_rt_system() {
 //    if (sys_rt_exec_accessory_override.bit.spindleOvrStop) {
 //        sys_rt_exec_accessory_override.bit.spindleOvrStop = false;
 //        // Spindle stop override allowed only while in HOLD state.
-//        // NOTE: Report counters are set in spindle_set_state() when spindle stop is executed.
+//        // NOTE: Report counters are set in spindle->spinDown()
 //        if (sys.state == State::Hold) {
 //            if (sys.spindle_stop_ovr.value == 0) {
 //                sys.spindle_stop_ovr.bit.initiate = true;
@@ -1039,7 +1039,7 @@ static void protocol_exec_rt_suspend() {
                     // Ensure any prior spindle stop override is disabled at start of safety door routine.
                     sys.spindle_stop_ovr.value = 0;  // Disable override
 #ifndef PARKING_ENABLE
-                    config->_spindle->set_state(SpindleState::Disable, 0);  // De-energize
+                    config->_spindle->spinDown();
                     config->_coolant->off();
 #else
                     // Get current position and store restore location and spindle retract waypoint.
@@ -1069,7 +1069,7 @@ static void protocol_exec_rt_suspend() {
                         pl_data->motion.systemMotion   = 1;
                         pl_data->motion.noFeedOverride = 1;
                         pl_data->spindle_speed         = 0.0;
-                        config->_spindle->set_state(pl_data->spindle, 0);  // De-energize
+                        config->_spindle->spinDown();
                         config->_coolant->set_state(pl_data->coolant);
                         // Execute fast parking retract motion to parking target location.
                         if (parking_target[PARKING_AXIS] < PARKING_TARGET) {
@@ -1080,7 +1080,7 @@ static void protocol_exec_rt_suspend() {
                     } else {
                         // Parking motion not possible. Just disable the spindle and coolant.
                         // NOTE: Laser mode does not start a parking motion to ensure the laser stops immediately.
-                        config->_spindle->set_state(SpindleState::Disable, 0);  // De-energize
+                        config->_spindle->spinDown();
                         config->_coolant->off();
                     }
 #endif
@@ -1090,7 +1090,7 @@ static void protocol_exec_rt_suspend() {
                     if (sys.state == State::Sleep) {
                         report_feedback_message(Message::SleepMode);
                         // Spindle and coolant should already be stopped, but do it again just to be sure.
-                        config->_spindle->set_state(SpindleState::Disable, 0);  // De-energize
+                        config->_spindle->spinDown();
                         config->_coolant->off();
                         st_go_idle();  // Disable steppers
                         while (!(sys.abort)) {
@@ -1166,7 +1166,7 @@ static void protocol_exec_rt_suspend() {
                     // Handles beginning of spindle stop
                     if (sys.spindle_stop_ovr.bit.initiate) {
                         if (gc_state.modal.spindle != SpindleState::Disable) {
-                            config->_spindle->set_state(SpindleState::Disable, 0);  // De-energize
+                            config->_spindle->spinDown();
                             sys.spindle_stop_ovr.value       = 0;
                             sys.spindle_stop_ovr.bit.enabled = true;  // Set stop override state to enabled, if de-energized.
                         } else {
diff --git a/Grbl_Esp32/src/Spindles/10vSpindle.cpp b/Grbl_Esp32/src/Spindles/10vSpindle.cpp
index ee8e3f37..8853caa0 100644
--- a/Grbl_Esp32/src/Spindles/10vSpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/10vSpindle.cpp
@@ -68,69 +68,20 @@ namespace Spindles {
                        _pwm_precision);
     }
 
-    uint32_t _10v::set_rpm(uint32_t rpm) {
-        uint32_t pwm_value;
+    // This appears identical to the code in PWMSpindle.cpp but
+    // it uses the 10v versions of set_enable and set_output
+    uint32_t IRAM_ATTR _10v::set_rpm(uint32_t rpm) {
+        sys.spindle_speed = rpm = limitRPM(overrideRPM(rpm));
 
-        if (_output_pin.undefined()) {
-            return rpm;
-        }
+        set_enable(gc_state.modal.spindle != SpindleState::Disable);
+        set_output(RPMtoPWM(rpm));
 
-        // apply speed overrides
-        rpm = rpm * sys.spindle_speed_ovr / 100;  // Scale by spindle speed override value (percent)
-
-        // apply limits limits
-        if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
-            rpm = _max_rpm;
-        } else if (rpm != 0 && rpm <= _min_rpm) {
-            rpm = _min_rpm;
-        }
-        sys.spindle_speed = rpm;
-
-        // determine the pwm value
-        if (rpm == 0) {
-            pwm_value = _pwm_off_value;
-        } else {
-            pwm_value = map_uint32_t(rpm, _min_rpm, _max_rpm, _pwm_min_value, _pwm_max_value);
-        }
-
-        set_output(pwm_value);
         return rpm;
     }
 
-    /*
-	void _10v::set_state(SpindleState state, uint32_t rpm) {
-		if (sys.abort) {
-            return;   // Block during abort.
-        }
-
-		if (state == SpindleState::Disable) { // Halt or set spindle direction and rpm.
-			sys.spindle_speed = 0;
-			stop();
-		} else {
-			set_direction(state == SpindleState:Cw);
-			set_rpm(rpm);
-		}
-
-		set_enable(state != SpindleState::Disable);
-
-		sys.report_ovr_counter = 0; // Set to report change immediately
-	}
-	*/
-
-    SpindleState _10v::get_state() {
-        if (_current_pwm_duty == 0 || _output_pin.undefined()) {
-            return SpindleState::Disable;
-        }
-        if (_direction_pin.defined()) {
-            return _direction_pin.read() ? SpindleState::Cw : SpindleState::Ccw;
-        }
-        return SpindleState::Cw;
-    }
-
     void _10v::stop() {
-        // inverts are delt with in methods
         set_enable(false);
-        set_output(_pwm_off_value);
+        set_output(_pwm_off);
     }
 
     void _10v::set_enable(bool enable) {
diff --git a/Grbl_Esp32/src/Spindles/10vSpindle.h b/Grbl_Esp32/src/Spindles/10vSpindle.h
index 35891591..bbf1e6db 100644
--- a/Grbl_Esp32/src/Spindles/10vSpindle.h
+++ b/Grbl_Esp32/src/Spindles/10vSpindle.h
@@ -41,11 +41,9 @@ namespace Spindles {
         void     init() override;
         void     config_message() override;
         uint32_t set_rpm(uint32_t rpm) override;
-        //void set_state(SpindleState state, uint32_t rpm);
 
-        SpindleState get_state() override;
-        void         stop() override;
-        void         deinit() override;
+        void stop() override;
+        void deinit() override;
 
         // Configuration handlers:
         void validate() const override { PWM::validate(); }
diff --git a/Grbl_Esp32/src/Spindles/BESCSpindle.cpp b/Grbl_Esp32/src/Spindles/BESCSpindle.cpp
index de2a7a99..01de8a23 100644
--- a/Grbl_Esp32/src/Spindles/BESCSpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/BESCSpindle.cpp
@@ -64,9 +64,9 @@ namespace Spindles {
         _pwm_precision = 16;
 
         // override these settings
-        _pwm_off_value = BESC_MIN_PULSE_CNT;
-        _pwm_min_value = _pwm_off_value;
-        _pwm_max_value = BESC_MAX_PULSE_CNT;
+        _pwm_off = BESC_MIN_PULSE_CNT;
+        _pwm_min = _pwm_off;
+        _pwm_max = BESC_MAX_PULSE_CNT;
 
         auto outputPin = _output_pin.getNative(Pin::Capabilities::PWM);
 
@@ -94,35 +94,6 @@ namespace Spindles {
                        _pwm_precision);
     }
 
-    uint32_t BESC::set_rpm(uint32_t rpm) {
-        uint32_t pwm_value;
-
-        if (_output_pin.undefined()) {
-            return rpm;
-        }
-
-        // apply speed overrides
-        rpm = rpm * sys.spindle_speed_ovr / 100;  // Scale by spindle speed override value (percent)
-
-        // apply limits limits
-        if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
-            rpm = _max_rpm;
-        } else if (rpm != 0 && rpm <= _min_rpm) {
-            rpm = _min_rpm;
-        }
-        sys.spindle_speed = rpm;
-
-        // determine the pwm value
-        if (rpm == 0) {
-            pwm_value = _pwm_off_value;
-        } else {
-            pwm_value = map_uint32_t(rpm, _min_rpm, _max_rpm, _pwm_min_value, _pwm_max_value);
-        }
-
-        set_output(pwm_value);
-        return rpm;
-    }
-
     // Configuration registration
     namespace {
         SpindleFactory::InstanceBuilder<BESC> registration("BESC");
diff --git a/Grbl_Esp32/src/Spindles/BESCSpindle.h b/Grbl_Esp32/src/Spindles/BESCSpindle.h
index a9f46439..32d810c7 100644
--- a/Grbl_Esp32/src/Spindles/BESCSpindle.h
+++ b/Grbl_Esp32/src/Spindles/BESCSpindle.h
@@ -45,9 +45,10 @@ namespace Spindles {
         BESC& operator=(const BESC&) = delete;
         BESC& operator=(BESC&&) = delete;
 
-        void     init() override;
-        void     config_message() override;
-        uint32_t set_rpm(uint32_t rpm) override;
+        void init() override;
+        void config_message() override;
+
+        // set_rpm() for BESC is the same as for PWM
 
         // Configuration handlers:
         void validate() const override { PWM::validate(); }
diff --git a/Grbl_Esp32/src/Spindles/DacSpindle.cpp b/Grbl_Esp32/src/Spindles/DacSpindle.cpp
index 60a1bdb8..c88e1ff7 100644
--- a/Grbl_Esp32/src/Spindles/DacSpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/DacSpindle.cpp
@@ -32,9 +32,9 @@ namespace Spindles {
             return;
         }
 
-        _pwm_min_value = 0;    // not actually PWM...DAC counts
-        _pwm_max_value = 255;  // not actually PWM...DAC counts
-        _gpio_ok       = true;
+        _pwm_min = 0;    // not actually PWM...DAC counts
+        _pwm_max = 255;  // not actually PWM...DAC counts
+        _gpio_ok = true;
 
         if (!_output_pin.capabilities().has(Pin::Capabilities::DAC)) {  // DAC can only be used on these pins
             _gpio_ok = false;
@@ -61,40 +61,9 @@ namespace Spindles {
     }
 
     uint32_t Dac::set_rpm(uint32_t rpm) {
-        if (_output_pin.undefined()) {
-            return rpm;
-        }
-
-        uint32_t pwm_value;
-
-        // apply overrides and limits
-        rpm = rpm * sys.spindle_speed_ovr / 100;  // Scale by spindle speed override value (percent)
-
-        // Calculate PWM register value based on rpm max/min settings and programmed rpm.
-        if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
-            // No PWM range possible. Set simple on/off spindle control pin state.
-            sys.spindle_speed = _max_rpm;
-            pwm_value         = _pwm_max_value;
-        } else if (rpm <= _min_rpm) {
-            if (rpm == 0) {  // S0 disables spindle
-                sys.spindle_speed = 0;
-                pwm_value         = 0;
-            } else {  // Set minimum PWM output
-                rpm               = _min_rpm;
-                sys.spindle_speed = rpm;
-                pwm_value         = 0;
-                grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RPM less than min RPM:%5.2f %d", rpm, pwm_value);
-            }
-        } else {
-            // Compute intermediate PWM value with linear spindle speed model.
-            // NOTE: A nonlinear model could be installed here, if required, but keep it VERY light-weight.
-            sys.spindle_speed = rpm;
-
-            pwm_value = map_uint32_t(rpm, _min_rpm, _max_rpm, _pwm_min_value, _pwm_max_value);
-        }
-
-        set_output(pwm_value);
+        sys.spindle_speed = rpm = limitRPM(overrideRPM(rpm));
 
+        set_output(RPMtoPWM(rpm));
         return rpm;
     }
 
diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp
index 8b7cc054..096a8ee7 100644
--- a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp
@@ -48,7 +48,7 @@ namespace Spindles {
         data.msg[5] = (mode == SpindleState::Ccw) ? 0x02 : (mode == SpindleState::Cw ? 0x01 : 0x06);
     }
 
-    void H2A::set_speed_command(uint32_t rpm, ModbusCommand& data) {
+    void IRAM_ATTR H2A::set_speed_command(uint32_t rpm, ModbusCommand& data) {
         // NOTE: data length is excluding the CRC16 checksum.
         data.tx_length = 6;
         data.rx_length = 6;
diff --git a/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp b/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp
index 721831b1..8222a5f4 100644
--- a/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp
@@ -177,7 +177,7 @@ namespace Spindles {
         }
     }
 
-    void Huanyang::set_speed_command(uint32_t rpm, ModbusCommand& data) {
+    void IRAM_ATTR Huanyang::set_speed_command(uint32_t rpm, ModbusCommand& data) {
         // NOTE: data length is excluding the CRC16 checksum.
         data.tx_length = 5;
         data.rx_length = 5;
diff --git a/Grbl_Esp32/src/Spindles/Laser.cpp b/Grbl_Esp32/src/Spindles/Laser.cpp
index 03c84e68..aa9c6109 100644
--- a/Grbl_Esp32/src/Spindles/Laser.cpp
+++ b/Grbl_Esp32/src/Spindles/Laser.cpp
@@ -53,15 +53,13 @@ namespace Spindles {
         _pwm_period    = (1 << _pwm_precision);
 
         // pre-calculate some PWM count values
-        _pwm_off_value = 0;
-        _pwm_min_value = 0;
-        _pwm_max_value = _pwm_period;
+        _pwm_off = 0;
+        _pwm_min = 0;
+        _pwm_max = _pwm_period;
 
         _min_rpm = 0;
         _max_rpm = _laser_full_power;
 
-        _piecewise_linear = false;
-
         _pwm_chan_num = 0;  // Channel 0 is reserved for spindle use
     }
 
diff --git a/Grbl_Esp32/src/Spindles/NullSpindle.cpp b/Grbl_Esp32/src/Spindles/NullSpindle.cpp
index ab6a03ff..0a9a07c7 100644
--- a/Grbl_Esp32/src/Spindles/NullSpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/NullSpindle.cpp
@@ -30,8 +30,8 @@ namespace Spindles {
         use_delays    = false;
         config_message();
     }
-    uint32_t Null::set_rpm(uint32_t rpm) {
-        sys.spindle_speed = rpm;
+    uint32_t IRAM_ATTR Null::set_rpm(uint32_t rpm) {
+        sys.spindle_speed = rpm = overrideRPM(rpm);
         return rpm;
     }
     void Null::set_state(SpindleState state, uint32_t rpm) {
diff --git a/Grbl_Esp32/src/Spindles/NullSpindle.h b/Grbl_Esp32/src/Spindles/NullSpindle.h
index 5273b39a..12e610c2 100644
--- a/Grbl_Esp32/src/Spindles/NullSpindle.h
+++ b/Grbl_Esp32/src/Spindles/NullSpindle.h
@@ -25,7 +25,7 @@
 
 namespace Spindles {
     // This is a dummy spindle that has no I/O.
-    // It is used to ignore spindle commands when no spinde is desired
+    // It is used to ignore spindle commands when no spindle is desired
     class Null : public Spindle {
     public:
         Null() = default;
diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp
index e9431bfe..f8d83029 100644
--- a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp
@@ -76,61 +76,25 @@ namespace Spindles {
         _pwm_precision = calc_pwm_precision(_pwm_freq);  // determine the best precision
         _pwm_period    = (1 << _pwm_precision);
 
-        if (_pwm_min_value > _pwm_max_value) {
+        if (_pwm_min_setting > _pwm_max_setting) {
             log_warn("Spindle min PWM is greater than max.");
         }
 
         // pre-calculate some PWM count values
-        _pwm_off_value = uint32_t(_pwm_period * _pwm_off_value_setting / 100.0);
-        _pwm_min_value = uint32_t(_pwm_period * _pwm_min_value_setting / 100.0);
-        _pwm_max_value = uint32_t(_pwm_period * _pwm_max_value_setting / 100.0);
-
-        if (_piecewise_linear) {
-            _min_rpm = RPM_MIN;
-            _max_rpm = RPM_MAX;
-        }
-        // The pwm_gradient is the pwm duty cycle units per rpm
-        // _pwm_gradient = (_pwm_max_value - _pwm_min_value) / (_max_rpm - _min_rpm);
+        _pwm_off = uint32_t(_pwm_period * _pwm_off_setting / 100.0);
+        _pwm_min = uint32_t(_pwm_period * _pwm_min_setting / 100.0);
+        _pwm_max = uint32_t(_pwm_period * _pwm_max_setting / 100.0);
 
         _pwm_chan_num = 0;  // Channel 0 is reserved for spindle use
     }
 
-    uint32_t PWM::set_rpm(uint32_t rpm) {
-        uint32_t pwm_value;
-
-        if (_output_pin.undefined()) {
-            return rpm;
-        }
-
-        // apply override
-        rpm = rpm * sys.spindle_speed_ovr / 100;  // Scale by spindle speed override value (uint8_t percent)
-
-        // apply limits
-        if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
-            rpm = _max_rpm;
-        } else if (rpm != 0 && rpm <= _min_rpm) {
-            rpm = _min_rpm;
-        }
-
-        sys.spindle_speed = rpm;
-
-        if (_piecewise_linear) {
-            //pwm_value = piecewise_linear_fit(rpm); TODO
-            pwm_value = 0;
-            log_warn("Linear fit not implemented yet.");
-
-        } else {
-            if (rpm == 0) {
-                pwm_value = _pwm_off_value;
-            } else {
-                pwm_value = map_uint32_t(rpm, _min_rpm, _max_rpm, _pwm_min_value, _pwm_max_value);
-            }
-        }
+    uint32_t IRAM_ATTR PWM::set_rpm(uint32_t rpm) {
+        sys.spindle_speed = rpm = limitRPM(overrideRPM(rpm));
 
         set_enable(gc_state.modal.spindle != SpindleState::Disable);
-        set_output(pwm_value);
+        set_output(RPMtoPWM(rpm));
 
-        return 0;
+        return rpm;
     }
 
     void PWM::set_state(SpindleState state, uint32_t rpm) {
@@ -169,9 +133,8 @@ namespace Spindles {
     }
 
     void PWM::stop() {
-        // inverts are delt with in methods
         set_enable(false);
-        set_output(_pwm_off_value);
+        set_output(_pwm_off);
     }
 
     // prints the startup message of the spindle config
@@ -182,12 +145,12 @@ namespace Spindles {
         );
     }
 
-    void PWM::set_output(uint32_t duty) {
+    void IRAM_ATTR PWM::set_output(uint32_t duty) {
         if (_output_pin.undefined()) {
             return;
         }
 
-        // to prevent excessive calls to ledcWrite, make sure duty hass changed
+        // to prevent excessive calls to ledcWrite, make sure duty has changed
         if (duty == _current_pwm_duty) {
             return;
         }
@@ -250,6 +213,43 @@ namespace Spindles {
         _direction_pin.setAttr(Pin::Attr::Input);
     }
 
+    uint32_t IRAM_ATTR PWM::limitRPM(uint32_t rpm) {
+        if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
+            rpm = _max_rpm;
+        } else if (rpm != 0 && rpm <= _min_rpm) {
+            rpm = _min_rpm;
+        }
+        return rpm;
+    }
+
+    uint32_t IRAM_ATTR PWM::RPMtoPWM(uint32_t rpm) {
+#ifdef PIECEWISE_LINEAR
+        // This is a num_segments generalization of the code after the endif.
+        // This two-segment array gives the same results as the old code
+        //    [{0, _pwm_off}, {_min_rpm, _pwm_min}, {_max_rpm, _pwm_max}]
+        if (rpm < rpm_in[0]) {
+            return pwm_out[0];
+        }
+        int i;
+        for (i = 0; i < num_segments; i++) {
+            if (rpm >= rpm_in[i]) {
+                break;
+            }
+        }
+        if (i == num_segments) {
+            return pwm_out[num_segments];
+        }
+        _min_rpm = rpm_in[i];
+        _max_rpm = rpm_in[i + 1];
+        _pwm_min = pwm_out[i];
+        _pwm_max = pwm_out[i + 1];
+#endif
+        if (rpm == 0 || rpm < _min_rpm) {
+            return _pwm_off;
+        }
+        return (rpm - _min_rpm) * (_pwm_max - _pwm_min) / (_max_rpm - _min_rpm) + _pwm_min;
+    }
+
     // Configuration registration
     namespace {
         SpindleFactory::InstanceBuilder<PWM> registration("PWM");
diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.h b/Grbl_Esp32/src/Spindles/PWMSpindle.h
index cfe1c15c..8c8bc52e 100644
--- a/Grbl_Esp32/src/Spindles/PWMSpindle.h
+++ b/Grbl_Esp32/src/Spindles/PWMSpindle.h
@@ -43,7 +43,8 @@ namespace Spindles {
         SpindleState     get_state() override;
         void             stop() override;
         void             config_message() override;
-
+        uint32_t         limitRPM(uint32_t RPM);
+        uint32_t         RPMtoPWM(uint32_t rpm);
         // Configuration handlers:
         void validate() const override { Spindle::validate(); }
 
@@ -52,12 +53,10 @@ namespace Spindles {
             handler.handle("max_rpm", _max_rpm);
 
             handler.handle("pwm_freq", _pwm_freq);
-            handler.handle("pwm_off", _pwm_off_value_setting);
-            handler.handle("pwm_min", _pwm_min_value_setting);
-            handler.handle("pwm_max", _pwm_max_value_setting);
+            handler.handle("pwm_off", _pwm_off_setting);
+            handler.handle("pwm_min", _pwm_min_setting);
+            handler.handle("pwm_max", _pwm_max_setting);
             handler.handle("invert_pwm", _invert_pwm);
-            handler.handle("piecewise_linear", _piecewise_linear);
-
             handler.handle("output_pin", _output_pin);
             handler.handle("enable_pin", _enable_pin);
             handler.handle("direction_pin", _direction_pin);
@@ -72,16 +71,16 @@ namespace Spindles {
         virtual ~PWM() {}
 
     protected:
-        uint32_t _pwm_off_value_setting;  // do we need these 3?
-        uint32_t _pwm_min_value_setting;
-        uint32_t _pwm_max_value_setting;
+        uint32_t _pwm_off_setting;  // do we need these 3?
+        uint32_t _pwm_min_setting;
+        uint32_t _pwm_max_setting;
 
         int32_t  _current_pwm_duty;
         uint32_t _min_rpm;
         uint32_t _max_rpm;
-        uint32_t _pwm_off_value;
-        uint32_t _pwm_min_value;
-        uint32_t _pwm_max_value;
+        uint32_t _pwm_off;
+        uint32_t _pwm_min;
+        uint32_t _pwm_max;
         Pin      _output_pin;
         Pin      _enable_pin;
         Pin      _direction_pin;
@@ -89,10 +88,8 @@ namespace Spindles {
         uint32_t _pwm_freq;
         uint32_t _pwm_period;  // how many counts in 1 period
         uint8_t  _pwm_precision;
-        bool     _piecewise_linear;
         bool     _off_with_zero_speed;
         bool     _invert_pwm;
-        //uint32_t _pwm_gradient; // Precalulated value to speed up rpm to PWM conversions.
 
         virtual void set_direction(bool Clockwise);
         virtual void set_output(uint32_t duty);
diff --git a/Grbl_Esp32/src/Spindles/RelaySpindle.cpp b/Grbl_Esp32/src/Spindles/RelaySpindle.cpp
index 64a6aaf1..431ea978 100644
--- a/Grbl_Esp32/src/Spindles/RelaySpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/RelaySpindle.cpp
@@ -54,14 +54,9 @@ namespace Spindles {
                        _direction_pin.name().c_str());
     }
 
-    uint32_t Relay::set_rpm(uint32_t rpm) {
-        if (_output_pin.undefined()) {
-            return rpm;
-        }
-
-        sys.spindle_speed = rpm;
+    uint32_t IRAM_ATTR Relay::set_rpm(uint32_t rpm) {
+        sys.spindle_speed = rpm = overrideRPM(rpm);
         set_output(rpm != 0);
-
         return rpm;
     }
 
diff --git a/Grbl_Esp32/src/Spindles/Spindle.h b/Grbl_Esp32/src/Spindles/Spindle.h
index 705edc3c..b18313aa 100644
--- a/Grbl_Esp32/src/Spindles/Spindle.h
+++ b/Grbl_Esp32/src/Spindles/Spindle.h
@@ -58,6 +58,9 @@ namespace Spindles {
         virtual bool         isRateAdjusted();
         virtual void         sync(SpindleState state, uint32_t rpm);
 
+        void                      spinDown() { set_state(SpindleState::Disable, 0); }
+        static uint32_t IRAM_ATTR overrideRPM(uint32_t rpm) { return rpm * sys.spindle_speed_ovr / 100; }
+
         bool                  is_reversable;
         bool                  use_delays;  // will SpinUp and SpinDown delays be used.
         volatile SpindleState _current_state = SpindleState::Disable;
diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp
index 6bfa0550..681f6f40 100644
--- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp
@@ -397,30 +397,23 @@ namespace Spindles {
         if (xQueueSend(vfd_cmd_queue, &mode_cmd, 0) != pdTRUE) {
             grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD Queue Full");
         }
-
         return true;
     }
 
-    uint32_t VFD::set_rpm(uint32_t rpm) {
-        if (!vfd_ok) {
-            return 0;
-        }
-
-#ifdef VFD_DEBUG_MODE
-        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm));
-#endif
-
-        // apply override
-        rpm = rpm * sys.spindle_speed_ovr / 100;  // Scale by spindle speed override value (uint8_t percent)
-
-        // apply limits
+    uint32_t IRAM_ATTR VFD::limitRPM(uint32_t rpm) {
         if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
             rpm = _max_rpm;
         } else if (rpm != 0 && rpm <= _min_rpm) {
             rpm = _min_rpm;
         }
+        return rpm;
+    }
 
-        sys.spindle_speed = rpm;
+    uint32_t IRAM_ATTR VFD::set_rpm(uint32_t rpm) {
+        if (!vfd_ok) {
+            return 0;
+        }
+        rpm = limitRPM(overrideRPM(rpm));
 
         if (rpm == _current_rpm) {  // prevent setting same RPM twice
             return rpm;
@@ -428,7 +421,7 @@ namespace Spindles {
 
         _current_rpm = rpm;
 
-        // TODO add the speed modifiers override, linearization, etc.
+        // TODO add linearization
 
         ModbusCommand rpm_cmd;
         rpm_cmd.msg[0] = VFD_RS485_ADDR;
@@ -441,10 +434,13 @@ namespace Spindles {
 
         rpm_cmd.critical = false;
 
-        if (xQueueSend(vfd_cmd_queue, &rpm_cmd, 0) != pdTRUE) {
-            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD Queue Full");
+        if (xPortInIsrContext()) {
+            xQueueSendFromISR(vfd_cmd_queue, &rpm_cmd, 0);
+        } else {
+            if (xQueueSend(vfd_cmd_queue, &rpm_cmd, 0) != pdTRUE) {
+                grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD Queue Full");
+            }
         }
-
         return rpm;
     }
 
diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.h b/Grbl_Esp32/src/Spindles/VFDSpindle.h
index e818b932..55063a55 100644
--- a/Grbl_Esp32/src/Spindles/VFDSpindle.h
+++ b/Grbl_Esp32/src/Spindles/VFDSpindle.h
@@ -81,6 +81,7 @@ namespace Spindles {
 
         // TODO FIXME: Have to make these public because of the parsers.
         // Should hide them and use a member function.
+        // _min_rpm and _max_rpm could possibly move to the Spindle base class
         uint32_t          _min_rpm;
         uint32_t          _max_rpm;
         volatile uint32_t _sync_rpm;
@@ -92,6 +93,7 @@ namespace Spindles {
         SpindleState get_state();
         uint32_t     set_rpm(uint32_t rpm);
         void         stop();
+        uint32_t     limitRPM(uint32_t RPM);
 
         // Configuration handlers:
         void validate() const override { Spindle::validate(); }
diff --git a/Grbl_Esp32/src/Spindles/YL620Spindle.cpp b/Grbl_Esp32/src/Spindles/YL620Spindle.cpp
index 8c049613..774aef30 100644
--- a/Grbl_Esp32/src/Spindles/YL620Spindle.cpp
+++ b/Grbl_Esp32/src/Spindles/YL620Spindle.cpp
@@ -102,7 +102,7 @@ namespace Spindles {
         }
     }
 
-    void YL620::set_speed_command(uint32_t rpm, ModbusCommand& data) {
+    void IRAM_ATTR YL620::set_speed_command(uint32_t rpm, ModbusCommand& data) {
         // NOTE: data length is excluding the CRC16 checksum.
         data.tx_length = 6;
         data.rx_length = 6;
diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp
index 44daad80..4bcf1f77 100644
--- a/Grbl_Esp32/src/Stepper.cpp
+++ b/Grbl_Esp32/src/Stepper.cpp
@@ -225,36 +225,6 @@ void IRAM_ATTR onStepperDriverTimer() {
 static void IRAM_ATTR stepper_pulse_func() {
     auto n_axis = config->_axes->_numberAxis;
 
-#ifdef LATER
-    // XXX this should be in the motor driver, not here
-    if (motors_direction(st.dir_outbits)) {
-        auto wait_direction = config->_directionDelayMicroSeconds;
-        if (wait_direction > 0) {
-            // Stepper drivers need some time between changing direction and doing a pulse.
-            switch (config->_stepType) {
-                case ST_I2S_STREAM:
-                    i2s_out_push_sample(wait_direction);
-                    break;
-                case ST_I2S_STATIC:
-                case ST_TIMED: {
-                    // wait for step pulse time to complete...some time expired during code above
-                    //
-                    // If we are using GPIO stepping as opposed to RMT, record the
-                    // time that we turned on the direction pins so we can delay a bit.
-                    // If we are using RMT, we can't delay here.
-                    auto direction_pulse_start_time = esp_timer_get_time() + wait_direction;
-                    while ((esp_timer_get_time() - direction_pulse_start_time) < 0) {
-                        NOP();  // spin here until time to turn off step
-                    }
-                    break;
-                }
-                case ST_RMT:
-                    break;
-            }
-        }
-    }
-#endif
-
     // If we are using GPIO stepping as opposed to RMT, record the
     // time that we turned on the step pins so we can turn them off
     // at the end of this routine without incurring another interrupt.
@@ -279,7 +249,7 @@ static void IRAM_ATTR stepper_pulse_func() {
                 st.exec_block       = &st_block_buffer[st.exec_block_index];
                 // Initialize Bresenham line and distance counters
                 for (int axis = 0; axis < n_axis; axis++) {
-                    st.counter[axis] = (st.exec_block->step_event_count >> 1);
+                    st.counter[axis] = st.exec_block->step_event_count >> 1;
                 }
             }
             st.dir_outbits = st.exec_block->direction_bits;
@@ -316,9 +286,9 @@ static void IRAM_ATTR stepper_pulse_func() {
         // Execute step displacement profile by Bresenham line algorithm
         st.counter[axis] += st.steps[axis];
         if (st.counter[axis] > st.exec_block->step_event_count) {
-            st.step_outbits |= bit(axis);
+            bitnum_true(st.step_outbits, axis);
             st.counter[axis] -= st.exec_block->step_event_count;
-            if (st.exec_block->direction_bits & bit(axis)) {
+            if (bitnum_istrue(st.exec_block->direction_bits, axis)) {
                 sys_position[axis]--;
             } else {
                 sys_position[axis]++;
@@ -442,7 +412,7 @@ void st_reset() {
 }
 
 // Stepper shutdown
-void st_go_idle() {
+void IRAM_ATTR st_go_idle() {
     // Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active.
     Stepper_Timer_Stop();
 
@@ -980,13 +950,8 @@ void IRAM_ATTR Stepper_Timer_Start() {
 }
 
 void IRAM_ATTR Stepper_Timer_Stop() {
-#ifdef ESP_DEBUG
-    //Serial.println("ST Stop");
-#endif
     if (config->_stepType == ST_I2S_STREAM) {
-#ifdef USE_I2S_STEPS
         i2s_out_set_passthrough();
-#endif
     } else {
         timerAlarmDisable(stepTimer);
     }