diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index dcd21b55..76420adf 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -317,6 +317,11 @@ bool Axis::run_closed_loop_control_loop() {
         // Handle the homing case
         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_.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);
 
                 controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL;