Commit currently running fet12v2 code
Works on the real power-wheels cart. Can't say too much more about it;
leaving more substantial clean-up for later.
Change-Id: I1b06b04f5cc52240c028b48828d76873e86c26c1
diff --git a/motors/algorithms.h b/motors/algorithms.h
index 63d6d19..c67383b 100644
--- a/motors/algorithms.h
+++ b/motors/algorithms.h
@@ -2,6 +2,7 @@
#define MOTORS_ALGORITHMS_H_
#include <stdint.h>
+#include <array>
namespace frc971 {
namespace motors {
@@ -26,6 +27,22 @@
// and the corresponding outputs will be inversely proportional to the weights.
BalancedReadings BalanceReadings(ReadingsToBalance to_balance);
+inline BalancedReadings BalanceSimpleReadings(
+ const ::std::array<float, 3> readings) {
+ float offset = 0;
+ for (int i = 0; i < 3; ++i) {
+ offset += readings[i];
+ }
+
+ offset = offset / 3.0f;
+
+ BalancedReadings r;
+ for (int i = 0; i < 3; ++i) {
+ r.readings[i] = static_cast<float>(readings[i]) - offset;
+ }
+ return r;
+}
+
inline BalancedReadings BalanceSimpleReadings(const uint16_t readings[3]) {
float offset = 0;
for (int i = 0; i < 3; ++i) {