diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp
index 6ecc9f1afcde14f135d41e7d6d3aa5b55a22cbba..737cb4fb32a29636a40c7f3e0b7f07435d3a297b 100644
--- a/src/boards/RIGv2/Actuators/Actuators.cpp
+++ b/src/boards/RIGv2/Actuators/Actuators.cpp
@@ -212,6 +212,12 @@ bool Actuators::start()
     return true;
 }
 
+void Actuators::armLightOn() { relays::armLight::low(); }
+void Actuators::armLightOff() { relays::armLight::high(); }
+
+void Actuators::igniterOn() { relays::ignition::low(); }
+void Actuators::igniterOff() { relays::ignition::high(); }
+
 bool Actuators::wiggleServo(ServosList servo)
 {
     // Wiggle means open the servo for 1s
diff --git a/src/boards/RIGv2/Actuators/Actuators.h b/src/boards/RIGv2/Actuators/Actuators.h
index 7bc883ab1d555d3ef32472b4ad2f17967fe216fd..42282e2a20f36e7b08b92a4fb0e334b841229900 100644
--- a/src/boards/RIGv2/Actuators/Actuators.h
+++ b/src/boards/RIGv2/Actuators/Actuators.h
@@ -82,6 +82,12 @@ public:
     uint64_t getServoOpeningTime(ServosList servo);
     float getServoMaxAperture(ServosList servo);
 
+    void armLightOn();
+    void armLightOff();
+
+    void igniterOn();
+    void igniterOff();
+
 private:
     ServoInfo *getServo(ServosList servo);
 
diff --git a/src/boards/RIGv2/BoardScheduler.h b/src/boards/RIGv2/BoardScheduler.h
new file mode 100644
index 0000000000000000000000000000000000000000..c36bd00d664d641ce4540c6410223f8b664f5646
--- /dev/null
+++ b/src/boards/RIGv2/BoardScheduler.h
@@ -0,0 +1,51 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: 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
+ * 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 <RIGv2/Configs/SchedulerConfig.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIGv2
+{
+
+class BoardScheduler : public Boardcore::Module
+{
+public:
+    BoardScheduler() : tars1(TARS1_PRIORITY), sensors(SENSORS_PRIORITY) {}
+
+    [[nodiscard]] bool start() { return tars1.start() && sensors.start(); }
+
+    Boardcore::TaskScheduler &getTars1Scheduler() { return tars1; }
+
+    Boardcore::TaskScheduler &getSensorsScheduler() { return sensors; }
+
+    Boardcore::TaskScheduler &getActuatorsScheduler() { return sensors; }
+
+private:
+    Boardcore::TaskScheduler tars1;
+    Boardcore::TaskScheduler sensors;
+};
+
+}  // namespace RIGv2
\ No newline at end of file
diff --git a/src/boards/RIGv2/Configs/SchedulerConfig.h b/src/boards/RIGv2/Configs/SchedulerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..4d07ba5b4fe68388cb7499497ca15ea046f6b71d
--- /dev/null
+++ b/src/boards/RIGv2/Configs/SchedulerConfig.h
@@ -0,0 +1,38 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: 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
+ * 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 <miosix.h>
+
+namespace RIGv2
+{
+
+// Used for TARS1 task scheduler/FSM
+static const miosix::Priority TARS1_PRIORITY = miosix::PRIORITY_MAX - 1;
+// Used for Sensors TaskScheduler
+static const miosix::Priority SENSORS_PRIORITY = miosix::PRIORITY_MAX - 2;
+
+// Used for GMM FSM
+static const miosix::Priority GMM_PRIORITY = miosix::PRIORITY_MAX - 1;
+
+}
\ No newline at end of file
diff --git a/src/boards/RIGv2/Configs/SensorsConfig.h b/src/boards/RIGv2/Configs/SensorsConfig.h
index 03c54eb8abbc26000984cfef221edb4b6c91901a..a9b2f1c65c380bdc9197ae2e12ff3f50ccd45e94 100644
--- a/src/boards/RIGv2/Configs/SensorsConfig.h
+++ b/src/boards/RIGv2/Configs/SensorsConfig.h
@@ -33,41 +33,30 @@ namespace Config
 namespace Sensors
 {
 
-static constexpr uint32_t ADC_SAMPLE_PERIOD = 10;
-static constexpr uint32_t TC_SAMPLE_PERIOD  = 100;
+namespace ADS131M08
+{
 
-static constexpr float ADC1_CH1_SHUNT_RESISTANCE = 29.4048;
-static constexpr float ADC1_CH2_SHUNT_RESISTANCE = 29.5830;
-static constexpr float ADC1_CH3_SHUNT_RESISTANCE = 29.4973;
-static constexpr float ADC1_CH4_SHUNT_RESISTANCE = 29.8849;
+static constexpr float CH1_SHUNT_RESISTANCE = 29.4048;
+static constexpr float CH2_SHUNT_RESISTANCE = 29.5830;
+static constexpr float CH3_SHUNT_RESISTANCE = 29.4973;
+static constexpr float CH4_SHUNT_RESISTANCE = 29.8849;
 
 // ADC channels definitions for various sensors
-static constexpr Boardcore::ADS131M08Defs::Channel ADC1_VESSEL_PT_CHANNEL =
+static constexpr Boardcore::ADS131M08Defs::Channel VESSEL_PT_CHANNEL =
     Boardcore::ADS131M08Defs::Channel::CHANNEL_0;
-static constexpr Boardcore::ADS131M08Defs::Channel ADC1_FILLING_PT_CHANNEL =
+static constexpr Boardcore::ADS131M08Defs::Channel FILLING_PT_CHANNEL =
     Boardcore::ADS131M08Defs::Channel::CHANNEL_1;
-static constexpr Boardcore::ADS131M08Defs::Channel ADC1_BOTTOM_PT_CHANNEL =
+static constexpr Boardcore::ADS131M08Defs::Channel BOTTOM_PT_CHANNEL =
     Boardcore::ADS131M08Defs::Channel::CHANNEL_2;
-static constexpr Boardcore::ADS131M08Defs::Channel ADC1_TOP_PT_CHANNEL =
+static constexpr Boardcore::ADS131M08Defs::Channel TOP_PT_CHANNEL =
     Boardcore::ADS131M08Defs::Channel::CHANNEL_3;
-static constexpr Boardcore::ADS131M08Defs::Channel ADC1_SERVO_CURRENT_CHANNEL =
+static constexpr Boardcore::ADS131M08Defs::Channel SERVO_CURRENT_CHANNEL =
     Boardcore::ADS131M08Defs::Channel::CHANNEL_4;
-static constexpr Boardcore::ADS131M08Defs::Channel ADC1_VESSEL_LC_CHANNEL =
+static constexpr Boardcore::ADS131M08Defs::Channel VESSEL_LC_CHANNEL =
     Boardcore::ADS131M08Defs::Channel::CHANNEL_5;
-static constexpr Boardcore::ADS131M08Defs::Channel ADC1_TANK_LC_CHANNEL =
+static constexpr Boardcore::ADS131M08Defs::Channel TANK_LC_CHANNEL =
     Boardcore::ADS131M08Defs::Channel::CHANNEL_6;
 
-static constexpr float PT_MIN_CURRENT = 4;
-static constexpr float PT_MAX_CURRENT = 20;
-
-static constexpr float FILLING_MAX_PRESSURE     = 100;  // bar
-static constexpr float TANK_TOP_MAX_PRESSURE    = 100;  // bar
-static constexpr float TANK_BOTTOM_MAX_PRESSURE = 100;  // bar
-static constexpr float VESSEL_MAX_PRESSURE      = 400;  // bar
-
-static constexpr unsigned int LC_CALIBRATE_SAMPLE_COUNT  = 10;
-static constexpr unsigned int LC_CALIBRATE_SAMPLE_PERIOD = 40;
-
 // Servo current sensor calibration data
 // - A: 0.0 V: 2.520
 // - A: 0.5 V: 2.513
@@ -76,20 +65,64 @@ static constexpr unsigned int LC_CALIBRATE_SAMPLE_PERIOD = 40;
 // - A: 2.0 V: 2.490
 // - A: 2.5 V: 2.484
 // - A: 5.2 V: 2.441
-static constexpr float SERVO_CURRENT_SCALE   = 4.5466;
-static constexpr float SERVO_CURRENT_ZERO    = 2.520 / SERVO_CURRENT_SCALE;
-static constexpr float BATTERY_VOLTAGE_SCALE = 4.7917;
+static constexpr float SERVO_CURRENT_SCALE = 4.5466;
+static constexpr float SERVO_CURRENT_ZERO  = 2.520 / SERVO_CURRENT_SCALE;
+
+static constexpr uint32_t SAMPLE_PERIOD = 10;
+static constexpr bool ENABLED           = true;
+}  // namespace ADS131M08
+
+namespace MAX31856
+{
+static constexpr uint32_t SAMPLE_PERIOD = 100;
+static constexpr bool ENABLED              = true;
+}  // namespace MAX31856
+
+namespace Trafag
+{
+static constexpr float FILLING_SHUNT_RESISTANCE =
+    ADS131M08::CH2_SHUNT_RESISTANCE;
+static constexpr float TANK_TOP_SHUNT_RESISTANCE =
+    ADS131M08::CH4_SHUNT_RESISTANCE;
+static constexpr float TANK_BOTTOM_SHUNT_RESISTANCE =
+    ADS131M08::CH3_SHUNT_RESISTANCE;
+static constexpr float VESSEL_SHUNT_RESISTANCE =
+    ADS131M08::CH1_SHUNT_RESISTANCE;
+
+static constexpr float MIN_CURRENT = 4;
+static constexpr float MAX_CURRENT = 20;
+
+static constexpr float FILLING_MAX_PRESSURE     = 100;  // bar
+static constexpr float TANK_TOP_MAX_PRESSURE    = 100;  // bar
+static constexpr float TANK_BOTTOM_MAX_PRESSURE = 100;  // bar
+static constexpr float VESSEL_MAX_PRESSURE      = 400;  // bar
+}  // namespace Trafag
+
+namespace LoadCell
+{
+static constexpr unsigned int CALIBRATE_SAMPLE_COUNT  = 10;
+static constexpr unsigned int CALIBRATE_SAMPLE_PERIOD = 40;
 
 // LC Tank sensor calibration data
 // - 1.866kg V: 0.000941
 // - 5.050kg V: 0.002550
 // - 6.916kg V: 0.003559
-static constexpr float LC_TANK_SCALE = 1968.8771f;
+static constexpr float TANK_SCALE = 1968.8771f;
 // LC Vessel sensor calibration data
 // - 1.866kg V: 0.00027
 // - 5.050kg V: 0.00073
 // - 6.916kg V: 0.00100
-static constexpr float LC_VESSEL_SCALE = 6914.9731f;
+static constexpr float VESSEL_SCALE = 6914.9731f;
+}
+
+namespace InternalADC
+{
+static constexpr Boardcore::InternalADC::Channel BATTERY_VOLTAGE_CHANNEL = Boardcore::InternalADC::CH14;
+
+static constexpr float BATTERY_VOLTAGE_SCALE = 4.7917;
+static constexpr uint32_t SAMPLE_PERIOD      = 100;
+static constexpr bool ENABLED                = true;
+}  // namespace InternalADC
 
 }  // namespace Sensors
 
diff --git a/src/boards/RIGv2/Sensors/Sensors.cpp b/src/boards/RIGv2/Sensors/Sensors.cpp
index 2aa8581cf3db92320f3fb6c7eb3aa97ab83f9694..882ea23b7e8b21a0627552c583155d90d9e84664 100644
--- a/src/boards/RIGv2/Sensors/Sensors.cpp
+++ b/src/boards/RIGv2/Sensors/Sensors.cpp
@@ -60,93 +60,66 @@ bool Sensors::start()
 
 InternalADCData Sensors::getInternalADCLastSample()
 {
-    PauseKernelLock l;
-    if (!internalAdc)
-    {
-        return {};
-    }
-    return internalAdc->getLastSample();
+    return internalAdc ? internalAdc->getLastSample() : InternalADCData{};
 }
 
 ADS131M08Data Sensors::getADC1LastSample()
 {
-    PauseKernelLock l;
-    if (!adc1)
-    {
-        return {};
-    }
-    return adc1->getLastSample();
+    return adc1 ? adc1->getLastSample() : ADS131M08Data{};
 }
 
 MAX31856Data Sensors::getTc1LastSample()
 {
-    PauseKernelLock l;
-    if (!tc1)
-    {
-        return {};
-    }
-
-    return tc1->getLastSample();
+    return tc1 ? tc1->getLastSample() : MAX31856Data{};
 }
 
 PressureData Sensors::getVesselPress()
 {
-    PauseKernelLock l;
-    if (!vesselPressure)
-    {
-        return {};
-    }
-    return vesselPressure->getLastSample();
+    return vesselPressure ? vesselPressure->getLastSample() : PressureData{};
 }
 
 PressureData Sensors::getFillingPress()
 {
-    PauseKernelLock l;
-    if (!fillingPressure)
-    {
-        return {};
-    }
-    return fillingPressure->getLastSample();
+    return fillingPressure ? fillingPressure->getLastSample() : PressureData{};
 }
 
 PressureData Sensors::getTopTankPress()
 {
-    PauseKernelLock l;
-    if (!topTankPressure)
-    {
-        return {};
-    }
-    return topTankPressure->getLastSample();
+    return topTankPressure ? topTankPressure->getLastSample() : PressureData{};
 }
 
 PressureData Sensors::getBottomTankPress()
 {
-    PauseKernelLock l;
-    if (!bottomTankPressure)
-    {
-        return {};
-    }
-    return bottomTankPressure->getLastSample();
+    return bottomTankPressure ? bottomTankPressure->getLastSample()
+                              : PressureData{};
 }
 
 LoadCellData Sensors::getVesselWeight()
 {
-    PauseKernelLock l;
-    if (!vesselWeight)
+    if (vesselWeight)
+    {
+        auto sample = vesselWeight->getLastSample();
+        sample.load -= vesselLcOffset;
+        return sample;
+    }
+    else
     {
         return {};
     }
-    return vesselWeight->getLastSample();
 }
 
 LoadCellData Sensors::getTankWeight()
 {
-    PauseKernelLock l;
-    if (!tankWeight)
+    if (tankWeight)
+    {
+        auto sample = tankWeight->getLastSample();
+        sample.load -= tankLcOffset;
+        return sample;
+    }
+    else
     {
         return {};
     }
-    return tankWeight->getLastSample();
 }
 
 CurrentData Sensors::getUmbilicalCurrent()
@@ -159,9 +132,10 @@ CurrentData Sensors::getServoCurrent()
     auto sample = getADC1LastSample();
 
     float current =
-        (sample.voltage[(int)Config::Sensors::ADC1_SERVO_CURRENT_CHANNEL] -
-         Config::Sensors::SERVO_CURRENT_ZERO) *
-        Config::Sensors::SERVO_CURRENT_SCALE;
+        (sample
+             .voltage[(int)Config::Sensors::ADS131M08::SERVO_CURRENT_CHANNEL] -
+         Config::Sensors::ADS131M08::SERVO_CURRENT_ZERO) *
+        Config::Sensors::ADS131M08::SERVO_CURRENT_SCALE;
     // Current reading are flipped
     return {sample.timestamp, -current / 5.0f * 50.0f};
 }
@@ -170,7 +144,10 @@ VoltageData Sensors::getBatteryVoltage()
 {
     auto sample = getInternalADCLastSample();
 
-    float voltage = sample.voltage[14] * Config::Sensors::BATTERY_VOLTAGE_SCALE;
+    float voltage =
+        sample.voltage[(
+            int)Config::Sensors::InternalADC::BATTERY_VOLTAGE_CHANNEL] *
+        Config::Sensors::InternalADC::BATTERY_VOLTAGE_SCALE;
     return {sample.timestamp, voltage};
 }
 
@@ -178,17 +155,14 @@ void Sensors::calibrate()
 {
     Stats vesselStats, tankStats;
 
-    for (unsigned int i = 0; i < Config::Sensors::LC_CALIBRATE_SAMPLE_COUNT;
-         i++)
+    for (unsigned int i = 0;
+         i < Config::Sensors::LoadCell::CALIBRATE_SAMPLE_COUNT; i++)
     {
-        auto sample = getADC1LastSample();
-
-        vesselStats.add(
-            sample.voltage[(int)Config::Sensors::ADC1_VESSEL_LC_CHANNEL]);
-        tankStats.add(
-            sample.voltage[(int)Config::Sensors::ADC1_TANK_LC_CHANNEL]);
+        // Tank readings WITHOUT offsets
+        vesselStats.add(vesselWeight->getLastSample().load);
+        tankStats.add(tankWeight->getLastSample().load);
 
-        Thread::sleep(Config::Sensors::LC_CALIBRATE_SAMPLE_PERIOD);
+        Thread::sleep(Config::Sensors::LoadCell::CALIBRATE_SAMPLE_PERIOD);
     }
 
     vesselLcOffset = vesselStats.getStats().mean;
@@ -197,17 +171,24 @@ void Sensors::calibrate()
 
 std::vector<SensorInfo> Sensors::getSensorInfos()
 {
-    return {
-        manager->getSensorInfo(vesselWeight.get()),
-        manager->getSensorInfo(tankWeight.get()),
-        manager->getSensorInfo(vesselPressure.get()),
-        manager->getSensorInfo(fillingPressure.get()),
-        manager->getSensorInfo(topTankPressure.get()),
-        manager->getSensorInfo(bottomTankPressure.get()),
-        manager->getSensorInfo(internalAdc.get()),
-        manager->getSensorInfo(adc1.get()),
-        manager->getSensorInfo(tc1.get()),
-    };
+    if (manager)
+    {
+        return {
+            manager->getSensorInfo(vesselWeight.get()),
+            manager->getSensorInfo(tankWeight.get()),
+            manager->getSensorInfo(vesselPressure.get()),
+            manager->getSensorInfo(fillingPressure.get()),
+            manager->getSensorInfo(topTankPressure.get()),
+            manager->getSensorInfo(bottomTankPressure.get()),
+            manager->getSensorInfo(internalAdc.get()),
+            manager->getSensorInfo(adc1.get()),
+            manager->getSensorInfo(tc1.get()),
+        };
+    }
+    else
+    {
+        return {};
+    }
 }
 
 void Sensors::internalAdcInit(Boardcore::SensorManager::SensorMap_t &map)
@@ -216,12 +197,15 @@ void Sensors::internalAdcInit(Boardcore::SensorManager::SensorMap_t &map)
 
     internalAdc->enableChannel(InternalADC::CH9);
     internalAdc->enableChannel(InternalADC::CH11);
-    internalAdc->enableChannel(InternalADC::CH14);
+    internalAdc->enableChannel(
+        Config::Sensors::InternalADC::BATTERY_VOLTAGE_CHANNEL);
     internalAdc->enableTemperature();
     internalAdc->enableVbat();
 
-    SensorInfo info("InternalAdc", Config::Sensors::ADC_SAMPLE_PERIOD,
-                    [this]() { internalAdcCallback(); });
+    SensorInfo info(
+        "InternalAdc", Config::Sensors::InternalADC::SAMPLE_PERIOD,
+        [this]() { internalAdcCallback(); },
+        Config::Sensors::InternalADC::ENABLED);
     map.emplace(internalAdc.get(), info);
 }
 
@@ -255,43 +239,44 @@ void Sensors::adc1Init(SensorManager::SensorMap_t &map)
     config.channelsConfig[7].enabled = false;
 
     // Configure all required channels
-    config.channelsConfig[(int)Config::Sensors::ADC1_VESSEL_PT_CHANNEL] = {
-        .enabled = true,
-        .pga     = ADS131M08Defs::PGA::PGA_1,
-        .offset  = 0,
-        .gain    = 1.0};
-
-    config.channelsConfig[(int)Config::Sensors::ADC1_FILLING_PT_CHANNEL] = {
-        .enabled = true,
-        .pga     = ADS131M08Defs::PGA::PGA_1,
-        .offset  = 0,
-        .gain    = 1.0};
-
-    config.channelsConfig[(int)Config::Sensors::ADC1_BOTTOM_PT_CHANNEL] = {
+    config.channelsConfig[(int)Config::Sensors::ADS131M08::VESSEL_PT_CHANNEL] =
+        {.enabled = true,
+         .pga     = ADS131M08Defs::PGA::PGA_1,
+         .offset  = 0,
+         .gain    = 1.0};
+
+    config.channelsConfig[(int)Config::Sensors::ADS131M08::FILLING_PT_CHANNEL] =
+        {.enabled = true,
+         .pga     = ADS131M08Defs::PGA::PGA_1,
+         .offset  = 0,
+         .gain    = 1.0};
+
+    config.channelsConfig[(int)Config::Sensors::ADS131M08::BOTTOM_PT_CHANNEL] =
+        {.enabled = true,
+         .pga     = ADS131M08Defs::PGA::PGA_1,
+         .offset  = 0,
+         .gain    = 1.0};
+
+    config.channelsConfig[(int)Config::Sensors::ADS131M08::TOP_PT_CHANNEL] = {
         .enabled = true,
         .pga     = ADS131M08Defs::PGA::PGA_1,
         .offset  = 0,
         .gain    = 1.0};
 
-    config.channelsConfig[(int)Config::Sensors::ADC1_TOP_PT_CHANNEL] = {
+    config.channelsConfig[(
+        int)Config::Sensors::ADS131M08::SERVO_CURRENT_CHANNEL] = {
         .enabled = true,
         .pga     = ADS131M08Defs::PGA::PGA_1,
         .offset  = 0,
         .gain    = 1.0};
 
-    config.channelsConfig[(int)Config::Sensors::ADC1_SERVO_CURRENT_CHANNEL] = {
-        .enabled = true,
-        .pga     = ADS131M08Defs::PGA::PGA_1,
-        .offset  = 0,
-        .gain    = 1.0};
+    config.channelsConfig[(int)Config::Sensors::ADS131M08::VESSEL_LC_CHANNEL] =
+        {.enabled = true,
+         .pga     = ADS131M08Defs::PGA::PGA_32,
+         .offset  = 0,
+         .gain    = 1.0};
 
-    config.channelsConfig[(int)Config::Sensors::ADC1_VESSEL_LC_CHANNEL] = {
-        .enabled = true,
-        .pga     = ADS131M08Defs::PGA::PGA_32,
-        .offset  = 0,
-        .gain    = 1.0};
-
-    config.channelsConfig[(int)Config::Sensors::ADC1_TANK_LC_CHANNEL] = {
+    config.channelsConfig[(int)Config::Sensors::ADS131M08::TANK_LC_CHANNEL] = {
         .enabled = true,
         .pga     = ADS131M08Defs::PGA::PGA_32,
         .offset  = 0,
@@ -301,7 +286,9 @@ void Sensors::adc1Init(SensorManager::SensorMap_t &map)
                                        sensors::ADS131_1::cs::getPin(),
                                        spiConfig, config);
 
-    SensorInfo info("ADS131M08_1", 500, [this]() { adc1Callback(); });
+    SensorInfo info(
+        "ADS131M08_1", Config::Sensors::ADS131M08::SAMPLE_PERIOD,
+        [this]() { adc1Callback(); }, Config::Sensors::ADS131M08::ENABLED);
     map.emplace(std::make_pair(adc1.get(), info));
 }
 
@@ -316,10 +303,12 @@ void Sensors::adc1Callback()
                   sample.voltage[6], sample.voltage[7]};
 
     // LOG_INFO(logger, "{:.4}\t{:.4}\t{:.4}\t{:.4}",
-    //          (sample.voltage[0] / Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE) * 1000.0f,
-    //          (sample.voltage[1] / Config::Sensors::ADC1_CH2_SHUNT_RESISTANCE) * 1000.0f,
-    //          (sample.voltage[2] / Config::Sensors::ADC1_CH3_SHUNT_RESISTANCE) * 1000.0f,
-    //          (sample.voltage[3] / Config::Sensors::ADC1_CH4_SHUNT_RESISTANCE) * 1000.0f);
+    //          (sample.voltage[0] / Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE)
+    //          * 1000.0f, (sample.voltage[1] /
+    //          Config::Sensors::ADC1_CH2_SHUNT_RESISTANCE) * 1000.0f,
+    //          (sample.voltage[2] / Config::Sensors::ADC1_CH3_SHUNT_RESISTANCE)
+    //          * 1000.0f, (sample.voltage[3] /
+    //          Config::Sensors::ADC1_CH4_SHUNT_RESISTANCE) * 1000.0f);
 
     sdLogger.log(data);
 }
@@ -336,8 +325,9 @@ void Sensors::tc1Init(SensorManager::SensorMap_t &map)
                                    sensors::MAX31856_1::cs::getPin(), spiConfig,
                                    MAX31856::ThermocoupleType::K_TYPE);
 
-    SensorInfo info("MAX31856_1", Config::Sensors::TC_SAMPLE_PERIOD,
-                    [this]() { tc1Callback(); });
+    SensorInfo info(
+        "MAX31856_1", Config::Sensors::MAX31856::SAMPLE_PERIOD,
+        [this]() { tc1Callback(); }, Config::Sensors::MAX31856::ENABLED);
     map.emplace(std::make_pair(tc1.get(), info));
 }
 
@@ -356,13 +346,15 @@ void Sensors::vesselPressureInit(Boardcore::SensorManager::SensorMap_t &map)
         [this]()
         {
             auto sample = adc1->getLastSample();
-            return sample.getVoltage(Config::Sensors::ADC1_VESSEL_PT_CHANNEL);
+            return sample.getVoltage(
+                Config::Sensors::ADS131M08::VESSEL_PT_CHANNEL);
         },
-        Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE,
-        Config::Sensors::VESSEL_MAX_PRESSURE, Config::Sensors::PT_MIN_CURRENT,
-        Config::Sensors::PT_MAX_CURRENT);
+        Config::Sensors::Trafag::VESSEL_SHUNT_RESISTANCE,
+        Config::Sensors::Trafag::VESSEL_MAX_PRESSURE,
+        Config::Sensors::Trafag::MIN_CURRENT,
+        Config::Sensors::Trafag::MAX_CURRENT);
 
-    SensorInfo info("VesselPressure", Config::Sensors::ADC_SAMPLE_PERIOD,
+    SensorInfo info("VesselPressure", Config::Sensors::ADS131M08::SAMPLE_PERIOD,
                     [this]() { vesselPressureCallback(); });
     map.emplace(std::make_pair(vesselPressure.get(), info));
 }
@@ -380,13 +372,16 @@ void Sensors::fillingPressureInit(Boardcore::SensorManager::SensorMap_t &map)
         [this]()
         {
             auto sample = adc1->getLastSample();
-            return sample.getVoltage(Config::Sensors::ADC1_FILLING_PT_CHANNEL);
+            return sample.getVoltage(
+                Config::Sensors::ADS131M08::FILLING_PT_CHANNEL);
         },
-        Config::Sensors::ADC1_CH2_SHUNT_RESISTANCE,
-        Config::Sensors::FILLING_MAX_PRESSURE, Config::Sensors::PT_MIN_CURRENT,
-        Config::Sensors::PT_MAX_CURRENT);
+        Config::Sensors::Trafag::FILLING_SHUNT_RESISTANCE,
+        Config::Sensors::Trafag::FILLING_MAX_PRESSURE,
+        Config::Sensors::Trafag::MIN_CURRENT,
+        Config::Sensors::Trafag::MAX_CURRENT);
 
-    SensorInfo info("FillingPressure", Config::Sensors::ADC_SAMPLE_PERIOD,
+    SensorInfo info("FillingPressure",
+                    Config::Sensors::ADS131M08::SAMPLE_PERIOD,
                     [this]() { fillingPressureCallback(); });
     map.emplace(std::make_pair(fillingPressure.get(), info));
 }
@@ -404,13 +399,16 @@ void Sensors::topTankPressureInit(Boardcore::SensorManager::SensorMap_t &map)
         [this]()
         {
             auto sample = adc1->getLastSample();
-            return sample.getVoltage(Config::Sensors::ADC1_TOP_PT_CHANNEL);
+            return sample.getVoltage(
+                Config::Sensors::ADS131M08::TOP_PT_CHANNEL);
         },
-        Config::Sensors::ADC1_CH3_SHUNT_RESISTANCE,
-        Config::Sensors::TANK_TOP_MAX_PRESSURE, Config::Sensors::PT_MIN_CURRENT,
-        Config::Sensors::PT_MAX_CURRENT);
+        Config::Sensors::Trafag::TANK_TOP_SHUNT_RESISTANCE,
+        Config::Sensors::Trafag::TANK_TOP_MAX_PRESSURE,
+        Config::Sensors::Trafag::MIN_CURRENT,
+        Config::Sensors::Trafag::MAX_CURRENT);
 
-    SensorInfo info("TopTankPressure", Config::Sensors::ADC_SAMPLE_PERIOD,
+    SensorInfo info("TopTankPressure",
+                    Config::Sensors::ADS131M08::SAMPLE_PERIOD,
                     [this]() { topTankPressureCallback(); });
     map.emplace(std::make_pair(topTankPressure.get(), info));
 }
@@ -428,13 +426,16 @@ void Sensors::bottomTankPressureInit(Boardcore::SensorManager::SensorMap_t &map)
         [this]()
         {
             auto sample = adc1->getLastSample();
-            return sample.getVoltage(Config::Sensors::ADC1_BOTTOM_PT_CHANNEL);
+            return sample.getVoltage(
+                Config::Sensors::ADS131M08::BOTTOM_PT_CHANNEL);
         },
-        Config::Sensors::ADC1_CH4_SHUNT_RESISTANCE,
-        Config::Sensors::TANK_BOTTOM_MAX_PRESSURE,
-        Config::Sensors::PT_MIN_CURRENT, Config::Sensors::PT_MAX_CURRENT);
+        Config::Sensors::Trafag::TANK_BOTTOM_SHUNT_RESISTANCE,
+        Config::Sensors::Trafag::TANK_BOTTOM_MAX_PRESSURE,
+        Config::Sensors::Trafag::MIN_CURRENT,
+        Config::Sensors::Trafag::MAX_CURRENT);
 
-    SensorInfo info("BottomTankPressure", Config::Sensors::ADC_SAMPLE_PERIOD,
+    SensorInfo info("BottomTankPressure",
+                    Config::Sensors::ADS131M08::SAMPLE_PERIOD,
                     [this]() { bottomTankPressureCallback(); });
     map.emplace(std::make_pair(bottomTankPressure.get(), info));
 }
@@ -452,22 +453,20 @@ void Sensors::vesselWeightInit(Boardcore::SensorManager::SensorMap_t &map)
         [this]()
         {
             auto sample = adc1->getLastSample();
-            auto voltage =
-                sample.getVoltage(Config::Sensors::ADC1_VESSEL_LC_CHANNEL);
-            voltage.voltage -= vesselLcOffset;
-
-            return voltage;
+            return sample.getVoltage(
+                Config::Sensors::ADS131M08::VESSEL_LC_CHANNEL);
         },
-        Config::Sensors::LC_VESSEL_SCALE);
+        Config::Sensors::LoadCell::VESSEL_SCALE);
 
-    SensorInfo info("VesselWeight", Config::Sensors::ADC_SAMPLE_PERIOD,
+    SensorInfo info("VesselWeight", Config::Sensors::ADS131M08::SAMPLE_PERIOD,
                     [this]() { vesselWeightCallback(); });
     map.emplace(std::make_pair(vesselWeight.get(), info));
 }
 
 void Sensors::vesselWeightCallback()
 {
-    LoadCellData sample = vesselWeight->getLastSample();
+    // Log CALIBRATED value
+    LoadCellData sample = getVesselWeight();
     LCsData data{sample.loadTimestamp, 1, sample.load};
     sdLogger.log(data);
 }
@@ -478,22 +477,20 @@ void Sensors::tankWeightInit(Boardcore::SensorManager::SensorMap_t &map)
         [this]()
         {
             auto sample = adc1->getLastSample();
-            auto voltage =
-                sample.getVoltage(Config::Sensors::ADC1_TANK_LC_CHANNEL);
-            voltage.voltage -= tankLcOffset;
-
-            return voltage;
+            return sample.getVoltage(
+                Config::Sensors::ADS131M08::TANK_LC_CHANNEL);
         },
-        Config::Sensors::LC_TANK_SCALE);
+        Config::Sensors::LoadCell::TANK_SCALE);
 
-    SensorInfo info("TankWeight", Config::Sensors::ADC_SAMPLE_PERIOD,
+    SensorInfo info("TankWeight", Config::Sensors::ADS131M08::SAMPLE_PERIOD,
                     [this]() { tankWeightCallback(); });
     map.emplace(std::make_pair(tankWeight.get(), info));
 }
 
 void Sensors::tankWeightCallback()
 {
-    LoadCellData sample = tankWeight->getLastSample();
+    // Log CALIBRATED value
+    LoadCellData sample = getTankWeight();
     LCsData data{sample.loadTimestamp, 2, sample.load};
     sdLogger.log(data);
 }
\ No newline at end of file
diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
index c876bb0d56ce2540f94e5b43b8f6f20c5ef73b13..a7a5d751b47708b70c487fd66940bfbd04c49e93 100644
--- a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
+++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
@@ -23,6 +23,7 @@
 #include "GroundModeManager.h"
 
 #include <RIGv2/Actuators/Actuators.h>
+#include <RIGv2/Configs/SchedulerConfig.h>
 #include <RIGv2/Sensors/Sensors.h>
 #include <common/Events.h>
 #include <events/EventBroker.h>
@@ -35,15 +36,9 @@ using namespace miosix;
 using namespace Common;
 using namespace RIGv2;
 
-void armLightOn() { relays::armLight::low(); }
-
-void armLightOff() { relays::armLight::high(); }
-
-void igniterOn() { relays::ignition::low(); }
-
-void igniterOff() { relays::ignition::high(); }
-
-GroundModeManager::GroundModeManager() : FSM(&GroundModeManager::state_idle)
+GroundModeManager::GroundModeManager()
+    : FSM(&GroundModeManager::state_idle, miosix::STACK_DEFAULT_FOR_PTHREAD,
+          GMM_PRIORITY)
 {
     EventBroker::getInstance().subscribe(this, TOPIC_MOTOR);
 }
@@ -124,7 +119,7 @@ void GroundModeManager::state_disarmed(const Boardcore::Event &event)
     {
         case EV_ENTRY:
         {
-            armLightOff();
+            modules.get<Actuators>()->armLightOff();
             logStatus(GMM_STATE_DISARMED);
             break;
         }
@@ -158,7 +153,7 @@ void GroundModeManager::state_armed(const Boardcore::Event &event)
     {
         case EV_ENTRY:
         {
-            armLightOn();
+            modules.get<Actuators>()->armLightOn();
             modules.get<Actuators>()->closeAllServos();
             logStatus(GMM_STATE_ARMED);
             break;
@@ -186,7 +181,7 @@ void GroundModeManager::state_igniting(const Boardcore::Event &event)
         case EV_ENTRY:
         {
             // Start ignition
-            igniterOn();
+            modules.get<Actuators>()->igniterOn();
 
             // Send event to open the oxidant
             openOxidantDelayEventId = EventBroker::getInstance().postDelayed(
@@ -199,7 +194,7 @@ void GroundModeManager::state_igniting(const Boardcore::Event &event)
         case MOTOR_OPEN_OXIDANT:
         {
             // Stop ignition
-            igniterOff();
+            modules.get<Actuators>()->igniterOff();
             modules.get<Actuators>()->openServo(ServosList::MAIN_VALVE);
             break;
         }
@@ -208,7 +203,7 @@ void GroundModeManager::state_igniting(const Boardcore::Event &event)
         case MOTOR_CLOSE_FEED_VALVE:
         {
             // Stop ignition
-            igniterOff();
+            modules.get<Actuators>()->igniterOff();
 
             // Close all of the valves
             modules.get<Actuators>()->closeAllServos();
diff --git a/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp b/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp
index 5ef495e690e49e832849fd6c6fbcd3357f4ee5da..5a36bc527135392b19eb4541a38e61b4169dfdb6 100644
--- a/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp
+++ b/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp
@@ -23,6 +23,7 @@
 #include "TARS1.h"
 
 #include <RIGv2/Actuators/Actuators.h>
+#include <RIGv2/Configs/SchedulerConfig.h>
 #include <RIGv2/Sensors/Sensors.h>
 #include <common/Events.h>
 #include <events/EventBroker.h>
@@ -35,7 +36,9 @@ using namespace Common;
 using namespace miosix;
 
 TARS1::TARS1(TaskScheduler& scheduler)
-    : FSM(&TARS1::state_ready), scheduler{scheduler}
+    : FSM(&TARS1::state_ready, miosix::STACK_DEFAULT_FOR_PTHREAD,
+          TARS1_PRIORITY),
+      scheduler{scheduler}
 {
     EventBroker::getInstance().subscribe(this, TOPIC_TARS);
     EventBroker::getInstance().subscribe(this, TOPIC_MOTOR);
diff --git a/src/entrypoints/RIGv2/rig-v2-entry.cpp b/src/entrypoints/RIGv2/rig-v2-entry.cpp
index 52d77721deefe641f288eb1dc4b4f844cdaa97d6..342d7853ab18c7711739f00f8fae3d70f3cc9c78 100644
--- a/src/entrypoints/RIGv2/rig-v2-entry.cpp
+++ b/src/entrypoints/RIGv2/rig-v2-entry.cpp
@@ -21,6 +21,7 @@
  */
 
 #include <RIGv2/Actuators/Actuators.h>
+#include <RIGv2/BoardScheduler.h>
 #include <RIGv2/Buses.h>
 #include <RIGv2/Radio/Radio.h>
 #include <RIGv2/Sensors/Sensors.h>
@@ -45,15 +46,13 @@ int main()
     PrintLogger logger     = Logging::getLogger("main");
     ModuleManager &modules = ModuleManager::getInstance();
 
-    // TODO: Move this to a dedicated board scheduler
-    TaskScheduler *scheduler1 = new TaskScheduler(2);
-    TaskScheduler *scheduler2 = new TaskScheduler(3);
+    Buses *buses              = new Buses();
+    BoardScheduler *scheduler = new BoardScheduler();
 
-    Buses *buses           = new Buses();
-    Sensors *sensors       = new Sensors(*scheduler1);
-    Actuators *actuators   = new Actuators(*scheduler2);
+    Sensors *sensors       = new Sensors(scheduler->getSensorsScheduler());
+    Actuators *actuators   = new Actuators(scheduler->getActuatorsScheduler());
     GroundModeManager *gmm = new GroundModeManager();
-    TARS1 *tars1           = new TARS1(*scheduler2);
+    TARS1 *tars1           = new TARS1(scheduler->getTars1Scheduler());
     Radio *radio           = new Radio();
 
     Logger &sdLogger    = Logger::getInstance();
@@ -68,44 +67,13 @@ int main()
             sdLogger.log(data);
         });
 
-    bool initResult = true;
-
     // Insert modules
-    if (!modules.insert<Buses>(buses))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error failed to insert Buses");
-    }
-
-    if (!modules.insert<Actuators>(actuators))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error failed to insert Actuators");
-    }
-
-    if (!modules.insert<Sensors>(sensors))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error failed to insert Sensors");
-    }
-
-    if (!modules.insert<Radio>(radio))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error failed to insert Radio");
-    }
-
-    if (!modules.insert<GroundModeManager>(gmm))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error failed to insert GroundModeManager");
-    }
-
-    if (!modules.insert<TARS1>(tars1))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error failed to insert TARS1");
-    }
+    bool initResult =
+        modules.insert<Buses>(buses) &&
+        modules.insert<BoardScheduler>(scheduler) &&
+        modules.insert<Actuators>(actuators) &&
+        modules.insert<Sensors>(sensors) && modules.insert<Radio>(radio) &&
+        modules.insert<GroundModeManager>(gmm) && modules.insert<TARS1>(tars1);
 
     // Start modules
     if (!sdLogger.testSDCard())
@@ -150,21 +118,21 @@ int main()
         LOG_ERR(logger, "Error failed to start TARS1 module");
     }
 
-    if (!scheduler1->start() || !scheduler2->start())
+    if (!scheduler->start())
     {
         initResult = false;
         LOG_ERR(logger, "Error failed to start scheduler");
     }
 
-    if (initResult)
+    if (!initResult)
     {
-        broker.post(FMM_INIT_OK, TOPIC_MOTOR);
-        LOG_INFO(logger, "All good!");
+        broker.post(FMM_INIT_ERROR, TOPIC_MOTOR);
+        LOG_ERR(logger, "Init failure!");
     }
     else
     {
-        broker.post(FMM_INIT_ERROR, TOPIC_MOTOR);
-        LOG_ERR(logger, "Init failure!");
+        broker.post(FMM_INIT_OK, TOPIC_MOTOR);
+        LOG_INFO(logger, "All good!");
     }
 
     // Periodic statistics