diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp
index f0200b71..9a5eb90d 100644
--- a/Firmware/communication/ascii_protocol.cpp
+++ b/Firmware/communication/ascii_protocol.cpp
@@ -176,6 +176,7 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
         } else {
             Axis* axis = axes[motor_number];
             axis->controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
+            axis->controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL;
             axis->controller_.input_pos_ = goal_point;
             axis->controller_.input_pos_updated();
             axis->watchdog_feed();