Code for the motor controller
This is basically what we used in Detroit.
Change-Id: If2820d7ec5fcbc5f33b4082025027a6e969ad0e1
diff --git a/motors/algorithms_test.cc b/motors/algorithms_test.cc
new file mode 100644
index 0000000..87cb5b5
--- /dev/null
+++ b/motors/algorithms_test.cc
@@ -0,0 +1,100 @@
+#include "motors/algorithms.h"
+
+#include <inttypes.h>
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace salsa {
+namespace testing {
+
+class BalanceReadingsTest : public ::testing::Test {
+ protected:
+ void CheckReadingsResult(const ReadingsToBalance &to_balance) {
+ ASSERT_GE((to_balance.weights[0] > 0) + (to_balance.weights[1] > 0) +
+ (to_balance.weights[2] > 0),
+ 2)
+ << "Need at least 2 readings";
+ ASSERT_GE(to_balance.weights[0], 0);
+ ASSERT_GE(to_balance.weights[1], 0);
+ ASSERT_GE(to_balance.weights[2], 0);
+
+ const BalancedReadings result = BalanceReadings(to_balance);
+
+ {
+ const auto sum =
+ result.readings[0] + result.readings[1] + result.readings[2];
+ EXPECT_GE(sum, -2);
+ EXPECT_LE(sum, 2);
+ }
+
+ if (to_balance.weights[0] == 0) {
+ const double averages[3] = {
+ 0, static_cast<double>(to_balance.sums[1]) / to_balance.weights[1],
+ static_cast<double>(to_balance.sums[2]) / to_balance.weights[2]};
+ EXPECT_LE(::std::abs((averages[1] - averages[2]) -
+ (result.readings[1] - result.readings[2])),
+ 0.5);
+ } else if (to_balance.weights[1] == 0) {
+ const double averages[3] = {
+ static_cast<double>(to_balance.sums[0]) / to_balance.weights[0], 0,
+ static_cast<double>(to_balance.sums[2]) / to_balance.weights[2]};
+ EXPECT_LE(::std::abs((averages[0] - averages[2]) -
+ (result.readings[0] - result.readings[2])),
+ 0.5);
+ } else if (to_balance.weights[2] == 0) {
+ const double averages[3] = {
+ static_cast<double>(to_balance.sums[0]) / to_balance.weights[0],
+ static_cast<double>(to_balance.sums[1]) / to_balance.weights[1], 0};
+ EXPECT_LE(::std::abs((averages[0] - averages[1]) -
+ (result.readings[0] - result.readings[1])),
+ 0.5);
+ } else {
+ const double averages[3] = {
+ static_cast<double>(to_balance.sums[0]) / to_balance.weights[0],
+ static_cast<double>(to_balance.sums[1]) / to_balance.weights[1],
+ static_cast<double>(to_balance.sums[2]) / to_balance.weights[2]};
+
+ const double middle = (averages[0] + averages[1] + averages[2]) / 3;
+ const double average_distances[3] = {
+ ::std::abs(averages[0] - middle - result.readings[0]),
+ ::std::abs(averages[1] - middle - result.readings[1]),
+ ::std::abs(averages[2] - middle - result.readings[2])};
+ // distances[0]/distances[1] = weights[1]/weights[0]
+ // distances[0]*weights[0]/weights[1] = distances[1]
+ EXPECT_LE(::std::abs(average_distances[0] *
+ static_cast<double>(to_balance.weights[0]) /
+ static_cast<double>(to_balance.weights[1]) -
+ average_distances[1]),
+ 0.01);
+ EXPECT_LE(::std::abs(average_distances[2] *
+ static_cast<double>(to_balance.weights[2]) /
+ static_cast<double>(to_balance.weights[1]) -
+ average_distances[1]),
+ 0.01);
+ EXPECT_LE(::std::abs(average_distances[0] *
+ static_cast<double>(to_balance.weights[0]) /
+ static_cast<double>(to_balance.weights[2]) -
+ average_distances[2]),
+ 0.01);
+ }
+ }
+};
+
+TEST_F(BalanceReadingsTest, Basic) {
+ CheckReadingsResult({{50, 50, 50}, {1, 1, 1}});
+ CheckReadingsResult({{50, 50, 0}, {1, 1, 0}});
+ CheckReadingsResult({{50, 0, 50}, {1, 0, 1}});
+ CheckReadingsResult({{0, 50, 50}, {0, 1, 1}});
+ CheckReadingsResult({{0, 50, 100}, {0, 1, 2}});
+ CheckReadingsResult({{100, 50, 50}, {2, 1, 1}});
+ CheckReadingsResult({{100, 100, 50}, {2, 2, 1}});
+
+ CheckReadingsResult({{150, 50, 50}, {1, 1, 1}});
+ CheckReadingsResult({{150, 50, 50}, {2, 2, 2}});
+ CheckReadingsResult({{3424, 5013, 3424}, {2, 2, 2}});
+}
+
+} // namespace testing
+} // namespace salsa
+} // namespace frc971