From b4eeab0233b76f6d4244fc68da2295f8277b779d Mon Sep 17 00:00:00 2001
From: Peter Jabra <jabrap1@mcmaster.ca>
Date: Mon, 4 Nov 2024 20:22:55 -0500
Subject: [PATCH] Steering Angle Logic (#290)

* created steering angle function

* fixed typo, removed unneeded import, and removed comment
---
 firmware/shared/controls/steering_angle.h     | 16 +++++++++
 .../shared/controls/steering_angle_test.cc    | 34 +++++++++++++++++++
 2 files changed, 50 insertions(+)
 create mode 100644 firmware/shared/controls/steering_angle.h
 create mode 100644 firmware/shared/controls/steering_angle_test.cc

diff --git a/firmware/shared/controls/steering_angle.h b/firmware/shared/controls/steering_angle.h
new file mode 100644
index 000000000..afab1a2b8
--- /dev/null
+++ b/firmware/shared/controls/steering_angle.h
@@ -0,0 +1,16 @@
+#pragma once
+
+// Peter Jabra
+
+namespace ctrl {
+
+template <typename T>
+T CalculateSteeringAngle(bool steering_error, T steering_angle) {
+    if (steering_error) {
+        return static_cast<T>(0.5);
+    } else {
+        return steering_angle;
+    }
+}
+
+}  // namespace ctrl
\ No newline at end of file
diff --git a/firmware/shared/controls/steering_angle_test.cc b/firmware/shared/controls/steering_angle_test.cc
new file mode 100644
index 000000000..31335e217
--- /dev/null
+++ b/firmware/shared/controls/steering_angle_test.cc
@@ -0,0 +1,34 @@
+#include "steering_angle.h"
+
+#include <iostream>
+
+#include "testing.h"
+
+int main() {
+    using namespace ctrl;
+
+    // Test case 1: No error expected
+    ASSERT_CLOSE(CalculateSteeringAngle(false, 0.8), 0.8);
+
+    // Test case 2: No error expected
+    ASSERT_CLOSE(CalculateSteeringAngle(false, 1.0), 1.0);
+
+    // Test case 3: No error expected
+    ASSERT_CLOSE(CalculateSteeringAngle(false, 0.0), 0.0);
+
+    // Test case 4: No error expected
+    ASSERT_CLOSE(CalculateSteeringAngle(false, 0.5), 0.5);
+
+    // Test case 5: Error, steering angle should be reset to 0.5
+    ASSERT_CLOSE(CalculateSteeringAngle(true, 0.8), 0.5);
+
+    // Test case 6: Error, steering angle should be reset to 0.5
+    ASSERT_CLOSE(CalculateSteeringAngle(true, 0.2), 0.5);
+
+    // Test case 7: Error, steering angle should be reset to 0.5
+    ASSERT_CLOSE(CalculateSteeringAngle(true, 0.5), 0.5);
+
+    std::cout << "All test cases passed!" << std::endl;
+
+    return 0;
+}
\ No newline at end of file