Finish the pistol grip code
Change-Id: I95c03a95ac0ec64b4314ec310ad6535176b1d529
diff --git a/motors/algorithms.h b/motors/algorithms.h
index d1fce7a..8b397f8 100644
--- a/motors/algorithms.h
+++ b/motors/algorithms.h
@@ -26,6 +26,19 @@
// and the corresponding outputs will be inversely proportional to the weights.
BalancedReadings BalanceReadings(ReadingsToBalance to_balance);
+inline BalancedReadings BalanceSimpleReadings(const uint16_t readings[3]) {
+ float offset = 0;
+ for (int i = 0; i < 3; ++i) {
+ offset += static_cast<float>(readings[i]);
+ }
+
+ BalancedReadings r;
+ for (int i = 0; i < 3; ++i) {
+ r.readings[i] = static_cast<float>(readings[i]) + offset / -3;
+ }
+ return r;
+}
+
} // namespace salsa
} // namespace frc971