diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp
index 85592b08..ed18ff40 100644
--- a/Grbl_Esp32/src/Limits.cpp
+++ b/Grbl_Esp32/src/Limits.cpp
@@ -309,16 +309,21 @@ uint8_t limit_mask = 0;
 
 void limits_init() {
     limit_mask     = 0;
-    Pin::Attr mode = Pin::Attr::Input | Pin::Attr::PullUp | Pin::Attr::ISR;
-#ifdef DISABLE_LIMIT_PIN_PULL_UP
-    mode = Pin::Attr::Input | Pin::Attr::ISR;
-#endif
+    Pin::Attr mode = Pin::Attr::Input | Pin::Attr::ISR;
 
     auto n_axis = number_axis->get();
     for (int axis = 0; axis < n_axis; axis++) {
         for (int gang_index = 0; gang_index < 2; gang_index++) {
             Pin pin;
             if ((pin = LimitPins[axis][gang_index]->get()) != Pin::UNDEFINED) {
+
+#ifndef DISABLE_LIMIT_PIN_PULL_UP
+                if (pin.capabilities().has(Pins::PinCapabilities::PullUp))
+                {
+                    mode = mode | Pin::Attr::PullUp;
+                }
+#endif
+
                 pin.setAttr(mode);
                 limit_mask |= bit(axis);
                 if (hard_limits->get()) {
diff --git a/Grbl_Esp32/src/Probe.cpp b/Grbl_Esp32/src/Probe.cpp
index 439f118e..33196bc2 100644
--- a/Grbl_Esp32/src/Probe.cpp
+++ b/Grbl_Esp32/src/Probe.cpp
@@ -37,7 +37,13 @@ void probe_init() {
 #ifdef DISABLE_PROBE_PIN_PULL_UP
         ProbePin->get().setAttr(Pin::Attr::Input);
 #else
-        ProbePin->get().setAttr(Pin::Attr::Input | Pin::Attr::PullUp);  // Enable internal pull-up resistors. Normal high operation.
+        if (ProbePin->get().capabilities().has(Pins::PinCapabilities::PullUp))
+        {
+            ProbePin->get().setAttr(Pin::Attr::Input | Pin::Attr::PullUp);  // Enable internal pull-up resistors. Normal high operation.
+        }
+        else {
+            ProbePin->get().setAttr(Pin::Attr::Input);
+        }
 #endif
 
         if (show_init_msg) {
diff --git a/Grbl_Esp32/src/StackTrace/AssertionFailed.cpp b/Grbl_Esp32/src/StackTrace/AssertionFailed.cpp
index a4000acf..6c914ebb 100644
--- a/Grbl_Esp32/src/StackTrace/AssertionFailed.cpp
+++ b/Grbl_Esp32/src/StackTrace/AssertionFailed.cpp
@@ -4,7 +4,6 @@
 #include <cstring>
 
 #ifdef ESP32
-#    ifdef UNIT_TEST
 
 #        include "debug_helpers.h"
 #        include "WString.h"
@@ -27,27 +26,6 @@ AssertionFailed AssertionFailed::create(const char* condition, const char* msg,
     return AssertionFailed(st);
 }
 
-#    else
-
-#        include "stdio.h"
-
-AssertionFailed AssertionFailed::create(const char* condition, const char* msg, ...) {
-    String st = "\r\nError ";
-    st += condition;
-    st += " failed: ";
-
-    char    tmp[255];
-    va_list arg;
-    va_start(arg, msg);
-    size_t len = vsnprintf(tmp, 255, msg, arg);
-    tmp[254]   = 0;
-    st += tmp;
-
-    return AssertionFailed(st);
-}
-
-#    endif
-
 #else
 
 #    include <iostream>
diff --git a/Grbl_Esp32/src/StackTrace/debug_helpers.cpp b/Grbl_Esp32/src/StackTrace/debug_helpers.cpp
index d19d46c4..3250af07 100644
--- a/Grbl_Esp32/src/StackTrace/debug_helpers.cpp
+++ b/Grbl_Esp32/src/StackTrace/debug_helpers.cpp
@@ -1,5 +1,4 @@
 #ifdef ESP32
-#    ifdef UNIT_TEST
 
 // Copyright 2015-2019 Espressif Systems (Shanghai) PTE LTD
 //
@@ -88,5 +87,4 @@ String IRAM_ATTR esp_backtrace_print(int depth) {
     return s;
 }
 
-#    endif
 #endif
diff --git a/Grbl_Esp32/src/StackTrace/debug_helpers.h b/Grbl_Esp32/src/StackTrace/debug_helpers.h
index a22286d4..dc8cfd59 100644
--- a/Grbl_Esp32/src/StackTrace/debug_helpers.h
+++ b/Grbl_Esp32/src/StackTrace/debug_helpers.h
@@ -1,5 +1,4 @@
 #ifdef ESP32
-#    ifdef UNIT_TEST
 // Copyright 2015-2019 Espressif Systems (Shanghai) PTE LTD
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
@@ -14,21 +13,21 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#        pragma once
+#    pragma once
 
-#        ifdef __cplusplus
+#    ifdef __cplusplus
 extern "C" {
-#        endif
+#    endif
 
-#        ifndef __ASSEMBLER__
+#    ifndef __ASSEMBLER__
 
-#            include <stdbool.h>
-#            include "esp_err.h"
-#            include "soc/soc.h"
+#        include <stdbool.h>
+#        include "esp_err.h"
+#        include "soc/soc.h"
 
-#            define ESP_WATCHPOINT_LOAD 0x40000000
-#            define ESP_WATCHPOINT_STORE 0x80000000
-#            define ESP_WATCHPOINT_ACCESS 0xC0000000
+#        define ESP_WATCHPOINT_LOAD 0x40000000
+#        define ESP_WATCHPOINT_STORE 0x80000000
+#        define ESP_WATCHPOINT_ACCESS 0xC0000000
 
 /*
      * @brief   Structure used for backtracing
@@ -97,10 +96,9 @@ bool esp_backtrace_get_next_frame(esp_backtrace_frame_t* frame);
 
 String esp_backtrace_print(int depth);
 
-#        endif
-#        ifdef __cplusplus
-}
-#        endif
-
 #    endif
+#    ifdef __cplusplus
+}
+#    endif
+
 #endif
diff --git a/Grbl_Esp32/src/StackTrace/debug_helpers_asm.S b/Grbl_Esp32/src/StackTrace/debug_helpers_asm.S
index 1232a6de..3790d2eb 100644
--- a/Grbl_Esp32/src/StackTrace/debug_helpers_asm.S
+++ b/Grbl_Esp32/src/StackTrace/debug_helpers_asm.S
@@ -1,5 +1,4 @@
 #ifdef ESP32
-#ifdef UNIT_TEST
 
 // Copyright 2015-2019 Espressif Systems (Shanghai) PTE LTD
 //
@@ -60,4 +59,3 @@ esp_backtrace_get_start:
     retw
 
 #endif
-#endif
diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp
index f9695b3e..90f7dc67 100644
--- a/Grbl_Esp32/src/System.cpp
+++ b/Grbl_Esp32/src/System.cpp
@@ -48,43 +48,83 @@ void system_ini() {  // Renamed from system_init() due to conflict with esp32 fi
     // setup control inputs
 
     if (ControlSafetyDoorPin->get() != Pin::UNDEFINED) {
-        ControlSafetyDoorPin->get().setAttr(Pin::Attr::Input | Pin::Attr::PullUp | Pin::Attr::ISR);
-        ControlSafetyDoorPin->get().attachInterrupt(isr_control_inputs, CHANGE);
+        auto pin  = ControlSafetyDoorPin->get();
+        auto attr = Pin::Attr::Input | Pin::Attr::ISR;
+        if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) {
+            attr = attr | Pin::Attr::PullUp;
+        }
+        pin.setAttr(attr);
+        pin.attachInterrupt(isr_control_inputs, CHANGE);
     }
 
     if (ControlResetPin->get() != Pin::UNDEFINED) {
-        ControlResetPin->get().setAttr(Pin::Attr::Input | Pin::Attr::PullUp | Pin::Attr::ISR);
-        ControlResetPin->get().attachInterrupt(isr_control_inputs, CHANGE);
+        auto pin  = ControlResetPin->get();
+        auto attr = Pin::Attr::Input | Pin::Attr::ISR;
+        if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) {
+            attr = attr | Pin::Attr::PullUp;
+        }
+        pin.setAttr(attr);
+        pin.attachInterrupt(isr_control_inputs, CHANGE);
     }
 
     if (ControlFeedHoldPin->get() != Pin::UNDEFINED) {
-        ControlFeedHoldPin->get().setAttr(Pin::Attr::Input | Pin::Attr::PullUp | Pin::Attr::ISR);
-        ControlFeedHoldPin->get().attachInterrupt(isr_control_inputs, CHANGE);
+        auto pin  = ControlFeedHoldPin->get();
+        auto attr = Pin::Attr::Input | Pin::Attr::ISR;
+        if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) {
+            attr = attr | Pin::Attr::PullUp;
+        }
+        pin.setAttr(attr);
+        pin.attachInterrupt(isr_control_inputs, CHANGE);
     }
 
     if (ControlCycleStartPin->get() != Pin::UNDEFINED) {
-        ControlCycleStartPin->get().setAttr(Pin::Attr::Input | Pin::Attr::PullUp | Pin::Attr::ISR);
-        ControlCycleStartPin->get().attachInterrupt(isr_control_inputs, CHANGE);
+        auto pin  = ControlCycleStartPin->get();
+        auto attr = Pin::Attr::Input | Pin::Attr::ISR;
+        if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) {
+            attr = attr | Pin::Attr::PullUp;
+        }
+        pin.setAttr(attr);
+        pin.attachInterrupt(isr_control_inputs, CHANGE);
     }
 
     if (MacroButton0Pin->get() != Pin::UNDEFINED) {
-        MacroButton0Pin->get().setAttr(Pin::Attr::Input | Pin::Attr::PullUp | Pin::Attr::ISR);
-        MacroButton0Pin->get().attachInterrupt(isr_control_inputs, CHANGE);
+        auto pin  = MacroButton0Pin->get();
+        auto attr = Pin::Attr::Input | Pin::Attr::ISR;
+        if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) {
+            attr = attr | Pin::Attr::PullUp;
+        }
+        pin.setAttr(attr);
+        pin.attachInterrupt(isr_control_inputs, CHANGE);
     }
 
     if (MacroButton1Pin->get() != Pin::UNDEFINED) {
-        MacroButton1Pin->get().setAttr(Pin::Attr::Input | Pin::Attr::PullUp | Pin::Attr::ISR);
-        MacroButton1Pin->get().attachInterrupt(isr_control_inputs, CHANGE);
+        auto pin  = MacroButton1Pin->get();
+        auto attr = Pin::Attr::Input | Pin::Attr::ISR;
+        if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) {
+            attr = attr | Pin::Attr::PullUp;
+        }
+        pin.setAttr(attr);
+        pin.attachInterrupt(isr_control_inputs, CHANGE);
     }
 
     if (MacroButton2Pin->get() != Pin::UNDEFINED) {
-        MacroButton2Pin->get().setAttr(Pin::Attr::Input | Pin::Attr::PullUp | Pin::Attr::ISR);
-        MacroButton2Pin->get().attachInterrupt(isr_control_inputs, CHANGE);
+        auto pin  = MacroButton2Pin->get();
+        auto attr = Pin::Attr::Input | Pin::Attr::ISR;
+        if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) {
+            attr = attr | Pin::Attr::PullUp;
+        }
+        pin.setAttr(attr);
+        pin.attachInterrupt(isr_control_inputs, CHANGE);
     }
 
     if (MacroButton3Pin->get() != Pin::UNDEFINED) {
-        MacroButton3Pin->get().setAttr(Pin::Attr::Input | Pin::Attr::PullUp | Pin::Attr::ISR);
-        MacroButton3Pin->get().attachInterrupt(isr_control_inputs, CHANGE);
+        auto pin  = MacroButton3Pin->get();
+        auto attr = Pin::Attr::Input | Pin::Attr::ISR;
+        if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) {
+            attr = attr | Pin::Attr::PullUp;
+        }
+        pin.setAttr(attr);
+        pin.attachInterrupt(isr_control_inputs, CHANGE);
     }
 
 #ifdef ENABLE_CONTROL_SW_DEBOUNCE