Code for the motor controller
This is basically what we used in Detroit.
Change-Id: If2820d7ec5fcbc5f33b4082025027a6e969ad0e1
diff --git a/motors/algorithms.cc b/motors/algorithms.cc
new file mode 100644
index 0000000..1d655ef
--- /dev/null
+++ b/motors/algorithms.cc
@@ -0,0 +1,50 @@
+#include "motors/algorithms.h"
+
+namespace frc971 {
+namespace salsa {
+
+BalancedReadings BalanceReadings(const ReadingsToBalance to_balance) {
+ // TODO(Brian): Get rid of the floating point divides.
+ BalancedReadings result;
+ if (to_balance.weights[0] == 0) {
+ const float average1 = static_cast<float>(to_balance.sums[1]) /
+ static_cast<float>(to_balance.weights[1]);
+ const float average2 = static_cast<float>(to_balance.sums[2]) /
+ static_cast<float>(to_balance.weights[2]);
+ result.readings[0] = -(average1 + average2);
+ result.readings[1] = average1;
+ result.readings[2] = average2;
+ } else if (to_balance.weights[1] == 0) {
+ const float average0 = static_cast<float>(to_balance.sums[0]) /
+ static_cast<float>(to_balance.weights[0]);
+ const float average2 = static_cast<float>(to_balance.sums[2]) /
+ static_cast<float>(to_balance.weights[2]);
+ result.readings[0] = average0;
+ result.readings[1] = -(average0 + average2);
+ result.readings[2] = average2;
+ } else if (to_balance.weights[2] == 0) {
+ const float average0 = static_cast<float>(to_balance.sums[0]) /
+ static_cast<float>(to_balance.weights[0]);
+ const float average1 = static_cast<float>(to_balance.sums[1]) /
+ static_cast<float>(to_balance.weights[1]);
+ result.readings[0] = average0;
+ result.readings[1] = average1;
+ result.readings[2] = -(average0 + average1);
+ } else {
+ const float average0 = static_cast<float>(to_balance.sums[0]) /
+ static_cast<float>(to_balance.weights[0]);
+ const float average1 = static_cast<float>(to_balance.sums[1]) /
+ static_cast<float>(to_balance.weights[1]);
+ const float average2 = static_cast<float>(to_balance.sums[2]) /
+ static_cast<float>(to_balance.weights[2]);
+ const float offset = (average0 + average1 + average2) / -3;
+ result.readings[0] = average0 + offset;
+ result.readings[1] = average1 + offset;
+ result.readings[2] = average2 + offset;
+ }
+
+ return result;
+}
+
+} // namespace salsa
+} // namespace frc971