Brake Pedal Position and Brake Pedal Light Logic

firmware/shared/controls/brake_pedal_lights.h

@@ -0,0 +1,25 @@
+#pragma once
+#include <algorithm>
+#include <iostream>
+
+namespace ctrl {
+
+template <typename T>
+T CalculateBrakePedalPosition(bool rear_brake_pedal_error,
+                              bool front_brake_pedal_error,
+                              T rear_brake_pedal_position,
+                              T front_brake_pedal_position) {
+    // Brake Error Handling
+    if (rear_brake_pedal_error || front_brake_pedal_error) {
+        return static_cast<T>(0);
+    } else {
+        return std::max(front_brake_pedal_position, rear_brake_pedal_position);
+    }
+}
+
+template <typename T>
+bool DetectBrakeLight(T brake_pedal_position) {
+    return (brake_pedal_position > 0.1);
+}
+
+}  // namespace ctrl

firmware/shared/controls/brake_pedal_lights_test.cc

@@ -0,0 +1,40 @@
+#include "brake_pedal_lights.h"
+
+#include <iostream>
+
+#include "testing.h"
+
+int main() {
+    using namespace ctrl;
+
+    // Test Case 1: CalculateBrakePedalPosition with No Errors and Edge Values
+    ASSERT_CLOSE(CalculateBrakePedalPosition(false, false, 0.0f, 0.0f), 0.0f);
+
+    ASSERT_CLOSE(CalculateBrakePedalPosition(false, false, 1.0f, 0.0f), 1.0f);
+
+    ASSERT_CLOSE(CalculateBrakePedalPosition(false, false, 0.5f, 0.7f), 0.7f);
+
+    // Test Case 2: CalculateBrakePedalPosition with Brake Error
+    ASSERT_CLOSE(CalculateBrakePedalPosition(true, false, 0.8f, 0.9f), 0.0f);
+
+    ASSERT_CLOSE(CalculateBrakePedalPosition(false, true, 0.8f, 0.9f), 0.0f);
+
+    ASSERT_CLOSE(CalculateBrakePedalPosition(true, true, 0.8f, 0.9f), 0.0f);
+
+    // Test Case 3: DetectBrakeLight with Values Just Below and Above the
+    // Threshold
+    ASSERT_FALSE(DetectBrakeLight(0.05f));  // Below threshold
+
+    ASSERT_FALSE(DetectBrakeLight(0.09f));  // Near threshold
+
+    ASSERT_TRUE(DetectBrakeLight(0.11f));  // Just above threshold
+
+    // Test Case 4: DetectBrakeLight with High Pedal Positions
+    ASSERT_TRUE(DetectBrakeLight(0.5f));
+
+    ASSERT_TRUE(DetectBrakeLight(1.0f));
+
+    std::cout << "All test cases passed!" << std::endl;
+
+    return 0;
+}