diff --git a/src/Parafoil/Actuators/Actuators.cpp b/src/Parafoil/Actuators/Actuators.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7b25f46ecf543af56057ec9e326a0dbc859e6b14
--- /dev/null
+++ b/src/Parafoil/Actuators/Actuators.cpp
@@ -0,0 +1,186 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "Actuators.h"
+
+#include <Parafoil/BoardScheduler.h>
+#include <Parafoil/Configs/ActuatorsConfig.h>
+#include <interfaces-impl/hwmapping.h>
+
+using namespace std::chrono;
+using namespace miosix;
+using namespace Boardcore;
+namespace config = Parafoil::Config::Actuators;
+
+namespace Parafoil
+{
+
+Actuators::Actuators()
+{
+    leftServo.servo = std::make_unique<Servo>(
+        config::LeftServo::TIMER, config::LeftServo::PWM_CH,
+        config::LeftServo::MIN_PULSE.count(),
+        config::LeftServo::MAX_PULSE.count());
+    leftServo.fullRangeAngle = config::LeftServo::ROTATION.value();
+
+    rightServo.servo = std::make_unique<Servo>(
+        config::RightServo::TIMER, config::RightServo::PWM_CH,
+        config::RightServo::MIN_PULSE.count(),
+        config::RightServo::MAX_PULSE.count());
+    rightServo.fullRangeAngle = config::RightServo::ROTATION.value();
+}
+
+bool Actuators::start()
+{
+    using namespace std::chrono;
+
+    auto& scheduler = getModule<BoardScheduler>()->actuators();
+
+    leftServo.servo->enable();
+    rightServo.servo->enable();
+
+    leftServo.servo->setPosition(0);
+    rightServo.servo->setPosition(0);
+
+    started = true;
+    return true;
+}
+
+bool Actuators::isStarted() { return started; }
+
+bool Actuators::setServoPosition(ServosList servoId, float position)
+{
+    auto actuator = getServoActuator(servoId);
+    if (!actuator)
+        return false;
+
+    miosix::Lock<miosix::FastMutex> lock(actuator->mutex);
+
+    actuator->servo->setPosition(position);
+
+    return true;
+}
+
+bool Actuators::setServoAngle(ServosList servoId, float angle)
+{
+    auto actuator = getServoActuator(servoId);
+    if (!actuator)
+        return false;
+
+    miosix::Lock<miosix::FastMutex> lock(actuator->mutex);
+
+    actuator->servo->setPosition(angle / actuator->fullRangeAngle);
+
+    return true;
+}
+
+float Actuators::getServoPosition(ServosList servoId)
+{
+    auto actuator = getServoActuator(servoId);
+    if (!actuator)
+        return -1.f;
+
+    miosix::Lock<miosix::FastMutex> lock(actuator->mutex);
+
+    return actuator->servo->getPosition();
+}
+
+float Actuators::getServoAngle(ServosList servoId)
+{
+    auto actuator = getServoActuator(servoId);
+    if (!actuator)
+        return -1.f;
+
+    miosix::Lock<miosix::FastMutex> lock(actuator->mutex);
+
+    return actuator->servo->getPosition() * actuator->fullRangeAngle;
+}
+
+bool Actuators::wiggleServo(ServosList servoId)
+{
+    auto actuator = getServoActuator(servoId);
+    if (!actuator)
+        return false;
+
+    miosix::Lock<miosix::FastMutex> lock(actuator->mutex);
+
+    actuator->servo->setPosition(1.0f);
+    Thread::sleep(1000);
+    actuator->servo->setPosition(0.0f);
+
+    return true;
+}
+
+bool Actuators::enableServo(ServosList servoId)
+{
+    auto actuator = getServoActuator(servoId);
+    if (!actuator)
+        return false;
+
+    miosix::Lock<miosix::FastMutex> lock(actuator->mutex);
+
+    actuator->servo->enable();
+
+    return true;
+}
+
+bool Actuators::disableServo(ServosList servoId)
+{
+    auto actuator = getServoActuator(servoId);
+    if (!actuator)
+        return false;
+
+    miosix::Lock<miosix::FastMutex> lock(actuator->mutex);
+
+    actuator->servo->disable();
+
+    return true;
+}
+
+Actuators::ServoActuator* Actuators::getServoActuator(ServosList servoId)
+{
+    switch (servoId)
+    {
+        case PARAFOIL_LEFT_SERVO:
+            assert(leftServo.servo);
+            return &leftServo;
+        case PARAFOIL_RIGHT_SERVO:
+            assert(rightServo.servo);
+            return &rightServo;
+        default:
+            return nullptr;
+    }
+}
+
+void Actuators::startTwirl()
+{
+    leftServo.servo->setPosition(config::SERVO_TWIRL_RADIUS);
+    rightServo.servo->setPosition(0);
+}
+
+void Actuators::stopTwirl()
+{
+    leftServo.servo->setPosition(0);
+    rightServo.servo->setPosition(0);
+}
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/Actuators/Actuators.h b/src/Parafoil/Actuators/Actuators.h
new file mode 100644
index 0000000000000000000000000000000000000000..524a42edef2ebbfd84539a18f5d16139619cc982
--- /dev/null
+++ b/src/Parafoil/Actuators/Actuators.h
@@ -0,0 +1,138 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Federico Lolli, Angelo Prete, Niccolò Betto, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <actuators/Servo/Servo.h>
+#include <common/MavlinkOrion.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Parafoil
+{
+class BoardScheduler;
+
+class Actuators : public Boardcore::InjectableWithDeps<BoardScheduler>
+{
+public:
+    /**
+     * @brief A small struct to hold a servo along with per-servo data.
+     */
+    struct ServoActuator
+    {
+        std::unique_ptr<Boardcore::Servo> servo;
+        float fullRangeAngle;  ///< The full range of the servo [degrees]
+        miosix::FastMutex mutex;
+    };
+
+    Actuators();
+
+    [[nodiscard]] bool start();
+
+    bool isStarted();
+
+    /**
+     * @brief Moves the specified servo to the given position.
+     *
+     * @param servoId Servo to move.
+     * @param percentage Angle to set [0-1].
+     * @return True if the the angle was set.
+     */
+    bool setServoPosition(ServosList servoId, float percentage);
+
+    /**
+     * @brief Moves the specified servo to the given position.
+     *
+     * @param servoId Servo to move.
+     * @param angle Angle to set [degree].
+     * @return True if the the angle was set.
+     */
+    bool setServoAngle(ServosList servoId, float angle);
+
+    /**
+     * @brief Wiggles the servo for few seconds.
+     *
+     * @param servoId Servo to move.
+     * @return true
+     * @return false
+     */
+    bool wiggleServo(ServosList servoId);
+
+    /**
+     * @brief Enables the specified servo.
+     *
+     * @param servoId Servo to enable.
+     * @return True if the servo was enabled.
+     * @return False if the servoId is invalid.
+     */
+    bool enableServo(ServosList servoId);
+
+    /**
+     * @brief Disables the specified servo.
+     *
+     * @param servoId Servo to disable.
+     * @return True if the servo was disabled.
+     * @return False if the servoId is invalid.
+     */
+    bool disableServo(ServosList servoId);
+
+    /**
+     * @brief Returns the current position of the specified servo.
+     *
+     * @param servoId Servo to read.
+     * @return float current Servo position in range [0-1], (-1) if the servoId
+     * is invalid.
+     */
+    float getServoPosition(ServosList servoId);
+
+    /**
+     * @brief Returns the current angle of the specified servo.
+     *
+     * @param servoId Servo to read.
+     * @return float current Servo angle in range [0-180], (-1) if the servoId
+     * is invalid.
+     */
+    float getServoAngle(ServosList servoId);
+
+    /**
+     * @brief Starts twirl (one servo is set to 0 and the other one is not).
+     */
+    void startTwirl();
+
+    /**
+     * @brief Stops twirling (both servos are set to 0).
+     */
+    void stopTwirl();
+
+private:
+    ServoActuator* getServoActuator(ServosList servoId);
+
+    ServoActuator leftServo;
+    ServoActuator rightServo;
+
+    std::atomic<bool> started{false};
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Actuators");
+};
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/BoardScheduler.h b/src/Parafoil/BoardScheduler.h
index cd331b343cb1718e8df0ba101324af21c60e2f67..75a8d06c7a57af2d3cccded0f97c2adf9eae997c 100644
--- a/src/Parafoil/BoardScheduler.h
+++ b/src/Parafoil/BoardScheduler.h
@@ -55,6 +55,7 @@ public:
     Boardcore::TaskScheduler& nasController() { return critical; }
     Boardcore::TaskScheduler& sensors() { return high; }
     Boardcore::TaskScheduler& pinHandler() { return high; }
+    Boardcore::TaskScheduler& actuators() { return low; }
 
     static Priority::PriorityLevel flightModeManagerPriority()
     {
diff --git a/src/Parafoil/Configs/ActuatorsConfig.h b/src/Parafoil/Configs/ActuatorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..915a2d757802064cdf08e1293ff7002a0b214f47
--- /dev/null
+++ b/src/Parafoil/Configs/ActuatorsConfig.h
@@ -0,0 +1,68 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <drivers/timer/TimerUtils.h>
+#include <units/Angle.h>
+
+#include <chrono>
+
+namespace Parafoil
+{
+namespace Config
+{
+namespace Actuators
+{
+
+/* linter off */ using namespace std::chrono_literals;
+/* linter off */ using namespace Boardcore::Units::Angle;
+
+namespace LeftServo
+{
+static TIM_TypeDef* const TIMER = TIM4;
+constexpr Boardcore::TimerUtils::Channel PWM_CH =
+    Boardcore::TimerUtils::Channel::CHANNEL_1;
+
+constexpr auto ROTATION  = 180_deg;
+constexpr auto MIN_PULSE = 500us;
+constexpr auto MAX_PULSE = 2460us;
+}  // namespace LeftServo
+
+namespace RightServo
+{
+static TIM_TypeDef* const TIMER = TIM8;
+constexpr Boardcore::TimerUtils::Channel PWM_CH =
+    Boardcore::TimerUtils::Channel::CHANNEL_2;
+
+constexpr auto ROTATION  = 180_deg;
+constexpr auto MIN_PULSE = 2460us;
+constexpr auto MAX_PULSE = 500us;
+}  // namespace RightServo
+
+constexpr auto SERVO_TWIRL_RADIUS = 0.5f;  // [%]
+
+// Status configs
+
+}  // namespace Actuators
+}  // namespace Config
+}  // namespace Parafoil
diff --git a/src/Parafoil/Configs/SensorsConfig.h b/src/Parafoil/Configs/SensorsConfig.h
index d5906dd378db620a635978c4aa7a38ff0b72a2a6..3147326619b4fd54a39ed6346670f5c5661301ca 100644
--- a/src/Parafoil/Configs/SensorsConfig.h
+++ b/src/Parafoil/Configs/SensorsConfig.h
@@ -126,4 +126,4 @@ constexpr auto CALIBRATION_PATH = "/sd/bmx160_magnetometer_calibration.csv";
 
 }  // namespace Config
 
-}  // namespace Parafoil
\ No newline at end of file
+}  // namespace Parafoil
diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp
index 0b5451b3f21aa99ced8bdbfd34373ff5ecf9679b..da56cf30367c899643e4c84942705789616ac10d 100644
--- a/src/Parafoil/parafoil-entry.cpp
+++ b/src/Parafoil/parafoil-entry.cpp
@@ -20,11 +20,13 @@
  * THE SOFTWARE.
  */
 
+#include <Parafoil/Actuators/Actuators.h>
 #include <Parafoil/BoardScheduler.h>
 #include <Parafoil/Buses.h>
 #include <Parafoil/PinHandler/PinHandler.h>
 #include <Parafoil/Sensors/Sensors.h>
 #include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Parafoil/StateMachines/NASController/NASController.h>
 #include <common/Events.h>
 #include <common/Topics.h>
 #include <diagnostic/PrintLogger.h>
@@ -98,6 +100,10 @@ int main()
     auto flightModeManager = new FlightModeManager();
     initResult &= depman.insert(flightModeManager);
 
+    // Attitude estimation
+    auto nasController = new NASController();
+    initResult &= depman.insert(nasController);
+
     // Sensors
     auto sensors = new Sensors();
     initResult &= depman.insert(sensors);
@@ -106,7 +112,11 @@ int main()
 
     // TODO: Radio
 
-    // TODO: Flight algorithms
+    // TODO: Wing Algorithm
+
+    // Actuators
+    auto actuators = new Actuators();
+    initResult &= depman.insert(actuators);
 
     START_SINGLETON(Logger)
     {
@@ -116,15 +126,13 @@ int main()
     }
 
     START_MODULE(flightModeManager);
-    // START_MODULE(pinHandler);
+    START_MODULE(pinHandler);
     // START_MODULE(radio) { miosix::led2On(); }
-    // START_MODULE(canHandler) { miosix::led3On(); }
-    // START_MODULE(nasController);
-    // START_MODULE(altitudeTrigger);
+    START_MODULE(nasController);
     // START_MODULE(wingController);
-    // START_MODULE(actuators);
+    START_MODULE(actuators);
 
-    // START_MODULE(scheduler);
+    START_MODULE(scheduler);
 
     START_MODULE(sensors);
 
@@ -133,13 +141,11 @@ int main()
         EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
         // Turn on the initialization led on the CU
         miosix::ledOn();
-        // TODO: actuators->setStatusOk();
         std::cout << "Payload initialization Ok!" << std::endl;
     }
     else
     {
         EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM);
-        // TODO: actuators->setStatusError();
         std::cerr << "*** Payload initialization error ***" << std::endl;
     }