diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp
index db8cf271..2f8c6410 100644
--- a/Firmware/MotorControl/endstop.cpp
+++ b/Firmware/MotorControl/endstop.cpp
@@ -3,24 +3,25 @@
 Endstop::Endstop(Endstop::Config_t& config)
     : config_(config) {
     update_config();
+    debounceTimer_.setInterval(current_meas_period);
 }
 
+
 void Endstop::update() {
-    uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
-    GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
-    bool last_pin_state = pin_state_;
-    pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
-    uint32_t now = static_cast<uint32_t>(axis_->loop_counter_ * current_meas_period * 1000);
-    if (pin_state_ != last_pin_state) {
-        debounce_timer_ = now;
-    }
+    debounceTimer_.update();
     if (config_.enabled) {
-        if ((now - debounce_timer_) >= config_.debounce_ms) {         // Debounce timer expired, take the new pin state
+        bool last_pin_state = pin_state_;
+
+        uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
+        GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
+        pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
+
+        // If the pin state has changed, reset the timer
+        if (pin_state_ != last_pin_state)
+            debounceTimer_.reset();
+
+        if (debounceTimer_.expired())
             endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_;  // endstop_state is the logical state
-            debounce_timer_ = now - config_.debounce_ms;              // Ensure timer doesn't have overflow issues
-        } else {
-            endstop_state_ = endstop_state_;  // Do nothing
-        }
     } else {
         endstop_state_ = false;
     }
@@ -30,11 +31,13 @@ bool Endstop::get_state() {
     return endstop_state_;
 }
 
-void Endstop::update_config(){
+void Endstop::update_config() {
     set_enabled(config_.enabled);
+    debounceTimer_.setInterval(config_.debounce_ms * 0.001f);
 }
 
 void Endstop::set_enabled(bool enable) {
+    debounceTimer_.reset();
     if (config_.gpio_num != 0) {
         uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
         GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
@@ -45,6 +48,8 @@ void Endstop::set_enabled(bool enable) {
             GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
             GPIO_InitStruct.Pull = config_.pullup ? GPIO_PULLUP : GPIO_PULLDOWN;
             HAL_GPIO_Init(gpio_port, &GPIO_InitStruct);
-        }
+            debounceTimer_.start();
+        } else
+            debounceTimer_.stop();
     }
 }
\ No newline at end of file
diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp
index b5c6e6d8..02f116b7 100644
--- a/Firmware/MotorControl/endstop.hpp
+++ b/Firmware/MotorControl/endstop.hpp
@@ -1,6 +1,7 @@
 #ifndef __ENDSTOP_HPP
 #define __ENDSTOP_HPP
 
+#include "timer.hpp"
 class Endstop {
    public:
     struct Config_t {
@@ -36,12 +37,13 @@ class Endstop {
                 make_protocol_property("offset", &config_.offset),
                 make_protocol_property("is_active_high", &config_.is_active_high),
                 make_protocol_property("pullup", &config_.pullup),
-                make_protocol_property("debounce_ms", &config_.debounce_ms)));
+                make_protocol_property("debounce_ms", &config_.debounce_ms,
+                                    [](void* ctx) { static_cast<Endstop*>(ctx)->update_config(); }, this)));
     }
 
    private:
     bool pin_state_                = false;
     float pos_when_pressed_        = 0.0f;
-    uint32_t debounce_timer_ = 0;
+    Timer debounceTimer_;
 };
 #endif
\ No newline at end of file
diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h
index ff8199c9..72097f36 100644
--- a/Firmware/MotorControl/odrive_main.h
+++ b/Firmware/MotorControl/odrive_main.h
@@ -33,9 +33,12 @@ extern "C" {
 //default timeout waiting for phase measurement signals
 #define PH_CURRENT_MEAS_TIMEOUT 2 // [ms]
 
-//TODO clean this up
+// Period in [s]
 static const float current_meas_period = CURRENT_MEAS_PERIOD;
+
+// Frequency in [Hz]
 static const int current_meas_hz = CURRENT_MEAS_HZ;
+
 // extern const float elec_rad_per_enc;
 extern uint32_t _reboot_cookie;
 extern bool user_config_loaded_;