diff --git a/firmware/shared/controls/steering_angle.h b/firmware/shared/controls/steering_angle.h new file mode 100644 index 00000000..afab1a2b --- /dev/null +++ b/firmware/shared/controls/steering_angle.h @@ -0,0 +1,16 @@ +#pragma once + +// Peter Jabra + +namespace ctrl { + +template +T CalculateSteeringAngle(bool steering_error, T steering_angle) { + if (steering_error) { + return static_cast(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 00000000..31335e21 --- /dev/null +++ b/firmware/shared/controls/steering_angle_test.cc @@ -0,0 +1,34 @@ +#include "steering_angle.h" + +#include + +#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