Add tests for utils

5150VEX · Nov 22, 2023 · fea5135 · fea5135
1 parent 0fa0af5
commit fea5135
Showing 1 changed file with 43 additions and 1 deletion.
diff --git a/test/util.cpp b/test/util.cpp
@@ -1,3 +1,45 @@
 #include <gtest/gtest.h>
+#include <cmath>
+#include <vector>
 
-TEST(UtilsTest, test) { ASSERT_EQ(1, 1); }
+#include "lemlib/util.hpp"
+#include "lemlib/pose.hpp"
+
+TEST(UtilsTest, slewUnderLimit) {
+    float output = lemlib::slew(50, 5, 100);
+
+    EXPECT_EQ(output, 50);
+}
+
+TEST(UtilsTest, slewOverLmimit) {
+    float output = lemlib::slew(500, 0, 100);
+
+    EXPECT_EQ(output, 100);
+}
+
+TEST(UtilsTest, angleErrorDegrees) {
+    float output = lemlib::angleError(90, 45, false);
+    EXPECT_EQ(output, 45);
+
+    output = lemlib::angleError(315, 45, false);
+    EXPECT_EQ(output, -90);
+}
+
+TEST(UtilsTest, angleErrorRadians) {
+    float output = lemlib::angleError(M_PI / 2, M_PI / 4, true);
+    EXPECT_FLOAT_EQ(output, (M_PI / 4));
+
+    output = lemlib::angleError(((7 * M_PI) / 4), (M_PI / 4), true);
+    EXPECT_FLOAT_EQ(output, -M_PI / 2);
+}
+
+TEST(UtilsTest, avg) {
+    // bob
+    float output = lemlib::avg({1, 2, 3, 4});
+    EXPECT_FLOAT_EQ(output, 2.5);
+}
+
+TEST(UtilsTest, ema) {
+    float output = lemlib::ema(127, 0, 0.5);
+    EXPECT_FLOAT_EQ(output, 63.5);
+}