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