diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index fbef6135..a2bae293 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -316,11 +316,11 @@ bool Axis::run_closed_loop_control_loop() {
         if (homing_.homing_state == HOMING_STATE_HOMING) {
             if (min_endstop_.getEndstopState()) {
                 // pos_setpoint is the starting position for the trap_traj so we need to set it.
-                controller_.pos_setpoint_ = min_endstop_.config_.offset;
+                controller_.pos_setpoint_ = min_endstop_.config_.offset - (controller_.config_.homing_speed * (min_endstop_.config_.debounce_ms / 1000.0f));
                 controller_.vel_setpoint_ = 0.0f;  // Change directions without decelerating
 
                 // Set our current position in encoder counts to make control more logical
-                encoder_.set_linear_count(min_endstop_.config_.offset - static_cast<int32_t>(controller_.config_.homing_speed * min_endstop_.config_.debounce_ms / 1000.0f));
+                encoder_.set_linear_count(static_cast<int32_t>(controller_.pos_setpoint_));
 
                 controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL;
                 controller_.config_.input_mode   = Controller::INPUT_MODE_TRAP_TRAJ;
diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp
index e30ab3d7..45195a2f 100644
--- a/Firmware/MotorControl/endstop.hpp
+++ b/Firmware/MotorControl/endstop.hpp
@@ -4,11 +4,11 @@
 class Endstop {
    public:
     struct Config_t {
+        float offset      = 0;
+        float debounce_ms = 50.0f;
         uint16_t gpio_num;
         bool enabled        = false;
-        int32_t offset      = 0;
         bool is_active_high = false;
-        float debounce_ms   = 50.0f;
     };
 
     Endstop(Endstop::Config_t& config);