diff --git a/CMakeLists.txt b/CMakeLists.txt
index af7fb520a42518fdf2ffd12661669be1836707ba..1b31cb98642320cf129d5f48817efa4a2bd20cd5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -60,7 +60,7 @@ sbs_target(payload-entry-euroc stm32f767zi_death_stack_v4)
 
 add_executable(motor-entry src/entrypoints/Motor/motor-entry.cpp ${MOTOR_SOURCES})
 target_include_directories(motor-entry PRIVATE ${OBSW_INCLUDE_DIRS})
-sbs_target(motor-entry stm32f767zi_gemini_motor)
+sbs_target(motor-entry stm32f767zi_lyra_motor)
 
 add_executable(rig-entry src/entrypoints/RIG/rig-entry.cpp ${RIG_COMPUTER})
 target_include_directories(rig-entry PRIVATE ${OBSW_INCLUDE_DIRS})
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index b6d1c4caa7ef76f225bac38751994a2274d82192..c18e37c5e5dd2662f12ed506911b1c99e09c75b4 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -44,9 +44,9 @@ set(MAIN_COMPUTER
 )
 
 set(MOTOR_SOURCES
-    src/boards/Motor/Actuators/Actuators.cpp
+    # src/boards/Motor/Actuators/Actuators.cpp
     src/boards/Motor/Sensors/Sensors.cpp
-    src/boards/Motor/BoardScheduler.cpp
+    # src/boards/Motor/BoardScheduler.cpp
     src/boards/Motor/CanHandler/CanHandler.cpp
 ) 
 
diff --git a/src/boards/Motor/Buses.h b/src/boards/Motor/Buses.h
index 899fde6bbc0cf07e07fff4fc0588414ab5aef839..41b06f7c2fff68f5ea1d1c0a438d3cf50f658559 100644
--- a/src/boards/Motor/Buses.h
+++ b/src/boards/Motor/Buses.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Alberto Nidasio
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -24,7 +24,6 @@
 
 #include <drivers/spi/SPIBus.h>
 #include <drivers/usart/USART.h>
-#include <miosix.h>
 
 #include <utils/ModuleManager/ModuleManager.hpp>
 
@@ -34,13 +33,22 @@ namespace Motor
 class Buses : public Boardcore::Module
 {
 public:
-    Boardcore::SPIBus spi1;
-    Boardcore::SPIBus spi3;
-    Boardcore::SPIBus spi4;
+    Buses() {}
 
-    Boardcore::USART usart2;
+    Boardcore::SPIBus &getH3LIS331DL() { return spi1; }
+    Boardcore::SPIBus &getLPS22DF() { return spi1; }
+    Boardcore::SPIBus &getLIS2MDL() { return spi3; }
+    Boardcore::SPIBus &getLSM6DSRX() { return spi3; }
+    Boardcore::SPIBus &getADS131M08() { return spi4; }
 
-    Buses() : spi1(SPI1), spi3(SPI3), spi4(SPI4), usart2(USART2, 115200) {}
+    Boardcore::USART &getHILUart() { return usart4; }
+
+private:
+    Boardcore::SPIBus spi1{SPI1};
+    Boardcore::SPIBus spi3{SPI3};
+    Boardcore::SPIBus spi4{SPI4};
+
+    Boardcore::USART usart4{UART4, 460800, 1024};
 };
 
-}  // namespace Motor
\ No newline at end of file
+}
\ No newline at end of file
diff --git a/src/boards/Motor/CanHandler/CanHandler.cpp b/src/boards/Motor/CanHandler/CanHandler.cpp
index 0ef05206248bcfc0996d8ac70e146d2d01a5a05a..0cda12469670620d2ac4ba7c5212974af552837c 100644
--- a/src/boards/Motor/CanHandler/CanHandler.cpp
+++ b/src/boards/Motor/CanHandler/CanHandler.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Authors: Federico Mandelli, Alberto Nidasio, Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -22,243 +22,48 @@
 
 #include "CanHandler.h"
 
-#include <Motor/Actuators/Actuators.h>
-#include <Motor/Configs/CanHandlerConfig.h>
-#include <Motor/Sensors/Sensors.h>
 #include <common/CanConfig.h>
-#include <common/Events.h>
 
-#include <functional>
-
-using namespace std;
-using namespace placeholders;
+using namespace Motor;
 using namespace Boardcore;
-using namespace Canbus;
+using namespace Boardcore::Canbus;
 using namespace Common;
-using namespace CanConfig;
-using namespace Motor::CanHandlerConfig;
-
-namespace Motor
-{
 
-CanHandler::CanHandler(TaskScheduler *sched) : scheduler(sched)
+CanHandler::CanHandler()
 {
-    // Configure the CAN driver
     CanbusDriver::AutoBitTiming bitTiming;
-    bitTiming.baudRate    = BAUD_RATE;
-    bitTiming.samplePoint = SAMPLE_POINT;
-    CanbusDriver::CanbusConfig config;
-
-    driver = new CanbusDriver(CAN2, config, bitTiming);
-
-    // Create the protocol with the defined driver
-    protocol =
-        new CanProtocol(driver, bind(&CanHandler::handleCanMessage, this, _1),
-                        miosix::PRIORITY_MAX - 1);
-
-    // Accept messages only from the main and RIG board
-    protocol->addFilter(static_cast<uint8_t>(Board::MAIN),
-                        static_cast<uint8_t>(Board::BROADCAST));
-    protocol->addFilter(static_cast<uint8_t>(Board::RIG),
-                        static_cast<uint8_t>(Board::BROADCAST));
-    driver->init();
-}
-
-bool CanHandler::start()
-{
-    // 0 if fail
-    auto result1 = scheduler->addTask(
-        [&]()
-        {
-            auto sensors = ModuleManager::getInstance().get<Sensors>();
-
-            protocol->enqueueData(static_cast<uint8_t>(Priority::HIGH),
-                                  static_cast<uint8_t>(PrimaryType::SENSORS),
-                                  static_cast<uint8_t>(Board::MOTOR),
-                                  static_cast<uint8_t>(Board::BROADCAST),
-                                  static_cast<uint8_t>(SensorId::CC_PRESSURE),
-                                  static_cast<PressureData>(
-                                      sensors->getChamberPressureSensorData()));
-
-            protocol->enqueueData(
-                static_cast<uint8_t>(Priority::HIGH),
-                static_cast<uint8_t>(PrimaryType::SENSORS),
-                static_cast<uint8_t>(Board::MOTOR),
-                static_cast<uint8_t>(Board::BROADCAST),
-                static_cast<uint8_t>(SensorId::BOTTOM_TANK_PRESSURE),
-                static_cast<PressureData>(
-                    sensors->getTankPressureSensor1Data()));
-
-            protocol->enqueueData(
-                static_cast<uint8_t>(Priority::HIGH),
-                static_cast<uint8_t>(PrimaryType::SENSORS),
-                static_cast<uint8_t>(Board::MOTOR),
-                static_cast<uint8_t>(Board::BROADCAST),
-                static_cast<uint8_t>(SensorId::TOP_TANK_PRESSURE),
-                static_cast<PressureData>(
-                    sensors->getTankPressureSensor2Data()));
-        },
-        PRESSURES_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER);
-
-    auto result2 = scheduler->addTask(
-        [&]()
-        {
-            auto sensors = ModuleManager::getInstance().get<Sensors>();
-
-            protocol->enqueueData(
-                static_cast<uint8_t>(Priority::MEDIUM),
-                static_cast<uint8_t>(PrimaryType::SENSORS),
-                static_cast<uint8_t>(Board::MOTOR),
-                static_cast<uint8_t>(Board::BROADCAST),
-                static_cast<uint8_t>(SensorId::TANK_TEMPERATURE),
-                static_cast<TemperatureData>(sensors->getMAX31856Data()));
-
-            protocol->enqueueData(
-                static_cast<uint8_t>(Priority::MEDIUM),
-                static_cast<uint8_t>(PrimaryType::SENSORS),
-                static_cast<uint8_t>(Board::MOTOR),
-                static_cast<uint8_t>(Board::BROADCAST),
-                static_cast<uint8_t>(SensorId::MOTOR_ACTUATORS_CURRENT),
-                static_cast<CurrentData>(sensors->getServoCurrentData()));
-
-            protocol->enqueueData(
-                static_cast<uint8_t>(Priority::MEDIUM),
-                static_cast<uint8_t>(PrimaryType::SENSORS),
-                static_cast<uint8_t>(Board::MOTOR),
-                static_cast<uint8_t>(Board::BROADCAST),
-                static_cast<uint8_t>(SensorId::MOTOR_BOARD_VOLTAGE),
-                static_cast<BatteryVoltageSensorData>(
-                    sensors->getBatteryData()));
-        },
-        TEMPERATURE_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER);
+    bitTiming.baudRate    = CanConfig::BAUD_RATE;
+    bitTiming.samplePoint = CanConfig::SAMPLE_POINT;
 
-    auto result3 = scheduler->addTask(
-        [&]()
-        {
-            protocol->enqueueSimplePacket(
-                static_cast<uint8_t>(Priority::MEDIUM),
-                static_cast<uint8_t>(PrimaryType::STATUS),
-                static_cast<uint8_t>(Board::MOTOR),
-                static_cast<uint8_t>(Board::BROADCAST), initStatus, initStatus);
-        },
-        STATUS_TRANSMISSION_PERIOD);
+    CanbusDriver::CanbusConfig config;
 
-    auto result4 = scheduler->addTask(
-        [&]()
-        {
-            ModuleManager &modules = ModuleManager::getInstance();
+    driver = std::make_unique<CanbusDriver>(CAN1, config, bitTiming);
 
-            protocol->enqueueData(
-                static_cast<uint8_t>(Priority::HIGH),
-                static_cast<uint8_t>(PrimaryType::ACTUATORS),
-                static_cast<uint8_t>(Board::MOTOR),
-                static_cast<uint8_t>(Board::BROADCAST),
-                static_cast<uint8_t>(ServosList::MAIN_VALVE),
-                ServoData{TimestampTimer::getTimestamp(), 0, 0,
-                          modules.get<Actuators>()->getServoPosition(
-                              ServosList::MAIN_VALVE)});
+    protocol = std::make_unique<CanProtocol>(
+        driver.get(),
+        [this](const Canbus::CanMessage &msg) { handleCanMessage(msg); }, 3);
 
-            protocol->enqueueData(
-                static_cast<uint8_t>(Priority::HIGH),
-                static_cast<uint8_t>(PrimaryType::ACTUATORS),
-                static_cast<uint8_t>(Board::MOTOR),
-                static_cast<uint8_t>(Board::BROADCAST),
-                static_cast<uint8_t>(ServosList::VENTING_VALVE),
-                ServoData{TimestampTimer::getTimestamp(), 0, 0,
-                          modules.get<Actuators>()->getServoPosition(
-                              ServosList::VENTING_VALVE)});
-        },
-        ACTUATORS_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER);
+    protocol->addFilter(static_cast<uint8_t>(CanConfig::Board::RIG),
+                        static_cast<uint8_t>(CanConfig::Board::BROADCAST));
+    protocol->addFilter(static_cast<uint8_t>(CanConfig::Board::MAIN),
+                        static_cast<uint8_t>(CanConfig::Board::BROADCAST));
 
-    return protocol->start() && result1 != 0 && result2 != 0 && result3 != 0 &&
-           result4 != 0;
+    driver->init();
 }
 
-bool CanHandler::isStarted()
-{
-    return protocol->isStarted() && scheduler->isRunning();
-}
+bool CanHandler::start() { return protocol->start(); }
 
-void CanHandler::sendEvent(EventId event)
+void CanHandler::sendEvent(Common::CanConfig::EventId event)
 {
-    protocol->enqueueEvent(static_cast<uint8_t>(Priority::CRITICAL),
-                           static_cast<uint8_t>(PrimaryType::EVENTS),
-                           static_cast<uint8_t>(Board::MOTOR),
-                           static_cast<uint8_t>(Board::BROADCAST),
+    protocol->enqueueEvent(static_cast<uint8_t>(CanConfig::Priority::CRITICAL),
+                           static_cast<uint8_t>(CanConfig::PrimaryType::EVENTS),
+                           static_cast<uint8_t>(CanConfig::Board::MAIN),
+                           static_cast<uint8_t>(CanConfig::Board::BROADCAST),
                            static_cast<uint8_t>(event));
 }
 
-void CanHandler::setInitStatus(bool initResult) { initStatus = initResult; }
-
-void CanHandler::handleCanMessage(const CanMessage &msg)
+void CanHandler::handleCanMessage(const Canbus::CanMessage &msg)
 {
-    PrimaryType msgType = static_cast<PrimaryType>(msg.getPrimaryType());
-
-    // Depending on the received message, call the handling method
-    switch (msgType)
-    {
-        case PrimaryType::EVENTS:
-        {
-            handleCanEvent(msg);
-            break;
-        }
-        case PrimaryType::COMMAND:
-        {
-            handleCanCommand(msg);
-            break;
-        }
-        default:
-        {
-            LOG_WARN(logger, "Received unsupported message type: type={}",
-                     msgType);
-            break;
-        }
-    }
+    LOG_INFO(logger, "Received can message {}", msg.getPrimaryType(),
+             msg.getSecondaryType());
 }
-
-void CanHandler::handleCanEvent(const CanMessage &msg)
-{
-    EventId eventId = static_cast<EventId>(msg.getSecondaryType());
-
-    switch (eventId)
-    {
-        case EventId::CALIBRATE:
-        {
-            auto sensors = ModuleManager::getInstance().get<Sensors>();
-            sensors->calibrate();
-
-            break;
-        }
-        default:
-        {
-            LOG_WARN(logger, "Received unsupported event: id={}", eventId);
-            break;
-        }
-    }
-}
-
-void CanHandler::handleCanCommand(const CanMessage &msg)
-{
-    uint64_t payload = msg.payload[0];
-    ServosList servo = static_cast<ServosList>(msg.getSecondaryType());
-    bool targetState = static_cast<uint8_t>(payload);
-    uint32_t delay   = static_cast<uint32_t>(payload >> 8);
-
-    auto actuators = ModuleManager::getInstance().get<Actuators>();
-
-    if (actuators != nullptr)
-    {
-        if (targetState)
-        {
-            // printf("Opening %d for %ld\n", msg.getSecondaryType(), delay);
-            actuators->openServoAtomic(servo, delay);
-        }
-        else
-        {
-            // printf("Closing %d for %ld\n", msg.getSecondaryType(), delay);
-            actuators->closeServo(servo);
-        }
-    }
-}
-
-}  // namespace Motor
diff --git a/src/boards/Motor/CanHandler/CanHandler.h b/src/boards/Motor/CanHandler/CanHandler.h
index 1ccc2b90a4c43e77250b634882491a866f71e2b5..309f169b818ac218e669b5a66280609f008f6e72 100644
--- a/src/boards/Motor/CanHandler/CanHandler.h
+++ b/src/boards/Motor/CanHandler/CanHandler.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Authors: Federico Mandelli, Alberto Nidasio
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -22,9 +22,8 @@
 
 #pragma once
 
-#include <common/CanConfig.h>
 #include <drivers/canbus/CanProtocol/CanProtocol.h>
-#include <scheduler/TaskScheduler.h>
+#include <common/CanConfig.h>
 
 #include <utils/ModuleManager/ModuleManager.hpp>
 
@@ -33,52 +32,20 @@ namespace Motor
 
 class CanHandler : public Boardcore::Module
 {
-
 public:
-    explicit CanHandler(Boardcore::TaskScheduler *sched);
+    CanHandler();
 
-    /**
-     * @brief Adds the periodic task to the scheduler and starts the protocol
-     * threads
-     */
     bool start();
 
-    /**
-     * @brief Returns true if the protocol threads are started and the scheduler
-     * is running
-     */
-    bool isStarted();
-
-    /**
-     * @brief Sends a CAN event on the bus
-     */
     void sendEvent(Common::CanConfig::EventId event);
 
-    /**
-     * @brief Set the initialization flag to true
-     */
-    void setInitStatus(bool initResult);
-
 private:
-    /**
-     * @brief Handles a generic CAN message and dispatch the message to the
-     * correct handler
-     */
     void handleCanMessage(const Boardcore::Canbus::CanMessage &msg);
 
-    // CAN message handlers
-    void handleCanEvent(const Boardcore::Canbus::CanMessage &msg);
-    void handleCanCommand(const Boardcore::Canbus::CanMessage &msg);
-
-    // Init status
-    std::atomic<bool> initStatus{false};
-
-    // CAN interfaces
-    Boardcore::Canbus::CanbusDriver *driver;
-    Boardcore::Canbus::CanProtocol *protocol;
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("CanHandler");
 
-    Boardcore::TaskScheduler *scheduler;
-    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler");
+    std::unique_ptr<Boardcore::Canbus::CanbusDriver> driver;
+    std::unique_ptr<Boardcore::Canbus::CanProtocol> protocol;
 };
 
-}  // namespace Motor
+}  // namespace Main
diff --git a/src/boards/Motor/Configs/SensorsConfig.h b/src/boards/Motor/Configs/SensorsConfig.h
index 44b13b762ad92deb64f7870ab34d8a1fdac827d0..1166dc6612e4d47d84f544c37ab091b883d7a895 100644
--- a/src/boards/Motor/Configs/SensorsConfig.h
+++ b/src/boards/Motor/Configs/SensorsConfig.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Authors: Alberto Nidasio
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -22,178 +22,78 @@
 
 #pragma once
 
-#include <Motor/Sensors/Sensors.h>
+#include <sensors/ADS131M08/ADS131M08.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS2MDL/LIS2MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+#include <sensors/LSM6DSRX/LSM6DSRX.h>
 
 namespace Motor
 {
 
-namespace SensorsConfig
+namespace Config
 {
 
-constexpr Boardcore::InternalADC::Channel ADC_BATTERY_VOLTAGE_CH =
-    Boardcore::InternalADC::Channel::CH15;
-// The battery is connected to the stm32's adc with a voltage divider
-constexpr float ADC_BATTERY_VOLTAGE_COEFF = (10 + 20) / 10;
-
-// Boardcore::LSM6DSRXConfig LSM6_SENSOR_CONFIG{
-//     .bdu       = Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE,
-//     .odrAcc    = Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_1660,
-//     .opModeAcc = Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL,
-//     .fsAcc     = Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16,
-//     .odrGyr    = Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_1660,
-//     .opModeGyr = Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL,
-//     .fsGyr     = Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_1000,
-//     .fifoMode  = Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS,
-//     .fifoTimestampDecimation =
-//         Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1,
-//     .fifoTemperatureBdr =
-//         Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED,
-//     .int1InterruptSelection = Boardcore::LSM6DSRXConfig::INTERRUPT::NOTHING,
-//     .int2InterruptSelection =
-//         Boardcore::LSM6DSRXConfig::INTERRUPT::FIFO_THRESHOLD,
-//     .fifoWatermark = 170,
-// };
-// Boardcore::SPIBusConfig LSM6_SPI_CONFIG{
-//     Boardcore::SPI::ClockDivider::DIV_16,
-//     Boardcore::SPI::Mode::MODE_0,
-// };
-
-Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS_ODR =
-    Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_1000;
-Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS_BDU =
-    Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE;
-Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS_FSR =
+namespace Sensors
+{
+
+namespace LPS22DF
+{
+static constexpr Boardcore::LPS22DF::AVG AVG = Boardcore::LPS22DF::AVG_4;
+static constexpr Boardcore::LPS22DF::ODR ODR = Boardcore::LPS22DF::ODR_100;
+static constexpr uint32_t PERIOD             = 20;  // [ms] 50Hz
+}  // namespace LPS22DF
+
+namespace H3LIS331DL
+{
+static constexpr Boardcore::H3LIS331DLDefs::OutputDataRate ODR =
+    Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_400;
+static constexpr Boardcore::H3LIS331DLDefs::FullScaleRange FS =
     Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100;
-Boardcore::SPIBusConfig H3LIS_SPI_CONFIG{
-    Boardcore::SPI::ClockDivider::DIV_16,
-};
-
-Boardcore::LIS2MDL::Config LIS2_SENSOR_CONFIG{
-    .odr                = Boardcore::LIS2MDL::ODR_100_HZ,
-    .deviceMode         = Boardcore::LIS2MDL::MD_CONTINUOUS,
-    .temperatureDivider = 100,
-};
-Boardcore::SPIBusConfig LIS2_SPI_CONFIG{
-    Boardcore::SPI::ClockDivider::DIV_16,
-    Boardcore::SPI::Mode::MODE_3,
-    Boardcore::SPI::Order::MSB_FIRST,
-    Boardcore::SPI::Order::LSB_FIRST,
-};
-
-Boardcore::LPS22DF::Config LPS22_SENSOR_CONFIG{
-    .odr = Boardcore::LPS22DF::ODR_100,
-    .avg = Boardcore::LPS22DF::AVG_512,
-};
-Boardcore::SPIBusConfig LPS22_SPI_CONFIG{
-    Boardcore::SPI::ClockDivider::DIV_16,
-    Boardcore::SPI::Mode::MODE_3,
-    Boardcore::SPI::Order::MSB_FIRST,
-    Boardcore::SPI::Order::LSB_FIRST,
-};
-
-Boardcore::ADS131M08::Config ADS131_SENSOR_CONFIG{
-    .channelsConfig =
-        {
-            {.enabled = false},
-            // For the servo motor's current sensor the current range is from 0A
-            // to 20A and the output voltage from 0.65V to 2.65V. The sensor is
-            // connected to the adc through a voltage divider with a gain
-            // of 4.166666 So at the ADC side we have maximum 0.636 and we have
-            // to set a gain of 1 on the ADC
-            {
-                .enabled = true,
-                .pga     = Boardcore::ADS131M08Defs::PGA::PGA_1,
-            },
-            {.enabled = false},
-            {.enabled = false},
-            {.enabled = false},
-            // The analog pressure sensors are measured with a 30ohm shunt
-            // resistor. The current ranges from 4mAh to 20mAh. So we can have a
-            // voltage up to 0.6V We set a gain of 2 to maximize resolution
-            {
-                .enabled = true,
-                .pga     = Boardcore::ADS131M08Defs::PGA::PGA_2,
-            },
-            {
-                .enabled = true,
-                .pga     = Boardcore::ADS131M08Defs::PGA::PGA_2,
-            },
-            {
-                .enabled = true,
-                .pga     = Boardcore::ADS131M08Defs::PGA::PGA_2,
-            },
-        },
-    .oversamplingRatio = Boardcore::ADS131M08Defs::OversamplingRatio::OSR_4096,
-    .globalChopModeEnabled = true,
-};
-Boardcore::SPIBusConfig ADS131_SPI_CONFIG{
-    Boardcore::SPI::ClockDivider::DIV_8,
-    Boardcore::SPI::Mode::MODE_1,
-};
-Boardcore::ADS131M08Defs::Channel ADS131_CHAMBER_PRESSURE_CH =
-    Boardcore::ADS131M08Defs::Channel::CHANNEL_5;
-Boardcore::ADS131M08Defs::Channel ADS131_TANK_PRESSURE_1_CH =
-    Boardcore::ADS131M08Defs::Channel::CHANNEL_6;
-Boardcore::ADS131M08Defs::Channel ADS131_TANK_PRESSURE_2_CH =
-    Boardcore::ADS131M08Defs::Channel::CHANNEL_7;
-Boardcore::ADS131M08Defs::Channel ADS131_SERVO_CURRENT_CH =
-    Boardcore::ADS131M08Defs::Channel::CHANNEL_1;
-
-// The chamber pressure sensor is a 0-40bar sensor with a 4-20mA output
-// On the motor motherboard there a 30ohm shunt. The shunt then goes through a
-// filter with 491Hz of bandwidth and unitary gain.
-// We use only a gain coefficient because the offset is removed by the ADC
-// offset calibration feature.
-constexpr float CHAMBER_PRESSURE_MAX      = 40;
-constexpr float CHAMBER_PRESSURE_MIN      = 0;
-constexpr float CHAMBER_PRESSURE_SHUNT    = 30;     // [ohm]
-constexpr float CHAMBER_PRESSURE_CURR_MIN = 0.004;  // [A]
-constexpr float CHAMBER_PRESSURE_CURR_MAX = 0.020;  // [A]
-constexpr float CHAMBER_PRESSURE_COEFF =
-    CHAMBER_PRESSURE_MAX /
-    (CHAMBER_PRESSURE_CURR_MAX - CHAMBER_PRESSURE_CURR_MIN);  // [bar/A]
-
-// The tank pressure sensors are 0-100bar sensors with a 4-20mA output
-// On the motor motherboard there a 30ohm shunt. The shunt then goes through a
-// filter with 491Hz of bandwidth and unitary gain.
-// We use only a gain coefficient because the offset is removed by the ADC
-// offset calibration feature.
-constexpr float TANK_PRESSURE_1_MAX      = 100;
-constexpr float TANK_PRESSURE_1_MIN      = 0;
-constexpr float TANK_PRESSURE_1_SHUNT    = 30;     // [ohm]
-constexpr float TANK_PRESSURE_1_CURR_MIN = 0.004;  // [A]
-constexpr float TANK_PRESSURE_1_CURR_MAX = 0.020;  // [A]
-constexpr float TANK_PRESSURE_1_COEFF =
-    TANK_PRESSURE_1_MAX /
-    (TANK_PRESSURE_1_CURR_MAX - TANK_PRESSURE_1_CURR_MIN);  // [bar/A]
-constexpr float TANK_PRESSURE_2_MAX      = 100;
-constexpr float TANK_PRESSURE_2_MIN      = 0;
-constexpr float TANK_PRESSURE_2_SHUNT    = 30;     // [ohm]
-constexpr float TANK_PRESSURE_2_CURR_MIN = 0.004;  // [A]
-constexpr float TANK_PRESSURE_2_CURR_MAX = 0.020;  // [A]
-constexpr float TANK_PRESSURE_2_COEFF =
-    TANK_PRESSURE_2_MAX /
-    (TANK_PRESSURE_2_CURR_MAX - TANK_PRESSURE_2_CURR_MIN);  // [bar/A]
-
-// The current sensor used to measure the servo motors current consumption:
-// - A range of 0A to 20A
-// - Has an output from 0.65V to 2.65V
-// - Is connected to a voltage divider with a coefficient of 12 / (38.3 + 12)
-//
-// We use only a gain coefficient because the offset is removed by the ADC
-// offset calibration feature.
-constexpr float SERVO_CURRENT_COEFF =
-    10 / (2.65 - 0.65) / (12 / (38.3 + 12));  // [A/V]
-
-// Sampling periods
-constexpr uint32_t SAMPLE_PERIOD_ADC    = 1000;
-constexpr uint32_t SAMPLE_PERIOD_LSM6   = 10;
-constexpr uint32_t SAMPLE_PERIOD_H3LIS  = 100;
-constexpr uint32_t SAMPLE_PERIOD_LIS2   = 10;
-constexpr uint32_t SAMPLE_PERIOD_LPS22  = 10;
-constexpr uint32_t SAMPLE_PERIOD_ADS131 = 1;
-constexpr uint32_t SAMPLE_PERIOD_MAX    = 10;
-
-}  // namespace SensorsConfig
-
-}  // namespace Motor
\ No newline at end of file
+static constexpr uint32_t PERIOD = 10;  // [ms] 100Hz
+}  // namespace H3LIS331DL
+
+namespace LIS2MDL
+{
+static constexpr Boardcore::LIS2MDL::ODR ODR = Boardcore::LIS2MDL::ODR_100_HZ;
+static constexpr unsigned int TEMP_DIVIDER   = 5;
+static constexpr uint32_t PERIOD             = 10;  // [ms] 100Hz
+}  // namespace LIS2MDL
+
+namespace LSM6DSRX
+{
+static constexpr Boardcore::LSM6DSRXConfig::ACC_FULLSCALE ACC_FS =
+    Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16;
+static constexpr Boardcore::LSM6DSRXConfig::ACC_ODR ACC_ODR =
+    Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_416;
+static constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE ACC_OP_MODE =
+    Boardcore::LSM6DSRXConfig::OPERATING_MODE::HIGH_PERFORMANCE;
+
+static constexpr Boardcore::LSM6DSRXConfig::GYR_FULLSCALE GYR_FS =
+    Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_4000;
+static constexpr Boardcore::LSM6DSRXConfig::GYR_ODR GYR_ODR =
+    Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_416;
+static constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE GYR_OP_MODE =
+    Boardcore::LSM6DSRXConfig::OPERATING_MODE::HIGH_PERFORMANCE;
+
+static constexpr uint32_t PERIOD = 20;  // [ms] 50Hz
+}  // namespace LSM6DSRX
+
+namespace ADS131M08
+{
+static constexpr Boardcore::ADS131M08Defs::OversamplingRatio OSR =
+    Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192;
+static constexpr bool GLOBAL_CHOP_MODE_EN = true;
+static constexpr uint32_t PERIOD          = 10;  // [ms] 100Hz
+}  // namespace ADS131M08
+
+namespace InternalADC
+{
+static constexpr uint32_t PERIOD = 100;  // [ms] 10Hz
+}
+
+}  // namespace Sensors
+
+}  // namespace Config
+
+}  // namespace Motor
diff --git a/src/boards/Motor/Sensors/Sensors.cpp b/src/boards/Motor/Sensors/Sensors.cpp
index 0643a6865d1b0d96f9d7ced1d301689efc21f478..e68a6f563ef578cbd300057f06e5ed00d5738e7a 100644
--- a/src/boards/Motor/Sensors/Sensors.cpp
+++ b/src/boards/Motor/Sensors/Sensors.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Luca Erbetta, Luca Conterio, Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -24,404 +24,183 @@
 
 #include <Motor/Buses.h>
 #include <Motor/Configs/SensorsConfig.h>
-#include <drivers/interrupt/external_interrupts.h>
 #include <interfaces-impl/hwmapping.h>
-#include <logger/Logger.h>
 
-using namespace std;
-using namespace miosix;
-using namespace Boardcore;
-using namespace Motor::SensorsConfig;
-
-namespace Motor
-{
-
-InternalADCData Sensors::getADCData()
-{
-    miosix::PauseKernelLock lock;
-    return adc != nullptr ? adc->getLastSample() : InternalADCData{};
-}
-
-BatteryVoltageSensorData Sensors::getBatteryData()
-{
-    miosix::PauseKernelLock lock;
-    return battery != nullptr ? battery->getLastSample()
-                              : BatteryVoltageSensorData{};
-}
-
-// LSM6DSRXData Sensors::getLSM6DSRXData()
-// {
-//     miosix::PauseKernelLock lock;
-//     return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{};
-// }
-
-H3LIS331DLData Sensors::getH3LIS331DLData()
-{
-    miosix::PauseKernelLock lock;
-    return h3lis331dl != nullptr ? h3lis331dl->getLastSample()
-                                 : H3LIS331DLData{};
-}
+#include <utils/ModuleManager/ModuleManager.hpp>
 
-LIS2MDLData Sensors::getLIS2MDLData()
-{
-    miosix::PauseKernelLock lock;
-    return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{};
-}
-
-LPS22DFData Sensors::getLPS22DFData()
-{
-    miosix::PauseKernelLock lock;
-    return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{};
-}
-
-ADS131M08Data Sensors::getADS131M08Data()
-{
-    miosix::PauseKernelLock lock;
-    return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{};
-}
-
-MAX31856Data Sensors::getMAX31856Data()
-{
-    miosix::PauseKernelLock lock;
-    return max31856 != nullptr ? max31856->getLastSample() : MAX31856Data{};
-}
-
-ChamberPressureSensorData Sensors::getChamberPressureSensorData()
-{
-    miosix::PauseKernelLock lock;
-    return chamberPressure != nullptr ? chamberPressure->getLastSample()
-                                      : ChamberPressureSensorData{};
-}
-
-TankPressureSensor1Data Sensors::getTankPressureSensor1Data()
-{
-    miosix::PauseKernelLock lock;
-    return tankPressure1 != nullptr ? tankPressure1->getLastSample()
-                                    : TankPressureSensor1Data{};
-}
-
-TankPressureSensor2Data Sensors::getTankPressureSensor2Data()
-{
-    miosix::PauseKernelLock lock;
-    return tankPressure2 != nullptr ? tankPressure2->getLastSample()
-                                    : TankPressureSensor2Data{};
-}
-
-CurrentData Sensors::getServoCurrentData()
-{
-    miosix::PauseKernelLock lock;
-    return servosCurrent != nullptr ? servosCurrent->getLastSample()
-                                    : CurrentData{};
-}
-
-Sensors::Sensors(TaskScheduler* sched) : scheduler(sched) {}
+using namespace Motor;
+using namespace Boardcore;
+using namespace miosix;
 
-Sensors::~Sensors() {}
+bool Sensors::isStarted() { return started; }
 
 bool Sensors::start()
 {
-    adcInit();
-    batteryInit();
-    // lsm6dsrxInit();
-    h3lis331dlInit();
-    // lis2mdlInit();
-    lps22dfInit();
-    max31856Init();
-    ads131m08Init();
-    chamberPressureInit();
-    tankPressure1Init();
-    tankPressure2Init();
-    servosCurrentInit();
-
-    sensorManager = new SensorManager(sensorsMap, scheduler);
-
-    return sensorManager->start();
-}
+    SensorManager::SensorMap_t map;
+    lps22dfInit(map);
+    h3lis331dlInit(map);
+    lis2mdlInit(map);
+    lsm6dsrxInit(map);
+    ads131m08Init(map);
+    internalAdcInit(map);
 
-void Sensors::calibrate()
-{
-    if (ads131m08 != nullptr)
+    manager = std::make_unique<SensorManager>(map, &scheduler);
+    if (!manager->start())
     {
-        ads131m08->calibrateOffset(ADS131_SERVO_CURRENT_CH);
+        return false;
     }
-}
-
-void Sensors::adcInit()
-{
-    adc = new InternalADC(ADC1);
-
-    adc->enableTemperature();
-    adc->enableVbat();
-    adc->enableChannel(ADC_BATTERY_VOLTAGE_CH);
 
-    SensorInfo info("ADC", SAMPLE_PERIOD_ADC,
-                    bind(&Sensors::adcCallback, this));
-
-    sensorsMap.emplace(make_pair(adc, info));
+    started = true;
+    return true;
 }
 
-void Sensors::batteryInit()
+std::vector<SensorInfo> Sensors::getSensorInfo()
 {
-    function<ADCData()> getADCVoltage(
-        bind(&InternalADC::getVoltage, adc, ADC_BATTERY_VOLTAGE_CH));
-
-    battery =
-        new BatteryVoltageSensor(getADCVoltage, ADC_BATTERY_VOLTAGE_COEFF);
-
-    SensorInfo info("BATTERY", SAMPLE_PERIOD_ADC,
-                    bind(&Sensors::batteryCallback, this));
-
-    sensorsMap.emplace(make_pair(battery, info));
+    return {manager->getSensorInfo(lps22df.get()),
+            manager->getSensorInfo(h3lis331dl.get()),
+            manager->getSensorInfo(lis2mdl.get()),
+            manager->getSensorInfo(lsm6dsrx.get()),
+            manager->getSensorInfo(ads131m08.get()),
+            manager->getSensorInfo(internalAdc.get())};
 }
 
-// void Sensors::lsm6dsrxInit()
-// {
-//     SPIBus& spi1 = ModuleManager::getInstance().get<Buses>()->spi1;
-
-//     lsm6dsrx = new LSM6DSRX(spi1, peripherals::lsm6dsrx::cs::getPin(),
-//                             LSM6_SPI_CONFIG, LSM6_SENSOR_CONFIG);
-
-//     SensorInfo info("LSM6DSRX", SAMPLE_PERIOD_LSM6,
-//                     bind(&Sensors::lsm6dsrxCallback, this));
-
-//     sensorsMap.emplace(make_pair(lsm6dsrx, info));
-// }
-
-void Sensors::h3lis331dlInit()
+void Sensors::lps22dfInit(SensorManager::SensorMap_t &map)
 {
-    SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3;
+    ModuleManager &modules = ModuleManager::getInstance();
 
-    h3lis331dl = new H3LIS331DL(spi3, peripherals::h3lis331dl::cs::getPin(),
-                                H3LIS_ODR, H3LIS_BDU, H3LIS_FSR);
+    SPIBusConfig spiConfig = LPS22DF::getDefaultSPIConfig();
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_16;
 
-    SensorInfo info("H3LIS331DL", SAMPLE_PERIOD_H3LIS,
-                    bind(&Sensors::h3lis331dlCallback, this));
+    LPS22DF::Config config;
+    config.avg = Config::Sensors::LPS22DF::AVG;
+    config.odr = Config::Sensors::LPS22DF::ODR;
 
-    sensorsMap.emplace(make_pair(h3lis331dl, info));
-}
+    lps22df = std::make_unique<LPS22DF>(modules.get<Buses>()->getLPS22DF(),
+                                        sensors::LPS22DF::cs::getPin(),
+                                        spiConfig, config);
 
-void Sensors::lis2mdlInit()
-{
-    SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3;
-
-    lis2mdl = new LIS2MDL(spi3, peripherals::lis2mdl::cs::getPin(),
-                          LIS2_SPI_CONFIG, LIS2_SENSOR_CONFIG);
-
-    SensorInfo info("LIS2MDL", SAMPLE_PERIOD_LIS2,
-                    bind(&Sensors::lis2mdlCallback, this));
-
-    sensorsMap.emplace(make_pair(lis2mdl, info));
+    SensorInfo info{"LPS22DF", Config::Sensors::LPS22DF::PERIOD,
+                    [this]() { lps22dfCallback(); }};
+    map.emplace(lps22df.get(), info);
 }
 
-void Sensors::lps22dfInit()
-{
-    SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3;
-
-    lps22df = new LPS22DF(spi3, peripherals::lps22df::cs::getPin(),
-                          LPS22_SPI_CONFIG, LPS22_SENSOR_CONFIG);
-
-    SensorInfo info("LPS22", SAMPLE_PERIOD_LPS22,
-                    bind(&Sensors::lps22dfCallback, this));
+void Sensors::lps22dfCallback() {}
 
-    sensorsMap.emplace(make_pair(lps22df, info));
-}
-
-void Sensors::max31856Init()
+void Sensors::h3lis331dlInit(SensorManager::SensorMap_t &map)
 {
-    SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3;
+    ModuleManager &modules = ModuleManager::getInstance();
 
-    max31856 = new MAX31856(spi3, peripherals::max31856::cs::getPin());
+    SPIBusConfig spiConfig = H3LIS331DL::getDefaultSPIConfig();
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_16;
 
-    SensorInfo info("MAX31856", SAMPLE_PERIOD_MAX,
-                    bind(&Sensors::max31856Callback, this));
+    h3lis331dl = std::make_unique<H3LIS331DL>(
+        modules.get<Buses>()->getH3LIS331DL(),
+        sensors::H3LIS331DL::cs::getPin(), spiConfig,
+        Config::Sensors::H3LIS331DL::ODR,
+        H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE,
+        Config::Sensors::H3LIS331DL::FS);
 
-    sensorsMap.emplace(make_pair(max31856, info));
+    SensorInfo info{"H3LIS331DL", Config::Sensors::H3LIS331DL::PERIOD,
+                    [this]() { h3lis331dlCallback(); }};
+    map.emplace(h3lis331dl.get(), info);
 }
 
-void Sensors::ads131m08Init()
-{
-    SPIBus& spi4 = ModuleManager::getInstance().get<Buses>()->spi4;
-
-    ads131m08 = new ADS131M08(spi4, peripherals::ads131m08::cs::getPin(),
-                              ADS131_SPI_CONFIG, ADS131_SENSOR_CONFIG);
-
-    SensorInfo info("ADS131M08", SAMPLE_PERIOD_ADS131,
-                    bind(&Sensors::ads131m08Callback, this));
-
-    sensorsMap.emplace(make_pair(ads131m08, info));
-}
+void Sensors::h3lis331dlCallback() {}
 
-void Sensors::chamberPressureInit()
+void Sensors::lis2mdlInit(SensorManager::SensorMap_t &map)
 {
-    if (ads131m08 == nullptr)
-    {
-        return;
-    }
-
-    function<ADCData()> getADCVoltage(
-        [&](void)
-        {
-            auto data = ads131m08->getLastSample();
-            return data.getVoltage(ADS131_CHAMBER_PRESSURE_CH);
-        });
-
-    function<float(float)> voltageToPressure(
-        [](float voltage)
-        {
-            float current =
-                voltage / CHAMBER_PRESSURE_SHUNT - CHAMBER_PRESSURE_CURR_MIN;
-            return current * CHAMBER_PRESSURE_COEFF;
-        });
-
-    chamberPressure =
-        new ChamberPressureSensor(getADCVoltage, voltageToPressure);
-
-    SensorInfo info("CHAMBER_PRESSURE", SAMPLE_PERIOD_ADS131,
-                    bind(&Sensors::chamberPressureCallback, this));
-
-    sensorsMap.emplace(make_pair(chamberPressure, info));
-}
-
-void Sensors::tankPressure1Init()
-{
-    if (ads131m08 == nullptr)
-    {
-        return;
-    }
-
-    function<ADCData()> getADCVoltage(
-        [&](void)
-        {
-            auto data = ads131m08->getLastSample();
-            return data.getVoltage(ADS131_TANK_PRESSURE_1_CH);
-        });
+    ModuleManager &modules = ModuleManager::getInstance();
 
-    function<float(float)> voltageToPressure(
-        [](float voltage)
-        {
-            float current =
-                voltage / TANK_PRESSURE_1_SHUNT - TANK_PRESSURE_1_CURR_MIN;
-            return current * TANK_PRESSURE_1_COEFF;
-        });
+    SPIBusConfig spiConfig = H3LIS331DL::getDefaultSPIConfig();
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_16;
 
-    tankPressure1 = new TankPressureSensor1(getADCVoltage, voltageToPressure);
+    LIS2MDL::Config config;
+    config.deviceMode         = LIS2MDL::MD_CONTINUOUS;
+    config.odr                = Config::Sensors::LIS2MDL::ODR;
+    config.temperatureDivider = Config::Sensors::LIS2MDL::TEMP_DIVIDER;
 
-    SensorInfo info("TANK_PRESSURE_1", SAMPLE_PERIOD_ADS131,
-                    bind(&Sensors::tankPressure1Callback, this));
+    lis2mdl = std::make_unique<LIS2MDL>(modules.get<Buses>()->getLIS2MDL(),
+                                        sensors::LIS2MDL::cs::getPin(),
+                                        spiConfig, config);
 
-    sensorsMap.emplace(make_pair(tankPressure1, info));
+    SensorInfo info{"LIS2MDL", Config::Sensors::LIS2MDL::PERIOD,
+                    [this]() { lis2mdlCallback(); }};
+    map.emplace(lis2mdl.get(), info);
 }
 
-void Sensors::tankPressure2Init()
-{
-    if (ads131m08 == nullptr)
-    {
-        return;
-    }
-
-    function<ADCData()> getADCVoltage(
-        [&](void)
-        {
-            auto data = ads131m08->getLastSample();
-            return data.getVoltage(ADS131_TANK_PRESSURE_2_CH);
-        });
-
-    function<float(float)> voltageToPressure(
-        [](float voltage)
-        {
-            float current =
-                voltage / TANK_PRESSURE_2_SHUNT - TANK_PRESSURE_2_CURR_MIN;
-            return current * TANK_PRESSURE_2_COEFF;
-        });
-
-    tankPressure2 = new TankPressureSensor2(getADCVoltage, voltageToPressure);
-
-    SensorInfo info("TANK_PRESSURE_2", SAMPLE_PERIOD_ADS131,
-                    bind(&Sensors::tankPressure2Callback, this));
+void Sensors::lis2mdlCallback() {}
 
-    sensorsMap.emplace(make_pair(tankPressure2, info));
-}
-
-void Sensors::servosCurrentInit()
+void Sensors::lsm6dsrxInit(SensorManager::SensorMap_t &map)
 {
-    if (ads131m08 == nullptr)
-    {
-        return;
-    }
+    ModuleManager &modules = ModuleManager::getInstance();
 
-    function<ADCData()> getADCVoltage(
-        [&](void)
-        {
-            auto data = ads131m08->getLastSample();
-            return data.getVoltage(ADS131_SERVO_CURRENT_CH);
-        });
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
+    spiConfig.mode         = SPI::Mode::MODE_0;
 
-    function<float(float)> voltageToCurrent(
-        [](float voltage) { return voltage * SERVO_CURRENT_COEFF; });
+    LSM6DSRXConfig config;
+    config.bdu = LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE;
 
-    servosCurrent = new CurrentSensor(getADCVoltage, voltageToCurrent);
+    config.fsAcc     = Config::Sensors::LSM6DSRX::ACC_FS;
+    config.odrAcc    = Config::Sensors::LSM6DSRX::ACC_ODR;
+    config.opModeAcc = Config::Sensors::LSM6DSRX::ACC_OP_MODE;
 
-    SensorInfo info("SERVOS_CURRENT", SAMPLE_PERIOD_ADS131,
-                    bind(&Sensors::servosCurrentCallback, this));
+    config.fsGyr     = Config::Sensors::LSM6DSRX::GYR_FS;
+    config.odrGyr    = Config::Sensors::LSM6DSRX::GYR_ODR;
+    config.opModeGyr = Config::Sensors::LSM6DSRX::GYR_OP_MODE;
 
-    sensorsMap.emplace(make_pair(servosCurrent, info));
-}
+    config.fifoMode = LSM6DSRXConfig::FIFO_MODE::CONTINUOUS;
+    config.fifoTimestampDecimation =
+        LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1;
+    config.fifoTemperatureBdr = LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED;
 
-void Sensors::adcCallback() { Logger::getInstance().log(adc->getLastSample()); }
+    lsm6dsrx = std::make_unique<LSM6DSRX>(modules.get<Buses>()->getLSM6DSRX(),
+                                          sensors::LSM6DSRX::cs::getPin(),
+                                          spiConfig, config);
 
-void Sensors::batteryCallback()
-{
-    Logger::getInstance().log(battery->getLastSample());
+    SensorInfo info{"LSM6DSRX", Config::Sensors::LSM6DSRX::PERIOD,
+                    [this]() { lsm6dsrxCallback(); }};
+    map.emplace(lsm6dsrx.get(), info);
 }
 
-// void Sensors::lsm6dsrxCallback()
-// {
-//     Logger::getInstance().log(lsm6dsrx->getLastSample());
-// }
+void Sensors::lsm6dsrxCallback() {}
 
-void Sensors::h3lis331dlCallback()
+void Sensors::ads131m08Init(SensorManager::SensorMap_t &map)
 {
-    Logger::getInstance().log(h3lis331dl->getLastSample());
-}
+    ModuleManager &modules = ModuleManager::getInstance();
 
-void Sensors::lis2mdlCallback()
-{
-    Logger::getInstance().log(lis2mdl->getLastSample());
-}
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
 
-void Sensors::lps22dfCallback()
-{
-    Logger::getInstance().log(lps22df->getLastSample());
-}
+    ADS131M08::Config config;
+    config.oversamplingRatio = Config::Sensors::ADS131M08::OSR;
+    config.globalChopModeEnabled =
+        Config::Sensors::ADS131M08::GLOBAL_CHOP_MODE_EN;
 
-void Sensors::max31856Callback()
-{
-    Logger::getInstance().log(max31856->getLastSample());
-}
+    ads131m08 = std::make_unique<ADS131M08>(
+        modules.get<Buses>()->getADS131M08(), sensors::ADS131M08::cs::getPin(),
+        spiConfig, config);
 
-void Sensors::ads131m08Callback()
-{
-    Logger::getInstance().log(ads131m08->getLastSample());
+    SensorInfo info{"ADS131M08", Config::Sensors::ADS131M08::PERIOD,
+                    [this]() { ads131m08Callback(); }};
+    map.emplace(ads131m08.get(), info);
 }
 
-void Sensors::chamberPressureCallback()
-{
-    Logger::getInstance().log(chamberPressure->getLastSample());
-}
+void Sensors::ads131m08Callback() {}
 
-void Sensors::tankPressure1Callback()
+void Sensors::internalAdcInit(SensorManager::SensorMap_t &map)
 {
-    Logger::getInstance().log(tankPressure1->getLastSample());
-}
+    ModuleManager &modules = ModuleManager::getInstance();
 
-void Sensors::tankPressure2Callback()
-{
-    Logger::getInstance().log(tankPressure2->getLastSample());
-}
+    internalAdc = std::make_unique<InternalADC>(ADC2);
+    internalAdc->enableChannel(InternalADC::CH9);
+    internalAdc->enableChannel(InternalADC::CH14);
+    internalAdc->enableTemperature();
+    internalAdc->enableVbat();
 
-void Sensors::servosCurrentCallback()
-{
-    Logger::getInstance().log(servosCurrent->getLastSample());
+    SensorInfo info{"InternalADC", Config::Sensors::InternalADC::PERIOD,
+                    [this]() { internalAdcCallback(); }};
+    map.emplace(internalAdc.get(), info);
 }
 
-}  // namespace Motor
\ No newline at end of file
+void Sensors::internalAdcCallback() {}
\ No newline at end of file
diff --git a/src/boards/Motor/Sensors/Sensors.h b/src/boards/Motor/Sensors/Sensors.h
index 780563efec9feef081b3258fed1fc4f66477854a..79013f223f2626d6ed6e8f51dcb54ffdf8bcd131 100644
--- a/src/boards/Motor/Sensors/Sensors.h
+++ b/src/boards/Motor/Sensors/Sensors.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Alberto Nidasio
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -22,22 +22,18 @@
 
 #pragma once
 
-#include <Motor/Sensors/ChamberPressureSensor/ChamberPressureSensor.h>
-#include <Motor/Sensors/TankPressureSensor1/TankPressureSensor1.h>
-#include <Motor/Sensors/TankPressureSensor2/TankPressureSensor2.h>
 #include <drivers/adc/InternalADC.h>
+#include <scheduler/TaskScheduler.h>
 #include <sensors/ADS131M08/ADS131M08.h>
 #include <sensors/H3LIS331DL/H3LIS331DL.h>
 #include <sensors/LIS2MDL/LIS2MDL.h>
 #include <sensors/LPS22DF/LPS22DF.h>
-// #include <sensors/LSM6DSRX/LSM6DSRX.h>
-#include <sensors/MAX31856/MAX31856.h>
+#include <sensors/LSM6DSRX/LSM6DSRX.h>
 #include <sensors/SensorManager.h>
-#include <sensors/analog/BatteryVoltageSensor.h>
-#include <sensors/analog/CurrentSensor.h>
-#include <sensors/analog/pressure/AnalogPressureSensor.h>
 
+#include <memory>
 #include <utils/ModuleManager/ModuleManager.hpp>
+#include <vector>
 
 namespace Motor
 {
@@ -45,80 +41,50 @@ namespace Motor
 class Sensors : public Boardcore::Module
 {
 public:
-    Boardcore::InternalADCData getADCData();
-    Boardcore::BatteryVoltageSensorData getBatteryData();
-    // Boardcore::LSM6DSRXData getLSM6DSRXData();
-    Boardcore::H3LIS331DLData getH3LIS331DLData();
-    Boardcore::LIS2MDLData getLIS2MDLData();
-    Boardcore::LPS22DFData getLPS22DFData();
-    Boardcore::ADS131M08Data getADS131M08Data();
-    Boardcore::MAX31856Data getMAX31856Data();
-    Boardcore::ChamberPressureSensorData getChamberPressureSensorData();
-    Boardcore::TankPressureSensor1Data getTankPressureSensor1Data();
-    Boardcore::TankPressureSensor2Data getTankPressureSensor2Data();
-    Boardcore::CurrentData getServoCurrentData();
+    explicit Sensors(Boardcore::TaskScheduler &scheduler) : scheduler{scheduler}
+    {
+    }
 
-    explicit Sensors(Boardcore::TaskScheduler* sched);
 
-    ~Sensors();
+    bool isStarted();
 
-    bool start();
+    [[nodiscard]] bool start();
 
-    void calibrate();
+    std::vector<Boardcore::SensorInfo> getSensorInfo();
 
 private:
-    void adcInit();
-    void adcCallback();
-
-    void batteryInit();
-    void batteryCallback();
-
-    // void lsm6dsrxInit();
-    // void lsm6dsrxCallback();
+    void lps22dfInit(Boardcore::SensorManager::SensorMap_t &map);
+    void lps22dfCallback();
 
-    void h3lis331dlInit();
+    void h3lis331dlInit(Boardcore::SensorManager::SensorMap_t &map);
     void h3lis331dlCallback();
 
-    void lis2mdlInit();
+    void lis2mdlInit(Boardcore::SensorManager::SensorMap_t &map);
     void lis2mdlCallback();
 
-    void lps22dfInit();
-    void lps22dfCallback();
-
-    void max31856Init();
-    void max31856Callback();
+    void lsm6dsrxInit(Boardcore::SensorManager::SensorMap_t &map);
+    void lsm6dsrxCallback();
 
-    void ads131m08Init();
+    void ads131m08Init(Boardcore::SensorManager::SensorMap_t &map);
     void ads131m08Callback();
 
-    void chamberPressureInit();
-    void chamberPressureCallback();
-
-    void tankPressure1Init();
-    void tankPressure1Callback();
-
-    void tankPressure2Init();
-    void tankPressure2Callback();
-
-    void servosCurrentInit();
-    void servosCurrentCallback();
-
-    Boardcore::InternalADC* adc              = nullptr;
-    Boardcore::BatteryVoltageSensor* battery = nullptr;
-    // Boardcore::LSM6DSRX* lsm6dsrx                     = nullptr;
-    Boardcore::H3LIS331DL* h3lis331dl                 = nullptr;
-    Boardcore::LIS2MDL* lis2mdl                       = nullptr;
-    Boardcore::LPS22DF* lps22df                       = nullptr;
-    Boardcore::MAX31856* max31856                     = nullptr;
-    Boardcore::ADS131M08* ads131m08                   = nullptr;
-    Boardcore::ChamberPressureSensor* chamberPressure = nullptr;
-    Boardcore::TankPressureSensor1* tankPressure1     = nullptr;
-    Boardcore::TankPressureSensor2* tankPressure2     = nullptr;
-    Boardcore::CurrentSensor* servosCurrent           = nullptr;
-
-    Boardcore::SensorManager::SensorMap_t sensorsMap;
-    Boardcore::SensorManager* sensorManager = nullptr;
-    Boardcore::TaskScheduler* scheduler     = nullptr;
+    void internalAdcInit(Boardcore::SensorManager::SensorMap_t &map);
+    void internalAdcCallback();
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors");
+
+    std::atomic<bool> started{false};
+
+    std::unique_ptr<Boardcore::LPS22DF> lps22df;
+    std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl;
+    std::unique_ptr<Boardcore::LIS2MDL> lis2mdl;
+    std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx;
+    std::unique_ptr<Boardcore::ADS131M08> ads131m08;
+    std::unique_ptr<Boardcore::InternalADC> internalAdc;
+
+    std::unique_ptr<Boardcore::SensorManager> manager;
+
+    Boardcore::TaskScheduler &scheduler;
 };
 
 }  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/Actuators/Actuators.cpp b/src/boards/Motor_old/Actuators/Actuators.cpp
similarity index 100%
rename from src/boards/Motor/Actuators/Actuators.cpp
rename to src/boards/Motor_old/Actuators/Actuators.cpp
diff --git a/src/boards/Motor/Actuators/Actuators.h b/src/boards/Motor_old/Actuators/Actuators.h
similarity index 100%
rename from src/boards/Motor/Actuators/Actuators.h
rename to src/boards/Motor_old/Actuators/Actuators.h
diff --git a/src/boards/Motor/Actuators/ActuatorsData.h b/src/boards/Motor_old/Actuators/ActuatorsData.h
similarity index 100%
rename from src/boards/Motor/Actuators/ActuatorsData.h
rename to src/boards/Motor_old/Actuators/ActuatorsData.h
diff --git a/src/boards/Motor/BoardScheduler.cpp b/src/boards/Motor_old/BoardScheduler.cpp
similarity index 100%
rename from src/boards/Motor/BoardScheduler.cpp
rename to src/boards/Motor_old/BoardScheduler.cpp
diff --git a/src/boards/Motor/BoardScheduler.h b/src/boards/Motor_old/BoardScheduler.h
similarity index 100%
rename from src/boards/Motor/BoardScheduler.h
rename to src/boards/Motor_old/BoardScheduler.h
diff --git a/src/boards/Motor_old/Buses.h b/src/boards/Motor_old/Buses.h
new file mode 100644
index 0000000000000000000000000000000000000000..899fde6bbc0cf07e07fff4fc0588414ab5aef839
--- /dev/null
+++ b/src/boards/Motor_old/Buses.h
@@ -0,0 +1,46 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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/spi/SPIBus.h>
+#include <drivers/usart/USART.h>
+#include <miosix.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Motor
+{
+
+class Buses : public Boardcore::Module
+{
+public:
+    Boardcore::SPIBus spi1;
+    Boardcore::SPIBus spi3;
+    Boardcore::SPIBus spi4;
+
+    Boardcore::USART usart2;
+
+    Buses() : spi1(SPI1), spi3(SPI3), spi4(SPI4), usart2(USART2, 115200) {}
+};
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor_old/CanHandler/CanHandler.cpp b/src/boards/Motor_old/CanHandler/CanHandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0ef05206248bcfc0996d8ac70e146d2d01a5a05a
--- /dev/null
+++ b/src/boards/Motor_old/CanHandler/CanHandler.cpp
@@ -0,0 +1,264 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Alberto Nidasio, Matteo Pignataro
+ *
+ * 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 "CanHandler.h"
+
+#include <Motor/Actuators/Actuators.h>
+#include <Motor/Configs/CanHandlerConfig.h>
+#include <Motor/Sensors/Sensors.h>
+#include <common/CanConfig.h>
+#include <common/Events.h>
+
+#include <functional>
+
+using namespace std;
+using namespace placeholders;
+using namespace Boardcore;
+using namespace Canbus;
+using namespace Common;
+using namespace CanConfig;
+using namespace Motor::CanHandlerConfig;
+
+namespace Motor
+{
+
+CanHandler::CanHandler(TaskScheduler *sched) : scheduler(sched)
+{
+    // Configure the CAN driver
+    CanbusDriver::AutoBitTiming bitTiming;
+    bitTiming.baudRate    = BAUD_RATE;
+    bitTiming.samplePoint = SAMPLE_POINT;
+    CanbusDriver::CanbusConfig config;
+
+    driver = new CanbusDriver(CAN2, config, bitTiming);
+
+    // Create the protocol with the defined driver
+    protocol =
+        new CanProtocol(driver, bind(&CanHandler::handleCanMessage, this, _1),
+                        miosix::PRIORITY_MAX - 1);
+
+    // Accept messages only from the main and RIG board
+    protocol->addFilter(static_cast<uint8_t>(Board::MAIN),
+                        static_cast<uint8_t>(Board::BROADCAST));
+    protocol->addFilter(static_cast<uint8_t>(Board::RIG),
+                        static_cast<uint8_t>(Board::BROADCAST));
+    driver->init();
+}
+
+bool CanHandler::start()
+{
+    // 0 if fail
+    auto result1 = scheduler->addTask(
+        [&]()
+        {
+            auto sensors = ModuleManager::getInstance().get<Sensors>();
+
+            protocol->enqueueData(static_cast<uint8_t>(Priority::HIGH),
+                                  static_cast<uint8_t>(PrimaryType::SENSORS),
+                                  static_cast<uint8_t>(Board::MOTOR),
+                                  static_cast<uint8_t>(Board::BROADCAST),
+                                  static_cast<uint8_t>(SensorId::CC_PRESSURE),
+                                  static_cast<PressureData>(
+                                      sensors->getChamberPressureSensorData()));
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::HIGH),
+                static_cast<uint8_t>(PrimaryType::SENSORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(SensorId::BOTTOM_TANK_PRESSURE),
+                static_cast<PressureData>(
+                    sensors->getTankPressureSensor1Data()));
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::HIGH),
+                static_cast<uint8_t>(PrimaryType::SENSORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(SensorId::TOP_TANK_PRESSURE),
+                static_cast<PressureData>(
+                    sensors->getTankPressureSensor2Data()));
+        },
+        PRESSURES_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER);
+
+    auto result2 = scheduler->addTask(
+        [&]()
+        {
+            auto sensors = ModuleManager::getInstance().get<Sensors>();
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::MEDIUM),
+                static_cast<uint8_t>(PrimaryType::SENSORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(SensorId::TANK_TEMPERATURE),
+                static_cast<TemperatureData>(sensors->getMAX31856Data()));
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::MEDIUM),
+                static_cast<uint8_t>(PrimaryType::SENSORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(SensorId::MOTOR_ACTUATORS_CURRENT),
+                static_cast<CurrentData>(sensors->getServoCurrentData()));
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::MEDIUM),
+                static_cast<uint8_t>(PrimaryType::SENSORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(SensorId::MOTOR_BOARD_VOLTAGE),
+                static_cast<BatteryVoltageSensorData>(
+                    sensors->getBatteryData()));
+        },
+        TEMPERATURE_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER);
+
+    auto result3 = scheduler->addTask(
+        [&]()
+        {
+            protocol->enqueueSimplePacket(
+                static_cast<uint8_t>(Priority::MEDIUM),
+                static_cast<uint8_t>(PrimaryType::STATUS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST), initStatus, initStatus);
+        },
+        STATUS_TRANSMISSION_PERIOD);
+
+    auto result4 = scheduler->addTask(
+        [&]()
+        {
+            ModuleManager &modules = ModuleManager::getInstance();
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::HIGH),
+                static_cast<uint8_t>(PrimaryType::ACTUATORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(ServosList::MAIN_VALVE),
+                ServoData{TimestampTimer::getTimestamp(), 0, 0,
+                          modules.get<Actuators>()->getServoPosition(
+                              ServosList::MAIN_VALVE)});
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::HIGH),
+                static_cast<uint8_t>(PrimaryType::ACTUATORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(ServosList::VENTING_VALVE),
+                ServoData{TimestampTimer::getTimestamp(), 0, 0,
+                          modules.get<Actuators>()->getServoPosition(
+                              ServosList::VENTING_VALVE)});
+        },
+        ACTUATORS_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER);
+
+    return protocol->start() && result1 != 0 && result2 != 0 && result3 != 0 &&
+           result4 != 0;
+}
+
+bool CanHandler::isStarted()
+{
+    return protocol->isStarted() && scheduler->isRunning();
+}
+
+void CanHandler::sendEvent(EventId event)
+{
+    protocol->enqueueEvent(static_cast<uint8_t>(Priority::CRITICAL),
+                           static_cast<uint8_t>(PrimaryType::EVENTS),
+                           static_cast<uint8_t>(Board::MOTOR),
+                           static_cast<uint8_t>(Board::BROADCAST),
+                           static_cast<uint8_t>(event));
+}
+
+void CanHandler::setInitStatus(bool initResult) { initStatus = initResult; }
+
+void CanHandler::handleCanMessage(const CanMessage &msg)
+{
+    PrimaryType msgType = static_cast<PrimaryType>(msg.getPrimaryType());
+
+    // Depending on the received message, call the handling method
+    switch (msgType)
+    {
+        case PrimaryType::EVENTS:
+        {
+            handleCanEvent(msg);
+            break;
+        }
+        case PrimaryType::COMMAND:
+        {
+            handleCanCommand(msg);
+            break;
+        }
+        default:
+        {
+            LOG_WARN(logger, "Received unsupported message type: type={}",
+                     msgType);
+            break;
+        }
+    }
+}
+
+void CanHandler::handleCanEvent(const CanMessage &msg)
+{
+    EventId eventId = static_cast<EventId>(msg.getSecondaryType());
+
+    switch (eventId)
+    {
+        case EventId::CALIBRATE:
+        {
+            auto sensors = ModuleManager::getInstance().get<Sensors>();
+            sensors->calibrate();
+
+            break;
+        }
+        default:
+        {
+            LOG_WARN(logger, "Received unsupported event: id={}", eventId);
+            break;
+        }
+    }
+}
+
+void CanHandler::handleCanCommand(const CanMessage &msg)
+{
+    uint64_t payload = msg.payload[0];
+    ServosList servo = static_cast<ServosList>(msg.getSecondaryType());
+    bool targetState = static_cast<uint8_t>(payload);
+    uint32_t delay   = static_cast<uint32_t>(payload >> 8);
+
+    auto actuators = ModuleManager::getInstance().get<Actuators>();
+
+    if (actuators != nullptr)
+    {
+        if (targetState)
+        {
+            // printf("Opening %d for %ld\n", msg.getSecondaryType(), delay);
+            actuators->openServoAtomic(servo, delay);
+        }
+        else
+        {
+            // printf("Closing %d for %ld\n", msg.getSecondaryType(), delay);
+            actuators->closeServo(servo);
+        }
+    }
+}
+
+}  // namespace Motor
diff --git a/src/boards/Motor_old/CanHandler/CanHandler.h b/src/boards/Motor_old/CanHandler/CanHandler.h
new file mode 100644
index 0000000000000000000000000000000000000000..1ccc2b90a4c43e77250b634882491a866f71e2b5
--- /dev/null
+++ b/src/boards/Motor_old/CanHandler/CanHandler.h
@@ -0,0 +1,84 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Alberto Nidasio
+ *
+ * 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 <common/CanConfig.h>
+#include <drivers/canbus/CanProtocol/CanProtocol.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Motor
+{
+
+class CanHandler : public Boardcore::Module
+{
+
+public:
+    explicit CanHandler(Boardcore::TaskScheduler *sched);
+
+    /**
+     * @brief Adds the periodic task to the scheduler and starts the protocol
+     * threads
+     */
+    bool start();
+
+    /**
+     * @brief Returns true if the protocol threads are started and the scheduler
+     * is running
+     */
+    bool isStarted();
+
+    /**
+     * @brief Sends a CAN event on the bus
+     */
+    void sendEvent(Common::CanConfig::EventId event);
+
+    /**
+     * @brief Set the initialization flag to true
+     */
+    void setInitStatus(bool initResult);
+
+private:
+    /**
+     * @brief Handles a generic CAN message and dispatch the message to the
+     * correct handler
+     */
+    void handleCanMessage(const Boardcore::Canbus::CanMessage &msg);
+
+    // CAN message handlers
+    void handleCanEvent(const Boardcore::Canbus::CanMessage &msg);
+    void handleCanCommand(const Boardcore::Canbus::CanMessage &msg);
+
+    // Init status
+    std::atomic<bool> initStatus{false};
+
+    // CAN interfaces
+    Boardcore::Canbus::CanbusDriver *driver;
+    Boardcore::Canbus::CanProtocol *protocol;
+
+    Boardcore::TaskScheduler *scheduler;
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler");
+};
+
+}  // namespace Motor
diff --git a/src/boards/Motor/Configs/ActuatorsConfig.h b/src/boards/Motor_old/Configs/ActuatorsConfig.h
similarity index 100%
rename from src/boards/Motor/Configs/ActuatorsConfig.h
rename to src/boards/Motor_old/Configs/ActuatorsConfig.h
diff --git a/src/boards/Motor/Configs/CanHandlerConfig.h b/src/boards/Motor_old/Configs/CanHandlerConfig.h
similarity index 100%
rename from src/boards/Motor/Configs/CanHandlerConfig.h
rename to src/boards/Motor_old/Configs/CanHandlerConfig.h
diff --git a/src/boards/Motor_old/Configs/SensorsConfig.h b/src/boards/Motor_old/Configs/SensorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..44b13b762ad92deb64f7870ab34d8a1fdac827d0
--- /dev/null
+++ b/src/boards/Motor_old/Configs/SensorsConfig.h
@@ -0,0 +1,199 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio
+ *
+ * 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 <Motor/Sensors/Sensors.h>
+
+namespace Motor
+{
+
+namespace SensorsConfig
+{
+
+constexpr Boardcore::InternalADC::Channel ADC_BATTERY_VOLTAGE_CH =
+    Boardcore::InternalADC::Channel::CH15;
+// The battery is connected to the stm32's adc with a voltage divider
+constexpr float ADC_BATTERY_VOLTAGE_COEFF = (10 + 20) / 10;
+
+// Boardcore::LSM6DSRXConfig LSM6_SENSOR_CONFIG{
+//     .bdu       = Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE,
+//     .odrAcc    = Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_1660,
+//     .opModeAcc = Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL,
+//     .fsAcc     = Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16,
+//     .odrGyr    = Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_1660,
+//     .opModeGyr = Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL,
+//     .fsGyr     = Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_1000,
+//     .fifoMode  = Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS,
+//     .fifoTimestampDecimation =
+//         Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1,
+//     .fifoTemperatureBdr =
+//         Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED,
+//     .int1InterruptSelection = Boardcore::LSM6DSRXConfig::INTERRUPT::NOTHING,
+//     .int2InterruptSelection =
+//         Boardcore::LSM6DSRXConfig::INTERRUPT::FIFO_THRESHOLD,
+//     .fifoWatermark = 170,
+// };
+// Boardcore::SPIBusConfig LSM6_SPI_CONFIG{
+//     Boardcore::SPI::ClockDivider::DIV_16,
+//     Boardcore::SPI::Mode::MODE_0,
+// };
+
+Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS_ODR =
+    Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_1000;
+Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS_BDU =
+    Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE;
+Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS_FSR =
+    Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100;
+Boardcore::SPIBusConfig H3LIS_SPI_CONFIG{
+    Boardcore::SPI::ClockDivider::DIV_16,
+};
+
+Boardcore::LIS2MDL::Config LIS2_SENSOR_CONFIG{
+    .odr                = Boardcore::LIS2MDL::ODR_100_HZ,
+    .deviceMode         = Boardcore::LIS2MDL::MD_CONTINUOUS,
+    .temperatureDivider = 100,
+};
+Boardcore::SPIBusConfig LIS2_SPI_CONFIG{
+    Boardcore::SPI::ClockDivider::DIV_16,
+    Boardcore::SPI::Mode::MODE_3,
+    Boardcore::SPI::Order::MSB_FIRST,
+    Boardcore::SPI::Order::LSB_FIRST,
+};
+
+Boardcore::LPS22DF::Config LPS22_SENSOR_CONFIG{
+    .odr = Boardcore::LPS22DF::ODR_100,
+    .avg = Boardcore::LPS22DF::AVG_512,
+};
+Boardcore::SPIBusConfig LPS22_SPI_CONFIG{
+    Boardcore::SPI::ClockDivider::DIV_16,
+    Boardcore::SPI::Mode::MODE_3,
+    Boardcore::SPI::Order::MSB_FIRST,
+    Boardcore::SPI::Order::LSB_FIRST,
+};
+
+Boardcore::ADS131M08::Config ADS131_SENSOR_CONFIG{
+    .channelsConfig =
+        {
+            {.enabled = false},
+            // For the servo motor's current sensor the current range is from 0A
+            // to 20A and the output voltage from 0.65V to 2.65V. The sensor is
+            // connected to the adc through a voltage divider with a gain
+            // of 4.166666 So at the ADC side we have maximum 0.636 and we have
+            // to set a gain of 1 on the ADC
+            {
+                .enabled = true,
+                .pga     = Boardcore::ADS131M08Defs::PGA::PGA_1,
+            },
+            {.enabled = false},
+            {.enabled = false},
+            {.enabled = false},
+            // The analog pressure sensors are measured with a 30ohm shunt
+            // resistor. The current ranges from 4mAh to 20mAh. So we can have a
+            // voltage up to 0.6V We set a gain of 2 to maximize resolution
+            {
+                .enabled = true,
+                .pga     = Boardcore::ADS131M08Defs::PGA::PGA_2,
+            },
+            {
+                .enabled = true,
+                .pga     = Boardcore::ADS131M08Defs::PGA::PGA_2,
+            },
+            {
+                .enabled = true,
+                .pga     = Boardcore::ADS131M08Defs::PGA::PGA_2,
+            },
+        },
+    .oversamplingRatio = Boardcore::ADS131M08Defs::OversamplingRatio::OSR_4096,
+    .globalChopModeEnabled = true,
+};
+Boardcore::SPIBusConfig ADS131_SPI_CONFIG{
+    Boardcore::SPI::ClockDivider::DIV_8,
+    Boardcore::SPI::Mode::MODE_1,
+};
+Boardcore::ADS131M08Defs::Channel ADS131_CHAMBER_PRESSURE_CH =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_5;
+Boardcore::ADS131M08Defs::Channel ADS131_TANK_PRESSURE_1_CH =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_6;
+Boardcore::ADS131M08Defs::Channel ADS131_TANK_PRESSURE_2_CH =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_7;
+Boardcore::ADS131M08Defs::Channel ADS131_SERVO_CURRENT_CH =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_1;
+
+// The chamber pressure sensor is a 0-40bar sensor with a 4-20mA output
+// On the motor motherboard there a 30ohm shunt. The shunt then goes through a
+// filter with 491Hz of bandwidth and unitary gain.
+// We use only a gain coefficient because the offset is removed by the ADC
+// offset calibration feature.
+constexpr float CHAMBER_PRESSURE_MAX      = 40;
+constexpr float CHAMBER_PRESSURE_MIN      = 0;
+constexpr float CHAMBER_PRESSURE_SHUNT    = 30;     // [ohm]
+constexpr float CHAMBER_PRESSURE_CURR_MIN = 0.004;  // [A]
+constexpr float CHAMBER_PRESSURE_CURR_MAX = 0.020;  // [A]
+constexpr float CHAMBER_PRESSURE_COEFF =
+    CHAMBER_PRESSURE_MAX /
+    (CHAMBER_PRESSURE_CURR_MAX - CHAMBER_PRESSURE_CURR_MIN);  // [bar/A]
+
+// The tank pressure sensors are 0-100bar sensors with a 4-20mA output
+// On the motor motherboard there a 30ohm shunt. The shunt then goes through a
+// filter with 491Hz of bandwidth and unitary gain.
+// We use only a gain coefficient because the offset is removed by the ADC
+// offset calibration feature.
+constexpr float TANK_PRESSURE_1_MAX      = 100;
+constexpr float TANK_PRESSURE_1_MIN      = 0;
+constexpr float TANK_PRESSURE_1_SHUNT    = 30;     // [ohm]
+constexpr float TANK_PRESSURE_1_CURR_MIN = 0.004;  // [A]
+constexpr float TANK_PRESSURE_1_CURR_MAX = 0.020;  // [A]
+constexpr float TANK_PRESSURE_1_COEFF =
+    TANK_PRESSURE_1_MAX /
+    (TANK_PRESSURE_1_CURR_MAX - TANK_PRESSURE_1_CURR_MIN);  // [bar/A]
+constexpr float TANK_PRESSURE_2_MAX      = 100;
+constexpr float TANK_PRESSURE_2_MIN      = 0;
+constexpr float TANK_PRESSURE_2_SHUNT    = 30;     // [ohm]
+constexpr float TANK_PRESSURE_2_CURR_MIN = 0.004;  // [A]
+constexpr float TANK_PRESSURE_2_CURR_MAX = 0.020;  // [A]
+constexpr float TANK_PRESSURE_2_COEFF =
+    TANK_PRESSURE_2_MAX /
+    (TANK_PRESSURE_2_CURR_MAX - TANK_PRESSURE_2_CURR_MIN);  // [bar/A]
+
+// The current sensor used to measure the servo motors current consumption:
+// - A range of 0A to 20A
+// - Has an output from 0.65V to 2.65V
+// - Is connected to a voltage divider with a coefficient of 12 / (38.3 + 12)
+//
+// We use only a gain coefficient because the offset is removed by the ADC
+// offset calibration feature.
+constexpr float SERVO_CURRENT_COEFF =
+    10 / (2.65 - 0.65) / (12 / (38.3 + 12));  // [A/V]
+
+// Sampling periods
+constexpr uint32_t SAMPLE_PERIOD_ADC    = 1000;
+constexpr uint32_t SAMPLE_PERIOD_LSM6   = 10;
+constexpr uint32_t SAMPLE_PERIOD_H3LIS  = 100;
+constexpr uint32_t SAMPLE_PERIOD_LIS2   = 10;
+constexpr uint32_t SAMPLE_PERIOD_LPS22  = 10;
+constexpr uint32_t SAMPLE_PERIOD_ADS131 = 1;
+constexpr uint32_t SAMPLE_PERIOD_MAX    = 10;
+
+}  // namespace SensorsConfig
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensor.h b/src/boards/Motor_old/Sensors/ChamberPressureSensor/ChamberPressureSensor.h
similarity index 100%
rename from src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensor.h
rename to src/boards/Motor_old/Sensors/ChamberPressureSensor/ChamberPressureSensor.h
diff --git a/src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensorData.h b/src/boards/Motor_old/Sensors/ChamberPressureSensor/ChamberPressureSensorData.h
similarity index 100%
rename from src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensorData.h
rename to src/boards/Motor_old/Sensors/ChamberPressureSensor/ChamberPressureSensorData.h
diff --git a/src/boards/Motor_old/Sensors/Sensors.cpp b/src/boards/Motor_old/Sensors/Sensors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0643a6865d1b0d96f9d7ced1d301689efc21f478
--- /dev/null
+++ b/src/boards/Motor_old/Sensors/Sensors.cpp
@@ -0,0 +1,427 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio, Matteo Pignataro
+ *
+ * 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 "Sensors.h"
+
+#include <Motor/Buses.h>
+#include <Motor/Configs/SensorsConfig.h>
+#include <drivers/interrupt/external_interrupts.h>
+#include <interfaces-impl/hwmapping.h>
+#include <logger/Logger.h>
+
+using namespace std;
+using namespace miosix;
+using namespace Boardcore;
+using namespace Motor::SensorsConfig;
+
+namespace Motor
+{
+
+InternalADCData Sensors::getADCData()
+{
+    miosix::PauseKernelLock lock;
+    return adc != nullptr ? adc->getLastSample() : InternalADCData{};
+}
+
+BatteryVoltageSensorData Sensors::getBatteryData()
+{
+    miosix::PauseKernelLock lock;
+    return battery != nullptr ? battery->getLastSample()
+                              : BatteryVoltageSensorData{};
+}
+
+// LSM6DSRXData Sensors::getLSM6DSRXData()
+// {
+//     miosix::PauseKernelLock lock;
+//     return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{};
+// }
+
+H3LIS331DLData Sensors::getH3LIS331DLData()
+{
+    miosix::PauseKernelLock lock;
+    return h3lis331dl != nullptr ? h3lis331dl->getLastSample()
+                                 : H3LIS331DLData{};
+}
+
+LIS2MDLData Sensors::getLIS2MDLData()
+{
+    miosix::PauseKernelLock lock;
+    return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{};
+}
+
+LPS22DFData Sensors::getLPS22DFData()
+{
+    miosix::PauseKernelLock lock;
+    return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{};
+}
+
+ADS131M08Data Sensors::getADS131M08Data()
+{
+    miosix::PauseKernelLock lock;
+    return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{};
+}
+
+MAX31856Data Sensors::getMAX31856Data()
+{
+    miosix::PauseKernelLock lock;
+    return max31856 != nullptr ? max31856->getLastSample() : MAX31856Data{};
+}
+
+ChamberPressureSensorData Sensors::getChamberPressureSensorData()
+{
+    miosix::PauseKernelLock lock;
+    return chamberPressure != nullptr ? chamberPressure->getLastSample()
+                                      : ChamberPressureSensorData{};
+}
+
+TankPressureSensor1Data Sensors::getTankPressureSensor1Data()
+{
+    miosix::PauseKernelLock lock;
+    return tankPressure1 != nullptr ? tankPressure1->getLastSample()
+                                    : TankPressureSensor1Data{};
+}
+
+TankPressureSensor2Data Sensors::getTankPressureSensor2Data()
+{
+    miosix::PauseKernelLock lock;
+    return tankPressure2 != nullptr ? tankPressure2->getLastSample()
+                                    : TankPressureSensor2Data{};
+}
+
+CurrentData Sensors::getServoCurrentData()
+{
+    miosix::PauseKernelLock lock;
+    return servosCurrent != nullptr ? servosCurrent->getLastSample()
+                                    : CurrentData{};
+}
+
+Sensors::Sensors(TaskScheduler* sched) : scheduler(sched) {}
+
+Sensors::~Sensors() {}
+
+bool Sensors::start()
+{
+    adcInit();
+    batteryInit();
+    // lsm6dsrxInit();
+    h3lis331dlInit();
+    // lis2mdlInit();
+    lps22dfInit();
+    max31856Init();
+    ads131m08Init();
+    chamberPressureInit();
+    tankPressure1Init();
+    tankPressure2Init();
+    servosCurrentInit();
+
+    sensorManager = new SensorManager(sensorsMap, scheduler);
+
+    return sensorManager->start();
+}
+
+void Sensors::calibrate()
+{
+    if (ads131m08 != nullptr)
+    {
+        ads131m08->calibrateOffset(ADS131_SERVO_CURRENT_CH);
+    }
+}
+
+void Sensors::adcInit()
+{
+    adc = new InternalADC(ADC1);
+
+    adc->enableTemperature();
+    adc->enableVbat();
+    adc->enableChannel(ADC_BATTERY_VOLTAGE_CH);
+
+    SensorInfo info("ADC", SAMPLE_PERIOD_ADC,
+                    bind(&Sensors::adcCallback, this));
+
+    sensorsMap.emplace(make_pair(adc, info));
+}
+
+void Sensors::batteryInit()
+{
+    function<ADCData()> getADCVoltage(
+        bind(&InternalADC::getVoltage, adc, ADC_BATTERY_VOLTAGE_CH));
+
+    battery =
+        new BatteryVoltageSensor(getADCVoltage, ADC_BATTERY_VOLTAGE_COEFF);
+
+    SensorInfo info("BATTERY", SAMPLE_PERIOD_ADC,
+                    bind(&Sensors::batteryCallback, this));
+
+    sensorsMap.emplace(make_pair(battery, info));
+}
+
+// void Sensors::lsm6dsrxInit()
+// {
+//     SPIBus& spi1 = ModuleManager::getInstance().get<Buses>()->spi1;
+
+//     lsm6dsrx = new LSM6DSRX(spi1, peripherals::lsm6dsrx::cs::getPin(),
+//                             LSM6_SPI_CONFIG, LSM6_SENSOR_CONFIG);
+
+//     SensorInfo info("LSM6DSRX", SAMPLE_PERIOD_LSM6,
+//                     bind(&Sensors::lsm6dsrxCallback, this));
+
+//     sensorsMap.emplace(make_pair(lsm6dsrx, info));
+// }
+
+void Sensors::h3lis331dlInit()
+{
+    SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3;
+
+    h3lis331dl = new H3LIS331DL(spi3, peripherals::h3lis331dl::cs::getPin(),
+                                H3LIS_ODR, H3LIS_BDU, H3LIS_FSR);
+
+    SensorInfo info("H3LIS331DL", SAMPLE_PERIOD_H3LIS,
+                    bind(&Sensors::h3lis331dlCallback, this));
+
+    sensorsMap.emplace(make_pair(h3lis331dl, info));
+}
+
+void Sensors::lis2mdlInit()
+{
+    SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3;
+
+    lis2mdl = new LIS2MDL(spi3, peripherals::lis2mdl::cs::getPin(),
+                          LIS2_SPI_CONFIG, LIS2_SENSOR_CONFIG);
+
+    SensorInfo info("LIS2MDL", SAMPLE_PERIOD_LIS2,
+                    bind(&Sensors::lis2mdlCallback, this));
+
+    sensorsMap.emplace(make_pair(lis2mdl, info));
+}
+
+void Sensors::lps22dfInit()
+{
+    SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3;
+
+    lps22df = new LPS22DF(spi3, peripherals::lps22df::cs::getPin(),
+                          LPS22_SPI_CONFIG, LPS22_SENSOR_CONFIG);
+
+    SensorInfo info("LPS22", SAMPLE_PERIOD_LPS22,
+                    bind(&Sensors::lps22dfCallback, this));
+
+    sensorsMap.emplace(make_pair(lps22df, info));
+}
+
+void Sensors::max31856Init()
+{
+    SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3;
+
+    max31856 = new MAX31856(spi3, peripherals::max31856::cs::getPin());
+
+    SensorInfo info("MAX31856", SAMPLE_PERIOD_MAX,
+                    bind(&Sensors::max31856Callback, this));
+
+    sensorsMap.emplace(make_pair(max31856, info));
+}
+
+void Sensors::ads131m08Init()
+{
+    SPIBus& spi4 = ModuleManager::getInstance().get<Buses>()->spi4;
+
+    ads131m08 = new ADS131M08(spi4, peripherals::ads131m08::cs::getPin(),
+                              ADS131_SPI_CONFIG, ADS131_SENSOR_CONFIG);
+
+    SensorInfo info("ADS131M08", SAMPLE_PERIOD_ADS131,
+                    bind(&Sensors::ads131m08Callback, this));
+
+    sensorsMap.emplace(make_pair(ads131m08, info));
+}
+
+void Sensors::chamberPressureInit()
+{
+    if (ads131m08 == nullptr)
+    {
+        return;
+    }
+
+    function<ADCData()> getADCVoltage(
+        [&](void)
+        {
+            auto data = ads131m08->getLastSample();
+            return data.getVoltage(ADS131_CHAMBER_PRESSURE_CH);
+        });
+
+    function<float(float)> voltageToPressure(
+        [](float voltage)
+        {
+            float current =
+                voltage / CHAMBER_PRESSURE_SHUNT - CHAMBER_PRESSURE_CURR_MIN;
+            return current * CHAMBER_PRESSURE_COEFF;
+        });
+
+    chamberPressure =
+        new ChamberPressureSensor(getADCVoltage, voltageToPressure);
+
+    SensorInfo info("CHAMBER_PRESSURE", SAMPLE_PERIOD_ADS131,
+                    bind(&Sensors::chamberPressureCallback, this));
+
+    sensorsMap.emplace(make_pair(chamberPressure, info));
+}
+
+void Sensors::tankPressure1Init()
+{
+    if (ads131m08 == nullptr)
+    {
+        return;
+    }
+
+    function<ADCData()> getADCVoltage(
+        [&](void)
+        {
+            auto data = ads131m08->getLastSample();
+            return data.getVoltage(ADS131_TANK_PRESSURE_1_CH);
+        });
+
+    function<float(float)> voltageToPressure(
+        [](float voltage)
+        {
+            float current =
+                voltage / TANK_PRESSURE_1_SHUNT - TANK_PRESSURE_1_CURR_MIN;
+            return current * TANK_PRESSURE_1_COEFF;
+        });
+
+    tankPressure1 = new TankPressureSensor1(getADCVoltage, voltageToPressure);
+
+    SensorInfo info("TANK_PRESSURE_1", SAMPLE_PERIOD_ADS131,
+                    bind(&Sensors::tankPressure1Callback, this));
+
+    sensorsMap.emplace(make_pair(tankPressure1, info));
+}
+
+void Sensors::tankPressure2Init()
+{
+    if (ads131m08 == nullptr)
+    {
+        return;
+    }
+
+    function<ADCData()> getADCVoltage(
+        [&](void)
+        {
+            auto data = ads131m08->getLastSample();
+            return data.getVoltage(ADS131_TANK_PRESSURE_2_CH);
+        });
+
+    function<float(float)> voltageToPressure(
+        [](float voltage)
+        {
+            float current =
+                voltage / TANK_PRESSURE_2_SHUNT - TANK_PRESSURE_2_CURR_MIN;
+            return current * TANK_PRESSURE_2_COEFF;
+        });
+
+    tankPressure2 = new TankPressureSensor2(getADCVoltage, voltageToPressure);
+
+    SensorInfo info("TANK_PRESSURE_2", SAMPLE_PERIOD_ADS131,
+                    bind(&Sensors::tankPressure2Callback, this));
+
+    sensorsMap.emplace(make_pair(tankPressure2, info));
+}
+
+void Sensors::servosCurrentInit()
+{
+    if (ads131m08 == nullptr)
+    {
+        return;
+    }
+
+    function<ADCData()> getADCVoltage(
+        [&](void)
+        {
+            auto data = ads131m08->getLastSample();
+            return data.getVoltage(ADS131_SERVO_CURRENT_CH);
+        });
+
+    function<float(float)> voltageToCurrent(
+        [](float voltage) { return voltage * SERVO_CURRENT_COEFF; });
+
+    servosCurrent = new CurrentSensor(getADCVoltage, voltageToCurrent);
+
+    SensorInfo info("SERVOS_CURRENT", SAMPLE_PERIOD_ADS131,
+                    bind(&Sensors::servosCurrentCallback, this));
+
+    sensorsMap.emplace(make_pair(servosCurrent, info));
+}
+
+void Sensors::adcCallback() { Logger::getInstance().log(adc->getLastSample()); }
+
+void Sensors::batteryCallback()
+{
+    Logger::getInstance().log(battery->getLastSample());
+}
+
+// void Sensors::lsm6dsrxCallback()
+// {
+//     Logger::getInstance().log(lsm6dsrx->getLastSample());
+// }
+
+void Sensors::h3lis331dlCallback()
+{
+    Logger::getInstance().log(h3lis331dl->getLastSample());
+}
+
+void Sensors::lis2mdlCallback()
+{
+    Logger::getInstance().log(lis2mdl->getLastSample());
+}
+
+void Sensors::lps22dfCallback()
+{
+    Logger::getInstance().log(lps22df->getLastSample());
+}
+
+void Sensors::max31856Callback()
+{
+    Logger::getInstance().log(max31856->getLastSample());
+}
+
+void Sensors::ads131m08Callback()
+{
+    Logger::getInstance().log(ads131m08->getLastSample());
+}
+
+void Sensors::chamberPressureCallback()
+{
+    Logger::getInstance().log(chamberPressure->getLastSample());
+}
+
+void Sensors::tankPressure1Callback()
+{
+    Logger::getInstance().log(tankPressure1->getLastSample());
+}
+
+void Sensors::tankPressure2Callback()
+{
+    Logger::getInstance().log(tankPressure2->getLastSample());
+}
+
+void Sensors::servosCurrentCallback()
+{
+    Logger::getInstance().log(servosCurrent->getLastSample());
+}
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor_old/Sensors/Sensors.h b/src/boards/Motor_old/Sensors/Sensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..780563efec9feef081b3258fed1fc4f66477854a
--- /dev/null
+++ b/src/boards/Motor_old/Sensors/Sensors.h
@@ -0,0 +1,124 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <Motor/Sensors/ChamberPressureSensor/ChamberPressureSensor.h>
+#include <Motor/Sensors/TankPressureSensor1/TankPressureSensor1.h>
+#include <Motor/Sensors/TankPressureSensor2/TankPressureSensor2.h>
+#include <drivers/adc/InternalADC.h>
+#include <sensors/ADS131M08/ADS131M08.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS2MDL/LIS2MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+// #include <sensors/LSM6DSRX/LSM6DSRX.h>
+#include <sensors/MAX31856/MAX31856.h>
+#include <sensors/SensorManager.h>
+#include <sensors/analog/BatteryVoltageSensor.h>
+#include <sensors/analog/CurrentSensor.h>
+#include <sensors/analog/pressure/AnalogPressureSensor.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Motor
+{
+
+class Sensors : public Boardcore::Module
+{
+public:
+    Boardcore::InternalADCData getADCData();
+    Boardcore::BatteryVoltageSensorData getBatteryData();
+    // Boardcore::LSM6DSRXData getLSM6DSRXData();
+    Boardcore::H3LIS331DLData getH3LIS331DLData();
+    Boardcore::LIS2MDLData getLIS2MDLData();
+    Boardcore::LPS22DFData getLPS22DFData();
+    Boardcore::ADS131M08Data getADS131M08Data();
+    Boardcore::MAX31856Data getMAX31856Data();
+    Boardcore::ChamberPressureSensorData getChamberPressureSensorData();
+    Boardcore::TankPressureSensor1Data getTankPressureSensor1Data();
+    Boardcore::TankPressureSensor2Data getTankPressureSensor2Data();
+    Boardcore::CurrentData getServoCurrentData();
+
+    explicit Sensors(Boardcore::TaskScheduler* sched);
+
+    ~Sensors();
+
+    bool start();
+
+    void calibrate();
+
+private:
+    void adcInit();
+    void adcCallback();
+
+    void batteryInit();
+    void batteryCallback();
+
+    // void lsm6dsrxInit();
+    // void lsm6dsrxCallback();
+
+    void h3lis331dlInit();
+    void h3lis331dlCallback();
+
+    void lis2mdlInit();
+    void lis2mdlCallback();
+
+    void lps22dfInit();
+    void lps22dfCallback();
+
+    void max31856Init();
+    void max31856Callback();
+
+    void ads131m08Init();
+    void ads131m08Callback();
+
+    void chamberPressureInit();
+    void chamberPressureCallback();
+
+    void tankPressure1Init();
+    void tankPressure1Callback();
+
+    void tankPressure2Init();
+    void tankPressure2Callback();
+
+    void servosCurrentInit();
+    void servosCurrentCallback();
+
+    Boardcore::InternalADC* adc              = nullptr;
+    Boardcore::BatteryVoltageSensor* battery = nullptr;
+    // Boardcore::LSM6DSRX* lsm6dsrx                     = nullptr;
+    Boardcore::H3LIS331DL* h3lis331dl                 = nullptr;
+    Boardcore::LIS2MDL* lis2mdl                       = nullptr;
+    Boardcore::LPS22DF* lps22df                       = nullptr;
+    Boardcore::MAX31856* max31856                     = nullptr;
+    Boardcore::ADS131M08* ads131m08                   = nullptr;
+    Boardcore::ChamberPressureSensor* chamberPressure = nullptr;
+    Boardcore::TankPressureSensor1* tankPressure1     = nullptr;
+    Boardcore::TankPressureSensor2* tankPressure2     = nullptr;
+    Boardcore::CurrentSensor* servosCurrent           = nullptr;
+
+    Boardcore::SensorManager::SensorMap_t sensorsMap;
+    Boardcore::SensorManager* sensorManager = nullptr;
+    Boardcore::TaskScheduler* scheduler     = nullptr;
+};
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1.h b/src/boards/Motor_old/Sensors/TankPressureSensor1/TankPressureSensor1.h
similarity index 100%
rename from src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1.h
rename to src/boards/Motor_old/Sensors/TankPressureSensor1/TankPressureSensor1.h
diff --git a/src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1Data.h b/src/boards/Motor_old/Sensors/TankPressureSensor1/TankPressureSensor1Data.h
similarity index 100%
rename from src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1Data.h
rename to src/boards/Motor_old/Sensors/TankPressureSensor1/TankPressureSensor1Data.h
diff --git a/src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2.h b/src/boards/Motor_old/Sensors/TankPressureSensor2/TankPressureSensor2.h
similarity index 100%
rename from src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2.h
rename to src/boards/Motor_old/Sensors/TankPressureSensor2/TankPressureSensor2.h
diff --git a/src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2Data.h b/src/boards/Motor_old/Sensors/TankPressureSensor2/TankPressureSensor2Data.h
similarity index 100%
rename from src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2Data.h
rename to src/boards/Motor_old/Sensors/TankPressureSensor2/TankPressureSensor2Data.h
diff --git a/src/entrypoints/Motor/motor-entry.cpp b/src/entrypoints/Motor/motor-entry.cpp
index 8d42fd5b4c35dc790a9990fd17f86ae753d08f88..61ad36f243e172e4fb848ac0a3db8c81c28ae97d 100644
--- a/src/entrypoints/Motor/motor-entry.cpp
+++ b/src/entrypoints/Motor/motor-entry.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Alberto Nidasio
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -20,189 +20,86 @@
  * THE SOFTWARE.
  */
 
-#include <Motor/Actuators/Actuators.h>
-#include <Motor/BoardScheduler.h>
 #include <Motor/Buses.h>
 #include <Motor/CanHandler/CanHandler.h>
 #include <Motor/Sensors/Sensors.h>
-#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <diagnostic/PrintLogger.h>
+#include <interfaces-impl/hwmapping.h>
 #include <miosix.h>
 
-using namespace miosix;
+#include <utils/ModuleManager/ModuleManager.hpp>
+
 using namespace Boardcore;
 using namespace Motor;
+using namespace miosix;
 
 int main()
 {
-    ModuleManager& modules = ModuleManager::getInstance();
-
-    // Overall status, if at some point it becomes false, there is a problem
-    // somewhere
-    bool initResult    = true;
-    PrintLogger logger = Logging::getLogger("main");
-
-    // Create modules
-    BoardScheduler* scheduler = new BoardScheduler();
-    Buses* buses              = new Buses();
-    auto actuators =
-        new Actuators(scheduler->getScheduler(miosix::PRIORITY_MAX));
-    auto sensors =
-        new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1));
-    auto canHandler =
-        new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+    ModuleManager &modules = ModuleManager::getInstance();
+    PrintLogger logger     = Logging::getLogger("main");
 
-    // Insert modules
-    if (!modules.insert<BoardScheduler>(scheduler))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the board scheduler module");
-    }
+    // TODO: Move this to a dedicated board scheduler
+    TaskScheduler *scheduler1 = new TaskScheduler(2);
+    TaskScheduler *scheduler2 = new TaskScheduler(3);
 
-    if (!modules.insert<Buses>(buses))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the buses module");
-    }
+    Buses *buses           = new Buses();
+    Sensors *sensors       = new Sensors(*scheduler2);
+    CanHandler *canHandler = new CanHandler();
+
+    bool initResult = true;
 
-    if (!modules.insert<Actuators>(actuators))
+    // Insert modules
+    if (!modules.insert<Buses>(buses))
     {
         initResult = false;
-        LOG_ERR(logger, "Error inserting the sensor module");
     }
 
     if (!modules.insert<Sensors>(sensors))
     {
         initResult = false;
-        LOG_ERR(logger, "Error inserting the sensor module");
     }
 
     if (!modules.insert<CanHandler>(canHandler))
     {
         initResult = false;
-        LOG_ERR(logger, "Error inserting the CanHandler module");
     }
 
     // Start modules
-    if (!Logger::getInstance().start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the logger module");
-    }
-
-    if (!modules.get<BoardScheduler>()->start())
+    if (!sensors->start())
     {
         initResult = false;
-        LOG_ERR(logger, "Error starting the board scheduler module");
+        LOG_ERR(logger, "Error failed to start Sensors module");
     }
 
-    if (!modules.get<Actuators>()->start())
+    if (!canHandler->start())
     {
         initResult = false;
-        LOG_ERR(logger, "Error starting the sensors module");
+        LOG_ERR(logger, "Error failed to start CanHandler module");
     }
 
-    if (!modules.get<Sensors>()->start())
+    if (initResult)
     {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the sensors module");
+        LOG_INFO(logger, "All good!");
     }
-
-    if (!modules.get<CanHandler>()->start())
+    else
     {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the CanHandler module");
+        LOG_ERR(logger, "Init failure!");
     }
 
-    // Calibration
-    modules.get<Sensors>()->calibrate();
-
-    // Set the init status inside the CAN handler
-    modules.get<CanHandler>()->setInitStatus(initResult);
-
-    // Check the init result and launch an event
-    if (initResult)
-    {
-        miosix::led1On();
-    }
-    else
+    for (auto info : sensors->getSensorInfo())
     {
-        miosix::led2On();
+        LOG_INFO(logger, "{} {}", info.isInitialized, info.id);
     }
 
     while (true)
     {
-        printf("Average CPU usage: %.1f%%\n", CpuMeter::getCpuStats().mean);
-
-        auto adcData     = sensors->getADCData();
-        auto batteryData = sensors->getBatteryData();
-        printf("[%.2fs]\tADC:\t%f %f %f\n", adcData.timestamp / 1e6,
-               adcData.temperature, adcData.vBat, batteryData.batVoltage);
-
-        // // WARNING: Fails self test
-        // auto lsm6Data = sensors->getLSM6DSRXData();
-        // printf("[%.2fs]\tLSM6:\t%fm/s^2 %fm/s^2 %fm/s^2\n",
-        //        lsm6Data.accelerationTimestamp / 1e6,
-        //        lsm6Data.accelerationX * 0.001, lsm6Data.accelerationY *
-        //        0.001, lsm6Data.accelerationZ * 0.001);
-        // printf("[%.2fs]\tLSM6:\t%frad/s %frad/s %frad/s\n",
-        //        lsm6Data.angularSpeedTimestamp / 1e6,
-        //        lsm6Data.angularSpeedX * 0.001, lsm6Data.angularSpeedY *
-        //        0.001, lsm6Data.angularSpeedZ * 0.001);
-
-        // // WARNING: The values are wrong
-        // auto h3lisData = sensors->getH3LIS331DLData();
-        // printf("[%.2fs]\tH3LIS:\t%fm/s %fm/s %fm/s\n",
-        //        h3lisData.accelerationTimestamp / 1e6,
-        //        h3lisData.accelerationX, h3lisData.accelerationY,
-        //        h3lisData.accelerationZ);
-
-        // auto lis2Data = sensors->getLIS2MDLData();
-        // printf("[%.2fs]\tLIS2:\t%f %f %f\n",
-        //        lis2Data.magneticFieldTimestamp / 1e6,
-        //        lis2Data.magneticFieldX, lis2Data.magneticFieldY,
-        //        lis2Data.magneticFieldZ);
-
-        // auto lps22Data = sensors->getLPS22DFData();
-        // printf("[%.2fs]\tLPS22:\t%f %f\n", lps22Data.pressureTimestamp / 1e6,
-        //        lps22Data.pressure, lps22Data.temperature);
-
-        // auto maxData = sensors->getMAX31856Data();
-        // printf("[%.2fs]\tMAX:\t%f° %f°\n", maxData.temperatureTimestamp /
-        // 1e6,
-        //        maxData.temperature, maxData.coldJunctionTemperature);
-
-        auto adsData = sensors->getADS131M08Data();
-        printf("[%.2fs]\tADS131:\t%f %f %f %f\n", adsData.timestamp / 1e6,
-               adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage,
-               adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage,
-               adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage,
-               adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage);
-
-        auto chamberData = sensors->getChamberPressureSensorData();
-        printf("[%.2fs]\tCHAMBER:\t%fbar\n",
-               chamberData.pressureTimestamp / 1e6, chamberData.pressure);
-
-        auto tank1Data = sensors->getTankPressureSensor1Data();
-        printf("[%.2fs]\tTANK1:\t\t%fbar\n", tank1Data.pressureTimestamp / 1e6,
-               tank1Data.pressure);
-
-        auto tank2Data = sensors->getTankPressureSensor2Data();
-        printf("[%.2fs]\tTANK2:\t\t%fbar\n", tank2Data.pressureTimestamp / 1e6,
-               tank2Data.pressure);
-
-        auto servoCurrent = sensors->getServoCurrentData();
-        printf("[%.2fs]\tSERVO:\t\t%fA\n", servoCurrent.currentTimestamp / 1e6,
-               servoCurrent.current);
-
-        auto batteryVoltage = sensors->getBatteryData();
-        printf("[%.2fs]\tBATTERY:\t%fV\n",
-               batteryVoltage.voltageTimestamp / 1e6,
-               batteryVoltage.batVoltage);
-
-        auto loggerData = Logger::getInstance().getStats();
-        printf("[%.2fs]\tLOGGER:\t%d\n", loggerData.timestamp / 1e6,
-               loggerData.writesFailed);
-
+        gpios::boardLed::low();
+        canHandler->sendEvent(Common::CanConfig::EventId::ARM);
+        Thread::sleep(1000);
+        gpios::boardLed::high();
+        canHandler->sendEvent(Common::CanConfig::EventId::DISARM);
         Thread::sleep(1000);
     }
-}
+
+    return 0;
+}
\ No newline at end of file
diff --git a/src/entrypoints/Motor_old/motor-entry.cpp b/src/entrypoints/Motor_old/motor-entry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8d42fd5b4c35dc790a9990fd17f86ae753d08f88
--- /dev/null
+++ b/src/entrypoints/Motor_old/motor-entry.cpp
@@ -0,0 +1,208 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <Motor/Actuators/Actuators.h>
+#include <Motor/BoardScheduler.h>
+#include <Motor/Buses.h>
+#include <Motor/CanHandler/CanHandler.h>
+#include <Motor/Sensors/Sensors.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <miosix.h>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Motor;
+
+int main()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Overall status, if at some point it becomes false, there is a problem
+    // somewhere
+    bool initResult    = true;
+    PrintLogger logger = Logging::getLogger("main");
+
+    // Create modules
+    BoardScheduler* scheduler = new BoardScheduler();
+    Buses* buses              = new Buses();
+    auto actuators =
+        new Actuators(scheduler->getScheduler(miosix::PRIORITY_MAX));
+    auto sensors =
+        new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1));
+    auto canHandler =
+        new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+
+    // Insert modules
+    if (!modules.insert<BoardScheduler>(scheduler))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the board scheduler module");
+    }
+
+    if (!modules.insert<Buses>(buses))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the buses module");
+    }
+
+    if (!modules.insert<Actuators>(actuators))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the sensor module");
+    }
+
+    if (!modules.insert<Sensors>(sensors))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the sensor module");
+    }
+
+    if (!modules.insert<CanHandler>(canHandler))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the CanHandler module");
+    }
+
+    // Start modules
+    if (!Logger::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the logger module");
+    }
+
+    if (!modules.get<BoardScheduler>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the board scheduler module");
+    }
+
+    if (!modules.get<Actuators>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the sensors module");
+    }
+
+    if (!modules.get<Sensors>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the sensors module");
+    }
+
+    if (!modules.get<CanHandler>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the CanHandler module");
+    }
+
+    // Calibration
+    modules.get<Sensors>()->calibrate();
+
+    // Set the init status inside the CAN handler
+    modules.get<CanHandler>()->setInitStatus(initResult);
+
+    // Check the init result and launch an event
+    if (initResult)
+    {
+        miosix::led1On();
+    }
+    else
+    {
+        miosix::led2On();
+    }
+
+    while (true)
+    {
+        printf("Average CPU usage: %.1f%%\n", CpuMeter::getCpuStats().mean);
+
+        auto adcData     = sensors->getADCData();
+        auto batteryData = sensors->getBatteryData();
+        printf("[%.2fs]\tADC:\t%f %f %f\n", adcData.timestamp / 1e6,
+               adcData.temperature, adcData.vBat, batteryData.batVoltage);
+
+        // // WARNING: Fails self test
+        // auto lsm6Data = sensors->getLSM6DSRXData();
+        // printf("[%.2fs]\tLSM6:\t%fm/s^2 %fm/s^2 %fm/s^2\n",
+        //        lsm6Data.accelerationTimestamp / 1e6,
+        //        lsm6Data.accelerationX * 0.001, lsm6Data.accelerationY *
+        //        0.001, lsm6Data.accelerationZ * 0.001);
+        // printf("[%.2fs]\tLSM6:\t%frad/s %frad/s %frad/s\n",
+        //        lsm6Data.angularSpeedTimestamp / 1e6,
+        //        lsm6Data.angularSpeedX * 0.001, lsm6Data.angularSpeedY *
+        //        0.001, lsm6Data.angularSpeedZ * 0.001);
+
+        // // WARNING: The values are wrong
+        // auto h3lisData = sensors->getH3LIS331DLData();
+        // printf("[%.2fs]\tH3LIS:\t%fm/s %fm/s %fm/s\n",
+        //        h3lisData.accelerationTimestamp / 1e6,
+        //        h3lisData.accelerationX, h3lisData.accelerationY,
+        //        h3lisData.accelerationZ);
+
+        // auto lis2Data = sensors->getLIS2MDLData();
+        // printf("[%.2fs]\tLIS2:\t%f %f %f\n",
+        //        lis2Data.magneticFieldTimestamp / 1e6,
+        //        lis2Data.magneticFieldX, lis2Data.magneticFieldY,
+        //        lis2Data.magneticFieldZ);
+
+        // auto lps22Data = sensors->getLPS22DFData();
+        // printf("[%.2fs]\tLPS22:\t%f %f\n", lps22Data.pressureTimestamp / 1e6,
+        //        lps22Data.pressure, lps22Data.temperature);
+
+        // auto maxData = sensors->getMAX31856Data();
+        // printf("[%.2fs]\tMAX:\t%f° %f°\n", maxData.temperatureTimestamp /
+        // 1e6,
+        //        maxData.temperature, maxData.coldJunctionTemperature);
+
+        auto adsData = sensors->getADS131M08Data();
+        printf("[%.2fs]\tADS131:\t%f %f %f %f\n", adsData.timestamp / 1e6,
+               adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage,
+               adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage,
+               adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage,
+               adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage);
+
+        auto chamberData = sensors->getChamberPressureSensorData();
+        printf("[%.2fs]\tCHAMBER:\t%fbar\n",
+               chamberData.pressureTimestamp / 1e6, chamberData.pressure);
+
+        auto tank1Data = sensors->getTankPressureSensor1Data();
+        printf("[%.2fs]\tTANK1:\t\t%fbar\n", tank1Data.pressureTimestamp / 1e6,
+               tank1Data.pressure);
+
+        auto tank2Data = sensors->getTankPressureSensor2Data();
+        printf("[%.2fs]\tTANK2:\t\t%fbar\n", tank2Data.pressureTimestamp / 1e6,
+               tank2Data.pressure);
+
+        auto servoCurrent = sensors->getServoCurrentData();
+        printf("[%.2fs]\tSERVO:\t\t%fA\n", servoCurrent.currentTimestamp / 1e6,
+               servoCurrent.current);
+
+        auto batteryVoltage = sensors->getBatteryData();
+        printf("[%.2fs]\tBATTERY:\t%fV\n",
+               batteryVoltage.voltageTimestamp / 1e6,
+               batteryVoltage.batVoltage);
+
+        auto loggerData = Logger::getInstance().getStats();
+        printf("[%.2fs]\tLOGGER:\t%d\n", loggerData.timestamp / 1e6,
+               loggerData.writesFailed);
+
+        Thread::sleep(1000);
+    }
+}