preliminary tcmux test

hytech-racing · Feb 8, 2024 · 39ee0e8 · 39ee0e8
1 parent 0c049df
commit 39ee0e8
Show file tree

Hide file tree

Showing 8 changed files with 94 additions and 31 deletions.
diff --git a/lib/systems/include/SysClock.h b/lib/systems/include/SysClock.h
@@ -22,7 +22,7 @@ struct SysTick_s
 class SysClock
 {
 private:
-    enum TriggerIndices_s
+    enum TriggerIndices_e
     {
         TRIG_1000 = 0,
         TRIG_500 = 1,

diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h
@@ -12,7 +12,7 @@
 const float maxSpeedForModeChange = 5.0; // m/s
 const float maxTorqueDeltaForModeChange = 0.5; // Nm
 
-enum TorqueLimit_e
+enum class TorqueLimit_e
 {
     TCMUX_LOW_TORQUE = 0,
     TCMUX_MID_TORQUE = 1,
@@ -33,9 +33,9 @@ class TorqueControllerMux
         {DialMode_e::ENDURANCE, TC_NO_CONTROLLER},
     };
     std::unordered_map<TorqueLimit_e, float> torqueLimitMap_ = {
-        {TCMUX_LOW_TORQUE, 10.0},
-        {TCMUX_MID_TORQUE, 15.0},
-        {TCMUX_FULL_TORQUE, 20.0}
+        {TorqueLimit_e::TCMUX_LOW_TORQUE, 10.0},
+        {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0},
+        {TorqueLimit_e::TCMUX_FULL_TORQUE, 20.0}
     };
     DrivetrainCommand_s controllerCommands_[static_cast<int>(TC_NUM_CONTROLLERS)];
     TorqueControllerNone torqueControllerNone_;
@@ -52,7 +52,7 @@ class TorqueControllerMux
     , torqueControllerSimple_(controllerCommands_[static_cast<int>(TC_SAFE_MODE)])
     {
         muxMode_ = TC_NO_CONTROLLER;
-        torqueLimit_ = TCMUX_LOW_TORQUE;
+        torqueLimit_ = TorqueLimit_e::TCMUX_LOW_TORQUE;
     }
 // Functions
     void tick(
@@ -71,6 +71,10 @@ class TorqueControllerMux
     {
         return drivetrainCommand_;
     };
+    const TorqueLimit_e& getTorqueLimit()
+    {
+        return torqueLimit_;
+    };
 };
 
 #endif /* __TORQUECTRLMUX_H__ */
diff --git a/lib/systems/src/SysClock.cpp b/lib/systems/src/SysClock.cpp
@@ -3,7 +3,10 @@
 // Constructor
 SysClock::SysClock()
 {
-
+    for (int i = 0; i < static_cast<int>(TriggerIndices_e::NUM_TRIGGERS); i++)
+    {
+        triggerTimes[i] = 0;
+    }
 }
 
 SysTick_s SysClock::tick(unsigned long sysMicros)
@@ -12,29 +15,31 @@ SysTick_s SysClock::tick(unsigned long sysMicros)
         .millis = sysMicros / 1000,
         .micros = sysMicros,
         .triggers = {
-            .trigger1000    = sysMicros > triggerTimes[TRIG_1000],
-            .trigger500     = sysMicros > triggerTimes[TRIG_500],
-            .trigger100     = sysMicros > triggerTimes[TRIG_100],
-            .trigger50      = sysMicros > triggerTimes[TRIG_50],
-            .trigger10      = sysMicros > triggerTimes[TRIG_10],
-            .trigger5       = sysMicros > triggerTimes[TRIG_5],
-            .trigger1       = sysMicros > triggerTimes[TRIG_1]
+            .trigger1000    = sysMicros > triggerTimes[static_cast<int>(TriggerIndices_e::TRIG_1000)],
+            .trigger500     = sysMicros > triggerTimes[static_cast<int>(TriggerIndices_e::TRIG_500)],
+            .trigger100     = sysMicros > triggerTimes[static_cast<int>(TriggerIndices_e::TRIG_100)],
+            .trigger50      = sysMicros > triggerTimes[static_cast<int>(TriggerIndices_e::TRIG_50)],
+            .trigger10      = sysMicros > triggerTimes[static_cast<int>(TriggerIndices_e::TRIG_10)],
+            .trigger5       = sysMicros > triggerTimes[static_cast<int>(TriggerIndices_e::TRIG_5)],
+            .trigger1       = sysMicros > triggerTimes[static_cast<int>(TriggerIndices_e::TRIG_1)]
         }
     };
 
     // Recalculate trigger times
     if (tickReturn.triggers.trigger1000)
-        triggerTimes[TRIG_1000] += 1'000;
+        triggerTimes[TriggerIndices_e::TRIG_1000] += 1'000;
     if (tickReturn.triggers.trigger500)
-        triggerTimes[TRIG_500] += 2'000;
+        triggerTimes[TriggerIndices_e::TRIG_500] += 2'000;
     if (tickReturn.triggers.trigger100)
-        triggerTimes[TRIG_100] += 10'000;
+        triggerTimes[TriggerIndices_e::TRIG_100] += 10'000;
     if (tickReturn.triggers.trigger50)
-        triggerTimes[TRIG_50] += 20'000;
+        triggerTimes[TriggerIndices_e::TRIG_50] += 20'000;
     if (tickReturn.triggers.trigger10)
-        triggerTimes[TRIG_10] += 100'000;
+        triggerTimes[TriggerIndices_e::TRIG_10] += 100'000;
     if (tickReturn.triggers.trigger5)
-        triggerTimes[TRIG_5] += 200'000;
+        triggerTimes[TriggerIndices_e::TRIG_5] += 200'000;
     if (tickReturn.triggers.trigger1)
-        triggerTimes[TRIG_1] += 1'000'000;
+        triggerTimes[TriggerIndices_e::TRIG_1] += 1'000'000;
+
+    return tickReturn;
 }
diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp
@@ -15,20 +15,20 @@ void TorqueControllerMux::tick(
     )
 {
     // Tick all torque controllers
-    torqueControllerSimple_.tick(tick, pedalsData, torqueLimit_);
+    torqueControllerSimple_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_]);
 
     // Tick torque button logic at 50hz
     if (tick.triggers.trigger50)
     {
         // detect high-to-low transition and lock out button presses for DEBOUNCE_MILLIS ms
         if (
             torqueLimitButtonPressed_ 
-            && dashboardTorqueModeButtonPressed
+            && !dashboardTorqueModeButtonPressed
             && tick.millis - torqueLimitButtonPressedTime_ > DEBOUNCE_MILLIS
         )
         {
             // WOW C++ is ass
-            torqueLimit_ = static_cast<TorqueLimit_e>((static_cast<int>(torqueLimit_) + 1) % static_cast<int>(TCMUX_NUM_TORQUE_LIMITS));
+            torqueLimit_ = static_cast<TorqueLimit_e>((static_cast<int>(torqueLimit_) + 1) % (static_cast<int>(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS)));
             torqueLimitButtonPressedTime_ = tick.millis;
         }
         torqueLimitButtonPressed_ = dashboardTorqueModeButtonPressed;

diff --git a/platformio.ini b/platformio.ini
@@ -5,7 +5,7 @@ build_src_filter =
   -<**/*.c>
   -<**/*.cpp>
 build_unflags = -std=gnu++11 
-build_flags = -std=c++17
+build_flags = -std=c++17 -g
 lib_ignore =
   interfaces-lib
 

diff --git a/test/controller_mux_test.h b/test/controller_mux_test.h
diff --git a/test/main.cpp b/test/main.cpp
@@ -2,7 +2,7 @@
 
 #include "state_machine_test.h"
 #include "pedals_system_test.h"
-
+#include "torque_controller_mux_test.h"
 
 
 int main(int argc, char **argv)

diff --git a/test/torque_controller_mux_test.h b/test/torque_controller_mux_test.h
@@ -0,0 +1,59 @@
+#ifndef TORQUE_CONTROLLER_MUX_TEST
+#define TORQUE_CONTROLLER_MUX_TEST
+
+#include <gtest/gtest.h>
+#include <gmock/gmock.h>
+#include <SysClock.h>
+#include <TorqueControllerMux.h>
+#include <PedalsSystem.h>
+#include <SteeringSystem.h>
+#include "AnalogSensorsInterface.h"
+#include "DashboardInterface.h"
+
+TEST(TorqueControllerMuxTesting, test_torque_button)
+{
+    const int SERIES_LENGTH = 10;
+    SysClock clock = SysClock();
+    SysTick_s cur_tick;
+    TorqueControllerMux torque_controller_mux = TorqueControllerMux();
+
+    bool buttonTimeSeries[SERIES_LENGTH]; // 50hz time series
+    // Cycle back to low test
+    buttonTimeSeries[1] = true;
+    buttonTimeSeries[3] = true;
+    buttonTimeSeries[5] = true;
+    // Cycle only on button release test
+    for (int i = 7; i < SERIES_LENGTH; i++)
+        buttonTimeSeries[i] = true;
+
+    ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_LOW_TORQUE);
+
+    // Run test
+    for (int i = 0; i < SERIES_LENGTH; i++)
+    {
+        cur_tick = clock.tick(i * 1000 * 20); // 20 ms increments
+        torque_controller_mux.tick(
+            cur_tick,
+            (const DrivetrainDynamicReport_s) {},
+            (const PedalsSystemData_s) {},
+            (const SteeringSystemData_s) {},
+            (const AnalogConversion_s) {},
+            (const AnalogConversion_s) {},
+            (const AnalogConversion_s) {},
+            (const AnalogConversion_s) {},
+            DialMode_e::MODE_1,
+            buttonTimeSeries[i]
+        );
+        // Test conditions
+        if (i == 2)
+            ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_MID_TORQUE);
+        if (i == 4)
+            ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_FULL_TORQUE);
+        if (i == 6)
+            ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_LOW_TORQUE);
+        if (i == SERIES_LENGTH - 1)
+            ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_LOW_TORQUE);
+    }
+}
+
+#endif /* TORQUE_CONTROLLER_MUX_TEST */