Controls: TC Scale Factor (#292)

* Controls: TC Scale Factor

* Removed the redundant function and made the stateflow placeholder the CalculateTCScaleFactor function. Also fixed the bounding if-statement in actual slip function.

firmware/shared/controls/tc_scale_factor.h

@@ -0,0 +1,29 @@
+#pragma once
+
+namespace ctrl {
+// Note: The CalculateActualSlip function has Div-by-Zero error if left front
+// and right front wheel speeds = 0.
+template <typename T>
+T CalculateActualSlip(T left_rear_wheel_speed, T right_rear_wheel_speed,
+                      T left_front_wheel_speed, T right_front_wheel_speed) {
+    T idle_wheel_spd = (left_front_wheel_speed + right_front_wheel_speed) / 2.0;
+    T actual_slip;
+
+    if (left_rear_wheel_speed > right_rear_wheel_speed) {
+        actual_slip = (left_rear_wheel_speed / idle_wheel_spd) - 1;
+    } else {
+        actual_slip = (right_rear_wheel_speed / idle_wheel_spd) - 1;
+    }
+
+    if (actual_slip < 0) {
+        actual_slip = 0;
+    }
+    return actual_slip;
+}
+
+template <typename T>
+T CalculateTCScaleFactor(T actual_slip, T target_slip) {
+    // Stateflow: Multi-Stage TC
+    return 0;
+}
+}  // namespace ctrl

firmware/shared/controls/tc_scale_factor_test.cc

@@ -0,0 +1,37 @@
+#include "tc_scale_factor.h"  // include the header with your function implementation
+
+#include <iostream>
+
+#include "testing.h"
+
+int main() {
+    // Write your test cases.
+
+    // Should return 0 because right-rear wheel speed is greater than idle wheel
+    // speed, forcing the bound to 0.
+    ASSERT_CLOSE(ctrl::CalculateActualSlip(132.5, 134.0, 140.0, 135.0), 0);
+
+    // Same as the last test but Left Rear wheel speed is used because it is
+    // greater then Right-Rear wheel speed.
+    ASSERT_CLOSE(ctrl::CalculateActualSlip(133.4, 130.2, 140.0, 135.0), 0);
+
+    // Should be a decimal since Right-Rear wheel speed is less than the idle
+    // speed, and the Actual-Slip value will return.
+    ASSERT_CLOSE(ctrl::CalculateActualSlip(155.6, 157.2, 155.3, 157.8),
+                 0.004152);
+
+    // Should be a decimal for same reasons as previous, but for Left-Rear wheel
+    // speed.
+    ASSERT_CLOSE(ctrl::CalculateActualSlip(156.4, 155.3, 155.2, 157.1),
+                 0.001601);
+
+    // (Unbounded) Should return 0 because stateflow functionality is not
+    // introduced yet, so function returns 0.
+    ASSERT_CLOSE(ctrl::CalculateTCScaleFactor(0.0042, 0.2), 0);
+
+    // (Bounded by 0) Same as previous test
+    ASSERT_CLOSE(ctrl::CalculateTCScaleFactor(0.0, 0.2), 0);
+
+    // This statement will not be reached if an assert fails.
+    std::cout << "All tests passed" << std::endl;
+}