diff --git a/include/SimpleMutex.h b/include/SimpleMutex.h
new file mode 100644
index 00000000..bd6df741
--- /dev/null
+++ b/include/SimpleMutex.h
@@ -0,0 +1,66 @@
+#pragma once
+
+#include <freertos/semphr.h>
+
+#include "Common.h"
+
+namespace OpenShock {
+  class SimpleMutex {
+    DISABLE_COPY(SimpleMutex);
+    DISABLE_MOVE(SimpleMutex);
+
+  public:
+    SimpleMutex();
+    ~SimpleMutex();
+
+    bool lock(TickType_t xTicksToWait);
+    void unlock();
+
+  private:
+    SemaphoreHandle_t m_mutex;
+  };
+
+  class ScopedLock {
+    DISABLE_COPY(ScopedLock);
+    DISABLE_MOVE(ScopedLock);
+
+  public:
+    ScopedLock(SimpleMutex* mutex, TickType_t xTicksToWait = portMAX_DELAY)
+      : m_mutex(mutex)
+    {
+      bool result = false;
+      if (m_mutex != nullptr) {
+        result = m_mutex->lock(xTicksToWait);
+      }
+
+      if (!result) {
+        m_mutex = nullptr;
+      }
+    }
+
+    ~ScopedLock()
+    {
+      if (m_mutex != nullptr) {
+        m_mutex->unlock();
+      }
+    }
+
+    bool isLocked() const { return m_mutex != nullptr; }
+
+    bool unlock()
+    {
+      if (m_mutex != nullptr) {
+        m_mutex->unlock();
+        m_mutex = nullptr;
+        return true;
+      }
+
+      return false;
+    }
+
+    SimpleMutex* getMutex() const { return m_mutex; }
+
+  private:
+    SimpleMutex* m_mutex;
+  };
+}  // namespace OpenShock
diff --git a/src/EStopManager.cpp b/src/EStopManager.cpp
index c6c3e171..1558d854 100644
--- a/src/EStopManager.cpp
+++ b/src/EStopManager.cpp
@@ -8,6 +8,7 @@ const char* const TAG = "EStopManager";
 #include "CommandHandler.h"
 #include "config/Config.h"
 #include "Logging.h"
+#include "SimpleMutex.h"
 #include "Time.h"
 #include "util/TaskUtils.h"
 #include "VisualStateManager.h"
@@ -19,224 +20,248 @@ const char* const TAG = "EStopManager";
 using namespace OpenShock;
 
 const uint32_t k_estopHoldToClearTime = 5000;
-const uint32_t k_estopDebounceTime    = 100;
+const uint32_t k_estopUpdateRate      = 5;   // 200 Hz
+const uint32_t k_estopCheckCount      = 13;  // 65 ms at 200 Hz
+const uint16_t k_estopCheckMask       = 0xFFFF >> ((sizeof(uint16_t) * 8) - k_estopCheckCount);
 
-static TaskHandle_t s_estopEventHandlerTask;
-static QueueHandle_t s_estopEventQueue;
+static OpenShock::SimpleMutex s_estopMutex = {};
+static gpio_num_t s_estopPin               = GPIO_NUM_NC;
+static TaskHandle_t s_estopTask;
 
-static bool s_estopActive          = false;
-static bool s_estopAwaitingRelease = false;
-static bool s_lastState            = false;
-static int64_t s_lastStateChange   = 0;
-static int64_t s_estopActivatedAt  = 0;
+static bool s_estopActive         = false;
+static int64_t s_estopActivatedAt = 0;
 
-static gpio_num_t s_estopPin = GPIO_NUM_NC;
+void _estopUpdateExternals(bool isActive, bool isAwaitingRelease)
+{
+  // Set visual state
+  OpenShock::VisualStateManager::SetEmergencyStopStatus(isActive, isAwaitingRelease);
 
-struct EstopEventQueueMessage {
-  bool pressed              : 1;
-  bool deactivatesAtChanged : 1;
-  int64_t deactivatesAt;
+  // Set KeepAlive state
+  OpenShock::CommandHandler::SetKeepAlivePaused(isActive);
+}
+
+enum class EStopState {
+  Idle,
+  ActiveAwaitingRelease,
+  Active,
+  Deactivating,
+  DeactivatingAwaitingRelease,
 };
 
-// This high-priority task is usually idling, waiting for
-// messages from the EStop interrupt or it's hold timer
-void _estopEventHandler(void* pvParameters) {
+// Samples the estop at a fixed rate and sends messages to the estop event handler task
+void _estopCheckerTask(void* pvParameters)
+{
+  uint16_t history = 0xFFFF;  // Bit history of samples, 0 is pressed
+
+  EStopState state      = EStopState::Idle;
   int64_t deactivatesAt = 0;
+
+  bool lastBtnState = false;
+
   for (;;) {
-    // Wait indefinitely for a message from the EStop interrupt routine
-    TickType_t waitTime = portMAX_DELAY;
-
-    // If the EStop is being deactivated, wait for the hold timer to trigger
-    if (deactivatesAt != 0) {
-      int64_t now = OpenShock::millis();
-      if (now >= deactivatesAt) {
-        waitTime = 0;
-      } else {
-        waitTime = pdMS_TO_TICKS(deactivatesAt - OpenShock::millis());
+    // Sleep for the update rate
+    vTaskDelay(pdMS_TO_TICKS(k_estopUpdateRate));
+
+    // Sample the EStop
+    history = (history << 1) | gpio_get_level(s_estopPin);
+
+    // Get current time
+    int64_t now = OpenShock::millis();
+
+    // Check if the EStop is released (not all bits are 1)
+    bool btnState = (history & k_estopCheckMask) != k_estopCheckMask;
+    if (btnState == lastBtnState) {
+      // If the state hasn't changed, handle timing transitions
+      if (state == EStopState::Deactivating && now > deactivatesAt) {
+        state = EStopState::DeactivatingAwaitingRelease;
+        _estopUpdateExternals(s_estopActive, true);
       }
+      continue;
+    }
+    lastBtnState = btnState;
+
+    switch (state) {
+      case EStopState::Idle:
+        if (btnState) {
+          state              = EStopState::ActiveAwaitingRelease;
+          s_estopActive      = true;
+          s_estopActivatedAt = now;
+        }
+        break;
+      case EStopState::ActiveAwaitingRelease:
+        if (!btnState) {
+          state = EStopState::Active;
+        }
+        break;
+      case EStopState::Active:
+        if (btnState) {
+          state         = EStopState::Deactivating;
+          deactivatesAt = now + k_estopHoldToClearTime;
+        }
+        break;
+      case EStopState::Deactivating:
+        if (!btnState) {
+          state = EStopState::Active;
+        } else if (now > deactivatesAt) {
+          state = EStopState::DeactivatingAwaitingRelease;
+        }
+        break;
+      case EStopState::DeactivatingAwaitingRelease:
+        if (!btnState) {
+          state         = EStopState::Idle;
+          s_estopActive = false;
+        }
+        break;
+      default:
+        continue;
     }
 
-    // Wait for a message from the EStop interrupt routine
-    EstopEventQueueMessage message;
-    if (xQueueReceive(s_estopEventQueue, &message, waitTime) == pdTRUE) {
-      if (message.pressed) {
-        OS_LOGI(TAG, "EStop pressed");
-      } else {
-        OS_LOGI(TAG, "EStop released");
-      }
+    _estopUpdateExternals(s_estopActive, state == EStopState::DeactivatingAwaitingRelease);
+  }
+}
 
-      if (message.deactivatesAtChanged) {
-        OS_LOGI(TAG, "EStop deactivation time changed");
-        deactivatesAt = message.deactivatesAt;
+bool _setEStopEnabledImpl(bool enabled)
+{
+  if (enabled) {
+    if (s_estopTask == nullptr) {
+      if (TaskUtils::TaskCreateUniversal(_estopCheckerTask, TAG, 4096, nullptr, 5, &s_estopTask, 1) != pdPASS) {  // TODO: Profile stack size and set priority
+        OS_LOGE(TAG, "Failed to create EStop event handler task");
+        return false;
       }
-
-      OpenShock::VisualStateManager::SetEmergencyStopStatus(s_estopActive, s_estopAwaitingRelease);
-      OpenShock::CommandHandler::SetKeepAlivePaused(EStopManager::IsEStopped());
-    } else if (deactivatesAt != 0 && OpenShock::millis() >= deactivatesAt) {  // If we didn't get a message, the time probably expired, check if the estop is pending deactivation and if we have reached that time
-      // Reset the deactivation time
-      deactivatesAt = 0;
-
-      // If the button is held for the specified time, clear the EStop
-      s_estopAwaitingRelease = true;
-      OpenShock::VisualStateManager::SetEmergencyStopStatus(s_estopActive, s_estopAwaitingRelease);
-
-      OS_LOGI(TAG, "EStop cleared, awaiting release");
+    }
+  } else {
+    if (s_estopTask != nullptr) {
+      vTaskDelete(s_estopTask);
+      s_estopTask = nullptr;
     }
   }
+
+  return true;
 }
 
-// Interrupt should only be a dumb sender of the GPIO change, additionally triggering if needed
-// Clearing and debouncing is handled by the task.
-void _estopEdgeInterrupt(void* arg) {
-  int64_t now = OpenShock::millis();
+bool _setEStopPinImpl(gpio_num_t pin)
+{
+  esp_err_t err;
 
-  // Debounce the EStop
-  bool debounce = now - s_lastStateChange < k_estopDebounceTime;
-  if (debounce) {
-    return;
+  if (s_estopPin == pin) {
+    return true;
   }
 
-  // TODO: Allow active HIGH EStops?
-  bool pressed = gpio_get_level(s_estopPin) == 0;
-
-  // If the state hasn't changed, ignore it (debounce will skip state changes)
-  if (pressed == s_lastState) {
-    return;
-  }
-  s_lastState       = pressed;
-  s_lastStateChange = now;
-
-  bool deactivatesAtChanged = false;
-  int64_t deactivatesAt     = 0;
-
-  if (!s_estopActive && pressed) {
-    s_estopActive      = true;
-    s_estopActivatedAt = now;
-  } else if (s_estopActive && pressed) {
-    deactivatesAtChanged = true;
-    deactivatesAt        = now + k_estopHoldToClearTime;
-  } else if (s_estopActive && !pressed && s_estopAwaitingRelease) {
-    s_estopActive          = false;
-    s_estopAwaitingRelease = false;
-  } else if (s_estopActive && !pressed) {
-    deactivatesAtChanged = true;
-    deactivatesAt        = 0;
+  if (!OpenShock::IsValidInputPin(pin)) {
+    OS_LOGE(TAG, "Invalid EStop pin: %hhi", static_cast<int8_t>(pin));
+    return false;
   }
 
-  BaseType_t higherPriorityTaskWoken = pdFALSE;
-  EstopEventQueueMessage message     = {
-        .pressed              = pressed,
-        .deactivatesAtChanged = deactivatesAtChanged,
-        .deactivatesAt        = deactivatesAt,
-  };
-
-  xQueueSendToBackFromISR(s_estopEventQueue, &message, &higherPriorityTaskWoken);  // TODO: Check if queue is full?
-
-  if (higherPriorityTaskWoken) {
-    portYIELD_FROM_ISR();
+  bool wasRunning = s_estopTask != nullptr;
+  if (wasRunning) {
+    if (!_setEStopEnabledImpl(false)) {
+      OS_LOGE(TAG, "Failed to disable EStop event handler task");
+      return false;
+    }
   }
-}
 
-bool EStopManager::Init() {
-  bool enabled = false;
-  if (!OpenShock::Config::GetEStopEnabled(enabled)) {
-    OS_LOGE(TAG, "Failed to get EStop enabled from config");
-    return false;
-  }
-  if (!enabled) {
-    OS_LOGI(TAG, "EStop disabled in config");
-    return true;  // TODO: If we never initialize the EStop, how do we do this later for enabling/disabling?
-  }
+  // Configure the new pin
+  gpio_config_t io_conf = {
+    .pin_bit_mask = 1ULL << pin,
+    .mode         = GPIO_MODE_INPUT,
+    .pull_up_en   = GPIO_PULLUP_ENABLE,
+    .pull_down_en = GPIO_PULLDOWN_DISABLE,
+    .intr_type    = GPIO_INTR_DISABLE,
+  };
 
-  gpio_num_t pin = GPIO_NUM_NC;
-  if (!OpenShock::Config::GetEStopGpioPin(pin)) {
-    OS_LOGE(TAG, "Failed to get EStop pin from config");
+  err = gpio_config(&io_conf);
+  if (err != ESP_OK) {
+    OS_LOGE(TAG, "Failed to configure EStop pin");
     return false;
   }
 
-  OS_LOGI(TAG, "Initializing on pin %hhi", static_cast<int8_t>(pin));
+  gpio_num_t oldPin = s_estopPin;
 
-  // TODO?: Should we maybe use statically allocated queues and timers? See CreateStatic for both.
-  s_estopEventQueue = xQueueCreate(8, sizeof(EstopEventQueueMessage));
+  // Set the new pin
+  s_estopPin = pin;
 
-  if (!EStopManager::SetEStopPin(pin)) {
-    OS_LOGE(TAG, "Failed to set EStop pin");
-    return false;
+  if (oldPin != GPIO_NUM_NC) {
+    // Reset the old pin
+    err = gpio_reset_pin(oldPin);
+    if (err != ESP_OK) {
+      OS_LOGE(TAG, "Failed to reset old EStop pin");
+      return false;
+    }
   }
 
-  if (TaskUtils::TaskCreateUniversal(_estopEventHandler, TAG, 4096, nullptr, 5, &s_estopEventHandlerTask, 1) != pdPASS) {
-    OS_LOGE(TAG, "Failed to create EStop event handler task");
-    return false;
+  if (wasRunning) {
+    if (!_setEStopEnabledImpl(true)) {
+      OS_LOGE(TAG, "Failed to re-enable EStop event handler task");
+      return false;
+    }
   }
 
   return true;
 }
 
-bool EStopManager::SetEStopEnabled(bool enabled) {
-  // TODO: Implement
-
-  return true;
-}
-
-bool EStopManager::SetEStopPin(gpio_num_t pin) {
-  if (!OpenShock::IsValidInputPin(pin)) {
-    OS_LOGE(TAG, "Invalid EStop pin: %hhi", static_cast<int8_t>(pin));
-    return false;
+static bool s_estopInitialized = false;
+bool EStopManager::Init()
+{
+  if (s_estopInitialized) {
+    return true;
   }
+  s_estopInitialized = true;
 
-  esp_err_t err = gpio_install_isr_service(ESP_INTR_FLAG_EDGE);
-  if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {  // ESP_ERR_INVALID_STATE is fine, it just means the ISR service is already installed
-    OS_LOGE(TAG, "Failed to install EStop ISR service");
+  Config::EStopConfig cfg;
+  if (!OpenShock::Config::GetEStop(cfg)) {
+    OS_LOGE(TAG, "Failed to get EStop pin from config");
     return false;
   }
 
-  // Configure the new pin
-  gpio_config_t io_conf;
-  io_conf.pin_bit_mask = 1ULL << pin;
-  io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
-  io_conf.pull_up_en   = GPIO_PULLUP_ENABLE;
-  io_conf.mode         = GPIO_MODE_INPUT;
-  io_conf.intr_type    = GPIO_INTR_ANYEDGE;
-  if (gpio_config(&io_conf) != ESP_OK) {
-    OS_LOGE(TAG, "Failed to configure EStop pin");
+  OpenShock::ScopedLock lock(&s_estopMutex);
+
+  if (!_setEStopPinImpl(cfg.gpioPin)) {
+    OS_LOGE(TAG, "Failed to set EStop pin");
     return false;
   }
 
-  // Add the new interrupt
-  err = gpio_isr_handler_add(pin, _estopEdgeInterrupt, nullptr);
-  if (err != ESP_OK) {
-    OS_LOGE(TAG, "Failed to add EStop ISR handler");
+  if (!_setEStopEnabledImpl(cfg.enabled)) {
+    OS_LOGE(TAG, "Failed to create EStop event handler task");
     return false;
   }
 
-  gpio_num_t oldPin = s_estopPin;
+  return true;
+}
 
-  // Set the new pin
-  s_estopPin = pin;
+bool EStopManager::SetEStopEnabled(bool enabled)
+{
+  OpenShock::ScopedLock lock(&s_estopMutex);
 
-  if (oldPin != GPIO_NUM_NC) {
-    // Remove the old interrupt
-    esp_err_t err = gpio_isr_handler_remove(oldPin);
-    if (err != ESP_OK) {
-      OS_LOGE(TAG, "Failed to remove old EStop ISR handler");
+  if (s_estopPin == GPIO_NUM_NC) {
+    gpio_num_t pin;
+    if (!OpenShock::Config::GetEStopGpioPin(pin)) {
+      OS_LOGE(TAG, "Failed to get EStop pin from config");
       return false;
     }
-
-    // Reset the old pin
-    err = gpio_reset_pin(oldPin);
-    if (err != ESP_OK) {
-      OS_LOGE(TAG, "Failed to reset old EStop pin");
+    if (!_setEStopPinImpl(pin)) {
+      OS_LOGE(TAG, "Failed to set EStop pin");
       return false;
     }
   }
 
-  return true;
+  bool success = _setEStopEnabledImpl(enabled);
+
+  return success;
+}
+
+bool EStopManager::SetEStopPin(gpio_num_t pin)
+{
+  OpenShock::ScopedLock lock(&s_estopMutex);
+
+  return _setEStopPinImpl(pin);
 }
 
-bool EStopManager::IsEStopped() {
+bool EStopManager::IsEStopped()
+{
   return s_estopActive;
 }
 
-int64_t EStopManager::LastEStopped() {
+int64_t EStopManager::LastEStopped()
+{
   return s_estopActivatedAt;
 }
diff --git a/src/SimpleMutex.cpp b/src/SimpleMutex.cpp
new file mode 100644
index 00000000..e2d3f9ab
--- /dev/null
+++ b/src/SimpleMutex.cpp
@@ -0,0 +1,25 @@
+#include <freertos/FreeRTOS.h>
+
+#include "SimpleMutex.h"
+
+const char* const TAG = "SimpleMutex";
+
+OpenShock::SimpleMutex::SimpleMutex()
+  : m_mutex(xSemaphoreCreateMutex())
+{
+}
+
+OpenShock::SimpleMutex::~SimpleMutex()
+{
+  vSemaphoreDelete(m_mutex);
+}
+
+bool OpenShock::SimpleMutex::lock(TickType_t xTicksToWait)
+{
+  return xSemaphoreTake(m_mutex, xTicksToWait) == pdTRUE;
+}
+
+void OpenShock::SimpleMutex::unlock()
+{
+  xSemaphoreGive(m_mutex);
+}