Code for the motor controller
This is basically what we used in Detroit.
Change-Id: If2820d7ec5fcbc5f33b4082025027a6e969ad0e1
diff --git a/motors/math.h b/motors/math.h
new file mode 100644
index 0000000..851a90a
--- /dev/null
+++ b/motors/math.h
@@ -0,0 +1,131 @@
+#ifndef MOTORS_MATH_H_
+#define MOTORS_MATH_H_
+
+#include <limits.h>
+
+#include <complex>
+#include <ratio>
+
+// This file has some specialized math functions useful for implementing our
+// controls in a minimal number of cycles.
+
+namespace frc971 {
+namespace salsa {
+
+inline constexpr unsigned int Log2RoundUp(unsigned int x) {
+ return (x < 2) ? x : (1 + Log2RoundUp(x / 2));
+}
+
+template <typename T>
+inline constexpr const T &ConstexprMax(const T &a, const T &b) {
+ return (a < b) ? b : a;
+}
+
+namespace math_internal {
+
+constexpr uint32_t SinCosTableSize() { return 1024; }
+
+constexpr float FloatMaxMagnitude() { return 1.0f; }
+
+constexpr bool IsPowerOf2(uint32_t value) {
+ return value == (1u << (Log2RoundUp(value) - 1));
+}
+
+static_assert(IsPowerOf2(SinCosTableSize()), "Tables need to be a power of 2");
+
+extern float sin_int_table[SinCosTableSize()];
+extern float cos_int_table[SinCosTableSize()];
+extern float sin_float_table[SinCosTableSize() + 1];
+extern float cos_float_table[SinCosTableSize() + 1];
+
+template <class Rotation>
+float FastTableLookupInt(uint32_t theta, const float *table) {
+ static_assert(IsPowerOf2(Rotation::den),
+ "Denominator needs to be a power of 2");
+
+ // Don't need to worry about the sizes of intermediates given this constraint.
+ static_assert(
+ ConstexprMax<uint32_t>(Rotation::den, SinCosTableSize()) * Rotation::num <
+ UINT32_MAX,
+ "Numerator and denominator are too big");
+
+ // Rounding/truncating here isn't supported.
+ static_assert(Rotation::den <= SinCosTableSize(),
+ "Tables need to be bigger");
+
+ // Don't feel like thinking through the consequences of this not being true.
+ static_assert(Rotation::num > 0 && Rotation::den > 0,
+ "Need a positive ratio");
+
+ constexpr uint32_t kDenominatorRatio = SinCosTableSize() / Rotation::den;
+
+ // These should always be true given the other constraints.
+ static_assert(kDenominatorRatio * Rotation::den == SinCosTableSize(),
+ "Math is broken");
+ static_assert(IsPowerOf2(kDenominatorRatio), "Math is broken");
+
+ return table[(theta * kDenominatorRatio * Rotation::num +
+ kDenominatorRatio * Rotation::num / 2) %
+ SinCosTableSize()];
+}
+
+inline float FastTableLookupFloat(float theta, const float *table) {
+ const int index = (SinCosTableSize() / 2) +
+ static_cast<int32_t>(theta * ((SinCosTableSize() / 2) /
+ FloatMaxMagnitude()));
+ return table[index];
+}
+
+} // namespace math_internal
+
+// All theta arguments to the float-based functions must be in [-0.2, 0.2].
+
+inline float FastSinFloat(float theta) {
+ return math_internal::FastTableLookupFloat(theta,
+ math_internal::sin_float_table);
+}
+
+inline float FastCosFloat(float theta) {
+ return math_internal::FastTableLookupFloat(theta,
+ math_internal::cos_float_table);
+}
+
+inline ::std::complex<float> ImaginaryExpFloat(float theta) {
+ return ::std::complex<float>(FastCosFloat(theta), FastSinFloat(theta));
+}
+
+// The integer-based sin/cos functions all have a Rotation template argument,
+// which should be a ::std::ratio. The real argument to the trigonometric
+// function is this ratio multiplied by the theta argument.
+//
+// Specifically, they return the function evaluated at
+// (Rotation * (theta + 0.5)).
+//
+// All theta arguments must be in [0, Rotation::den).
+//
+// All denominators must be powers of 2.
+
+template<class Rotation>
+float FastSinInt(uint32_t theta) {
+ return math_internal::FastTableLookupInt<Rotation>(
+ theta, math_internal::sin_int_table);
+}
+
+template<class Rotation>
+float FastCosInt(uint32_t theta) {
+ return math_internal::FastTableLookupInt<Rotation>(
+ theta, math_internal::cos_int_table);
+}
+
+template<class Rotation>
+::std::complex<float> ImaginaryExpInt(uint32_t theta) {
+ return ::std::complex<float>(FastCosInt<Rotation>(theta),
+ FastSinInt<Rotation>(theta));
+}
+
+void MathInit();
+
+} // namespace salsa
+} // namespace frc971
+
+#endif // MOTORS_MATH_H_