diff --git a/src/boards/Groundstation/Automated/Hub.cpp b/src/boards/Groundstation/Automated/Hub.cpp
index 6ffb83f63d9bb5fc4bae2328fc18589e8e3a9f1c..2f1ade0af87438b7777f60f7eb79ec2e3787137a 100644
--- a/src/boards/Groundstation/Automated/Hub.cpp
+++ b/src/boards/Groundstation/Automated/Hub.cpp
@@ -59,20 +59,19 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
             float angle = mavlink_msg_set_stepper_angle_tc_get_angle(&msg);
 
             // The stepper is moved of 'angle' degrees
-            modules.get<Actuators>()->moveDeg(stepperId, angle);
+            modules.get<SMController>()->moveStepperDeg(stepperId, angle);
             sendAck(msg);
             break;
         }
         case MAVLINK_MSG_ID_SET_STEPPER_STEPS_TC:
         {
-            // TODO set in which state we can use this command
 
             StepperList stepperId = static_cast<StepperList>(
                 mavlink_msg_set_stepper_steps_tc_get_stepper_id(&msg));
             int16_t steps = mavlink_msg_set_stepper_steps_tc_get_steps(&msg);
 
             // The stepper is moved of 'steps' steps
-            modules.get<Actuators>()->move(stepperId, steps);
+            modules.get<SMController>()->moveStepperSteps(stepperId, steps);
             sendAck(msg);
             break;
         }
diff --git a/src/boards/Groundstation/Automated/SMController/SMController.cpp b/src/boards/Groundstation/Automated/SMController/SMController.cpp
index 8de5dec2620daf80093d9759b3d76ff4c4b76032..5c366cf4d8b5d730139a97e4547d6d80c450e8c9 100644
--- a/src/boards/Groundstation/Automated/SMController/SMController.cpp
+++ b/src/boards/Groundstation/Automated/SMController/SMController.cpp
@@ -22,6 +22,7 @@
 
 #include "SMController.h"
 
+#include <Groundstation/Automated/Actuators/Actuators.h>
 #include <Groundstation/Automated/Config/FollowerConfig.h>
 #include <Groundstation/Automated/Config/PropagatorConfig.h>
 #include <Groundstation/Automated/Config/SMControllerConfig.h>
@@ -111,6 +112,33 @@ void SMController::setInitialRocketCoordinates(
     }
 }
 
+void SMController::moveStepperDeg(StepperList stepperId, float angle)
+{
+    if (!testState(&SMController::state_test) &&
+        !testState(&SMController::state_test_nf))
+    {
+        LOG_ERR(logger, "Stepper can only be manually moved in the TEST state");
+    }
+    else
+    {
+        ModuleManager::getInstance().get<Actuators>()->moveDeg(stepperId,
+                                                               angle);
+    }
+}
+
+void SMController::moveStepperSteps(StepperList stepperId, int16_t steps)
+{
+    if (!testState(&SMController::state_test) &&
+        !testState(&SMController::state_test_nf))
+    {
+        LOG_ERR(logger, "Stepper can only be manually moved in the TEST state");
+    }
+    else
+    {
+        ModuleManager::getInstance().get<Actuators>()->move(stepperId, steps);
+    }
+}
+
 void SMController::update()
 {
     switch (status.state)
diff --git a/src/boards/Groundstation/Automated/SMController/SMController.h b/src/boards/Groundstation/Automated/SMController/SMController.h
index 37cf858562ecd53bd3cdc1e266fa90783a056cda..2e41fda2061f1c0ee6e39fa630a98859aebb99de 100644
--- a/src/boards/Groundstation/Automated/SMController/SMController.h
+++ b/src/boards/Groundstation/Automated/SMController/SMController.h
@@ -22,6 +22,7 @@
 
 #pragma once
 
+#include <Groundstation/Automated/Actuators/Actuators.h>
 #include <Groundstation/Automated/Follower/Follower.h>
 #include <algorithms/NAS/NASState.h>
 #include <algorithms/Propagator/Propagator.h>
@@ -76,12 +77,27 @@ public:
     void setInitialRocketCoordinates(
         const Boardcore::GPSData& antennaCoordinates);
 
-    // Starts the FSM thread and adds an update function into the scheduler
+    /**
+     * @brief Starts the FSM thread and adds an update function into the
+     * scheduler
+     */
     bool start() override;
 
-    // Update routine called by the scheduler
+    /**
+     * @brief Update routine called by the scheduler
+     */
     void update();
 
+    /**
+     * @brief move the stepper `stepperId` of `angle` degrees
+     */
+    void moveStepperDeg(StepperList stepperId, float angle);
+
+    /**
+     * @brief move the stepper `stepperId` of `steps` steps
+     */
+    void moveStepperSteps(StepperList stepperId, int16_t steps);
+
 private:
     /**
      * @brief Logs the current state of the FSM