Code for the motor controller
This is basically what we used in Detroit.
Change-Id: If2820d7ec5fcbc5f33b4082025027a6e969ad0e1
diff --git a/motors/algorithms.h b/motors/algorithms.h
new file mode 100644
index 0000000..d1fce7a
--- /dev/null
+++ b/motors/algorithms.h
@@ -0,0 +1,32 @@
+#ifndef MOTORS_ALGORITHMS_H_
+#define MOTORS_ALGORITHMS_H_
+
+#include <stdint.h>
+
+namespace frc971 {
+namespace salsa {
+
+struct ReadingsToBalance {
+ // Adds a single reading at index.
+ void Add(int index, int32_t value) {
+ sums[index] += value;
+ ++weights[index];
+ }
+
+ int32_t sums[3];
+ int weights[3];
+};
+
+struct BalancedReadings {
+ float readings[3];
+};
+
+// Returns three readings which add up to 0 and are the same distances apart as
+// the input ones (by weight). The distances between the averages of the inputs
+// and the corresponding outputs will be inversely proportional to the weights.
+BalancedReadings BalanceReadings(ReadingsToBalance to_balance);
+
+} // namespace salsa
+} // namespace frc971
+
+#endif // MOTORS_ALGORITHMS_H_