Code for the motor controller
This is basically what we used in Detroit.
Change-Id: If2820d7ec5fcbc5f33b4082025027a6e969ad0e1
diff --git a/motors/BUILD b/motors/BUILD
index e652069..e75f74b 100644
--- a/motors/BUILD
+++ b/motors/BUILD
@@ -8,7 +8,32 @@
],
deps = [
':util',
+ ':motor',
+ ':motor_controls',
'//motors/core',
+ '//motors/peripheral:can',
+ '//motors/peripheral:adc',
+ '//motors/usb:legacy',
+ ],
+ restricted_to = mcu_cpus,
+)
+
+cc_library(
+ name = 'motor',
+ hdrs = [
+ 'motor.h',
+ ],
+ srcs = [
+ 'motor.cc',
+ ],
+ deps = [
+ ':algorithms',
+ ':util',
+ '//motors/core',
+ '//motors/peripheral:configuration',
+
+ '//motors/peripheral:adc',
+ '//motors/peripheral:can',
'//motors/usb:legacy',
],
restricted_to = mcu_cpus,
@@ -25,5 +50,86 @@
hdrs = [
'util.h',
],
+ deps = [
+ '//motors/core',
+ ],
+ restricted_to = mcu_cpus,
+)
+
+genrule(
+ name = 'doc',
+ srcs = [
+ 'NOTES.md',
+ ],
+ outs = [
+ 'NOTES.html',
+ ],
+ cmd = ' '.join([
+ 'pandoc',
+ '-f', 'markdown_github-hard_line_breaks',
+ '-t', 'html5',
+ '-o', '$@', '$<',
+ ]),
+)
+
+cc_library(
+ name = 'algorithms',
+ hdrs = [
+ 'algorithms.h',
+ ],
+ srcs = [
+ 'algorithms.cc',
+ ],
+ compatible_with = mcu_cpus,
+)
+
+cc_test(
+ name = 'algorithms_test',
+ srcs = [
+ 'algorithms_test.cc',
+ ],
+ deps = [
+ ':algorithms',
+ '//aos/testing:googletest',
+ ],
+)
+
+cc_library(
+ name = 'math',
+ hdrs = [
+ 'math.h',
+ ],
+ srcs = [
+ 'math.cc',
+ ],
+ compatible_with = mcu_cpus,
+)
+
+cc_test(
+ name = 'math_test',
+ srcs = [
+ 'math_test.cc',
+ ],
+ deps = [
+ ':math',
+ '//aos/testing:googletest',
+ '//third_party/googletest:googlemock',
+ ],
+)
+
+cc_library(
+ name = 'motor_controls',
+ hdrs = [
+ 'motor_controls.h',
+ ],
+ srcs = [
+ 'motor_controls.cc',
+ ],
+ deps = [
+ ':math',
+ ':motor',
+ '//motors/peripheral:configuration',
+ '//third_party/eigen',
+ ],
restricted_to = mcu_cpus,
)
diff --git a/motors/NOTES.md b/motors/NOTES.md
new file mode 100644
index 0000000..7185bed
--- /dev/null
+++ b/motors/NOTES.md
@@ -0,0 +1,79 @@
+* Teensy 3.5 has a MK64FX512VMD12 (Freescale Kinetis K64 sub-family)
+ * It's a Cortex-M4F, which means it has an FPU and DSP instructions.
+ * 512 KB of flash
+ * 192 kB SRAM (64 kB SRAM\_L, 128 kB SRAM\_U)
+ * Up to 120 MHz
+* [datasheet](http://cache.freescale.com/files/microcontrollers/doc/data_sheet/K64P144M120SF5.pdf)
+* [reference manual](http://cache.nxp.com/assets/documents/data/en/reference-manuals/K64P144M120SF5RM.pdf)
+* [schematic](https://www.pjrc.com/teensy/schematic.html).
+* [actual docs on the bit banding](http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0439b/Behcjiic.html)
+* [ARM Cortex-M Programming Guide to Memory Barrier Instructions](https://static.docs.arm.com/dai0321/a/DAI0321A_programming_guide_memory_barriers_for_m_profile.pdf)
+* [Cortex-M4 instruction timings](http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0439b/CHDDIGAC.html)
+* RM08, 5v, 2048 edges per revolution, single ended incremental, with index pulse
+* [motor](https://hobbyking.com/en_us/turnigy-aquastar-t20-3t-730kv-1280kv-water-cooled-brushless-motor.html)
+ * 2 pole pairs per revolution
+
+* Change MOSFET gate resistors to 1 ohm (or something else?)
+* Reshuffle input capacitor connections
+ * Thermals?
+ * Farther apart
+ * Other side?
+* More capacitors on battery power
+ * Like across individual half bridges
+* No paste on wire pads
+
+### Clocking
+* Running the core clock at its maximum of 120 MHz
+* Bus clocks (both of them) are their maximum of 60 MHz
+
+### Timing (what triggers what)
+Coordinating the timing of everything is pretty important. The general idea is
+to keep everything in sync based on the FTM module(s) which drive the motor
+outputs. They trigger the ADCs too.
+
+FTM0 and FTM3 are synced using the global time base.
+
+The timing is divided into "cycles". Each cycle is a fixed length of time.
+The start of the cycle is when the FTM module(s) have a count of 0.
+
+TODO(Brian): Update the rest of this.
+
+Both PDBs are used to trigger ADC samples, in conjunction with DMA to
+automatically setup the correct sequence and record the results. This gives
+tight control over the timing (DMA can take a bit to get there, and interrupts
+would be worse) and doesn't require CPU time besides reading the result at the
+end.
+The PDB is triggered by the initialization external trigger of an FTM.
+From there, DMA triggered by the ADC conversion complete event finishes and
+prepares for the next round.
+PDB triggers of ADCs alternate between channels so DMA can read+update the other
+one in the mean time.
+One DMA channel copies the result out, and then links to another channel which
+reconfigures that ADC conversion setup register to prepare for the next time.
+This doesn't allow skipping ADC conversions, but conversion results can always
+be ignored later.
+This also doesn't allow scheduling ADC conversions at any points besides at
+fixed multiples from thes start of the cycle, but that should be fine.
+The ADC setup should stop converting by the end of the cycle so the new PDB
+trigger will restart everything, and both DMA channels should reset themselves
+after two cycles (so that the code can read ADC results in one area while
+they're being written to the other).
+
+Both PDBs are used to trigger ADC samples, in conjunction with DMA to
+automatically setup the correct sequence and record the results. This gives
+tight control over the timing (DMA can take a bit to get there, and interrupts
+would be worse) and doesn't require CPU time besides reading the result at the
+end.
+The PDB is triggered by the initialization external trigger of an FTM.
+From there, DMA triggered by two unused channels of the FTMs alternates between
+copying results and reconfiguring each of the sets of ADC registers.
+The DMA uses the SMOD functionality to get the corresponding registers of both
+ADCs in one go, which means using a total of 4 DMA channels (1 to configure the
+A registers, 1 to configure the B registers, 1 to copy the A results, and 1 to
+copy the B results).
+
+Currently, ADC sampling all happens at the beginning of a cycle, then the
+computation of new output values happens, then these values need to be
+written to the hardware before the next cycle starts.
+ADC sampling may move to DMA-based at some point to increase flexibility and
+reduce CPU time spent on it.
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
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_
diff --git a/motors/algorithms_test.cc b/motors/algorithms_test.cc
new file mode 100644
index 0000000..87cb5b5
--- /dev/null
+++ b/motors/algorithms_test.cc
@@ -0,0 +1,100 @@
+#include "motors/algorithms.h"
+
+#include <inttypes.h>
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace salsa {
+namespace testing {
+
+class BalanceReadingsTest : public ::testing::Test {
+ protected:
+ void CheckReadingsResult(const ReadingsToBalance &to_balance) {
+ ASSERT_GE((to_balance.weights[0] > 0) + (to_balance.weights[1] > 0) +
+ (to_balance.weights[2] > 0),
+ 2)
+ << "Need at least 2 readings";
+ ASSERT_GE(to_balance.weights[0], 0);
+ ASSERT_GE(to_balance.weights[1], 0);
+ ASSERT_GE(to_balance.weights[2], 0);
+
+ const BalancedReadings result = BalanceReadings(to_balance);
+
+ {
+ const auto sum =
+ result.readings[0] + result.readings[1] + result.readings[2];
+ EXPECT_GE(sum, -2);
+ EXPECT_LE(sum, 2);
+ }
+
+ if (to_balance.weights[0] == 0) {
+ const double averages[3] = {
+ 0, static_cast<double>(to_balance.sums[1]) / to_balance.weights[1],
+ static_cast<double>(to_balance.sums[2]) / to_balance.weights[2]};
+ EXPECT_LE(::std::abs((averages[1] - averages[2]) -
+ (result.readings[1] - result.readings[2])),
+ 0.5);
+ } else if (to_balance.weights[1] == 0) {
+ const double averages[3] = {
+ static_cast<double>(to_balance.sums[0]) / to_balance.weights[0], 0,
+ static_cast<double>(to_balance.sums[2]) / to_balance.weights[2]};
+ EXPECT_LE(::std::abs((averages[0] - averages[2]) -
+ (result.readings[0] - result.readings[2])),
+ 0.5);
+ } else if (to_balance.weights[2] == 0) {
+ const double averages[3] = {
+ static_cast<double>(to_balance.sums[0]) / to_balance.weights[0],
+ static_cast<double>(to_balance.sums[1]) / to_balance.weights[1], 0};
+ EXPECT_LE(::std::abs((averages[0] - averages[1]) -
+ (result.readings[0] - result.readings[1])),
+ 0.5);
+ } else {
+ const double averages[3] = {
+ static_cast<double>(to_balance.sums[0]) / to_balance.weights[0],
+ static_cast<double>(to_balance.sums[1]) / to_balance.weights[1],
+ static_cast<double>(to_balance.sums[2]) / to_balance.weights[2]};
+
+ const double middle = (averages[0] + averages[1] + averages[2]) / 3;
+ const double average_distances[3] = {
+ ::std::abs(averages[0] - middle - result.readings[0]),
+ ::std::abs(averages[1] - middle - result.readings[1]),
+ ::std::abs(averages[2] - middle - result.readings[2])};
+ // distances[0]/distances[1] = weights[1]/weights[0]
+ // distances[0]*weights[0]/weights[1] = distances[1]
+ EXPECT_LE(::std::abs(average_distances[0] *
+ static_cast<double>(to_balance.weights[0]) /
+ static_cast<double>(to_balance.weights[1]) -
+ average_distances[1]),
+ 0.01);
+ EXPECT_LE(::std::abs(average_distances[2] *
+ static_cast<double>(to_balance.weights[2]) /
+ static_cast<double>(to_balance.weights[1]) -
+ average_distances[1]),
+ 0.01);
+ EXPECT_LE(::std::abs(average_distances[0] *
+ static_cast<double>(to_balance.weights[0]) /
+ static_cast<double>(to_balance.weights[2]) -
+ average_distances[2]),
+ 0.01);
+ }
+ }
+};
+
+TEST_F(BalanceReadingsTest, Basic) {
+ CheckReadingsResult({{50, 50, 50}, {1, 1, 1}});
+ CheckReadingsResult({{50, 50, 0}, {1, 1, 0}});
+ CheckReadingsResult({{50, 0, 50}, {1, 0, 1}});
+ CheckReadingsResult({{0, 50, 50}, {0, 1, 1}});
+ CheckReadingsResult({{0, 50, 100}, {0, 1, 2}});
+ CheckReadingsResult({{100, 50, 50}, {2, 1, 1}});
+ CheckReadingsResult({{100, 100, 50}, {2, 2, 1}});
+
+ CheckReadingsResult({{150, 50, 50}, {1, 1, 1}});
+ CheckReadingsResult({{150, 50, 50}, {2, 2, 2}});
+ CheckReadingsResult({{3424, 5013, 3424}, {2, 2, 2}});
+}
+
+} // namespace testing
+} // namespace salsa
+} // namespace frc971
diff --git a/motors/core/mk20dx128.c b/motors/core/mk20dx128.c
index f7dbd09..eef0488 100644
--- a/motors/core/mk20dx128.c
+++ b/motors/core/mk20dx128.c
@@ -31,6 +31,7 @@
#include "motors/core/kinetis.h"
#include <errno.h>
+#include <stdio.h>
// Flash Security Setting. On Teensy 3.2, you can lock the MK20 chip to prevent
// anyone from reading your code. You CAN still reprogram your Teensy while
@@ -63,6 +64,8 @@
void __libc_init_array(void);
void fault_isr(void) {
+ FTM0_C0V = FTM0_C1V = FTM0_C2V = FTM0_C3V = FTM0_C4V = FTM0_C5V = 0;
+ printf("fault_isr\n");
while (1) {
// keep polling some communication while in fault
// mode, so we don't completely die.
diff --git a/motors/decode_dump.py b/motors/decode_dump.py
new file mode 100755
index 0000000..1010e88
--- /dev/null
+++ b/motors/decode_dump.py
@@ -0,0 +1,60 @@
+#!/usr/bin/python3
+
+# Pipe the binary data in and give the CSV output filename as an argument.
+
+import struct
+import sys
+import select
+
+DatapointStruct = struct.Struct('<11h')
+DATAPOINTS = 5000
+TOTAL_SIZE = DatapointStruct.size * DATAPOINTS
+
+data = bytes()
+while len(data) < TOTAL_SIZE:
+ read_now = sys.stdin.buffer.read(TOTAL_SIZE - len(data))
+ if not read_now:
+ print('EOF before data finished', file=sys.stderr)
+ sys.exit(1)
+ data += read_now
+print('%s' % len(data))
+
+readable, _, _ = select.select([sys.stdin.buffer], [], [], 1)
+if readable:
+ print('Extra bytes', file=sys.stderr)
+ sys.exit(1)
+
+decoded = []
+for i in range(DATAPOINTS):
+ datapoint = DatapointStruct.unpack_from(data, i * DatapointStruct.size)
+ decoded.append(datapoint)
+
+def current(reading, ref):
+ reading_voltage = reading / 4096 * 3.3 / 1.47 * (0.768 + 1.47)
+ #reading_ref = ref / 4096 * 3.3
+ reading_ref = 2.5
+ reading_ref = 0
+ return (reading_voltage - reading_ref) / 50 / 0.0003
+
+with open(sys.argv[1], 'w') as out:
+ out.write('current0.0,current1.0,current2.0,current0.1,current1.1,current2.1,count\n')
+ #for point in decoded[2000:7200]:
+ for point in decoded:
+ out.write(','.join(str(d) for d in (
+ current(point[0], point[6]),
+ current(point[1], point[6]),
+ current(point[2], point[6]),
+ #current(point[3], point[6]),
+ #current(point[4], point[6]),
+ #current(point[5], point[6]),
+ point[3] / 100.0,
+ point[4] / 100.0,
+ point[5] / 100.0,
+ point[6] / 100.0,
+ point[7] / 100.0,
+ point[8] / 100.0,
+ point[9] / 100.0,
+ point[10] / 100.0,
+ )) + '\n')
+
+print('all done')
diff --git a/motors/math.cc b/motors/math.cc
new file mode 100644
index 0000000..291ca43
--- /dev/null
+++ b/motors/math.cc
@@ -0,0 +1,37 @@
+#include "motors/math.h"
+
+#include <math.h>
+
+namespace frc971 {
+namespace salsa {
+namespace math_internal {
+
+float sin_int_table[SinCosTableSize()];
+float cos_int_table[SinCosTableSize()];
+float sin_float_table[SinCosTableSize() + 1];
+float cos_float_table[SinCosTableSize() + 1];
+
+} // namespace math_internal
+
+using math_internal::SinCosTableSize;
+
+__attribute__((cold)) void MathInit() {
+ for (uint32_t i = 0; i < SinCosTableSize(); ++i) {
+ const double int_theta =
+ ((static_cast<double>(i) + 0.5) / SinCosTableSize()) * 2.0 * M_PI;
+ math_internal::sin_int_table[i] = sin(int_theta);
+ math_internal::cos_int_table[i] = cos(int_theta);
+ }
+ for (uint32_t i = 0; i < SinCosTableSize() + 1; ++i) {
+ const double float_theta =
+ (static_cast<int32_t>(i) -
+ static_cast<int32_t>(SinCosTableSize() / 2)) *
+ static_cast<double>(math_internal::FloatMaxMagnitude()) /
+ static_cast<double>(SinCosTableSize() / 2);
+ math_internal::sin_float_table[i] = sin(float_theta);
+ math_internal::cos_float_table[i] = cos(float_theta);
+ }
+}
+
+} // namespace salsa
+} // namespace frc971
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_
diff --git a/motors/math_test.cc b/motors/math_test.cc
new file mode 100644
index 0000000..672ee69
--- /dev/null
+++ b/motors/math_test.cc
@@ -0,0 +1,78 @@
+#include "motors/math.h"
+
+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+
+namespace frc971 {
+namespace salsa {
+namespace testing {
+
+class SinCosIntTest : public ::testing::Test {
+ public:
+ void SetUp() override {
+ MathInit();
+ }
+
+ template <class Rotation>
+ void CheckSinCos() {
+ static constexpr float kTolerance = 0.004;
+ for (uint32_t theta = 0; theta < Rotation::den; ++theta) {
+ EXPECT_THAT(
+ FastSinInt<Rotation>(theta),
+ ::testing::FloatNear(sin(ThetaToFloat<Rotation>(theta)), kTolerance));
+ EXPECT_THAT(
+ FastCosInt<Rotation>(theta),
+ ::testing::FloatNear(cos(ThetaToFloat<Rotation>(theta)), kTolerance));
+ }
+ }
+
+ private:
+ template <class Rotation>
+ double ThetaToFloat(uint32_t theta) {
+ const double rotation_double =
+ static_cast<double>(Rotation::num) / static_cast<double>(Rotation::den);
+ return (static_cast<double>(theta) + 0.5) * rotation_double * 2.0 * M_PI;
+ }
+};
+
+TEST_F(SinCosIntTest, Numerator1) {
+ CheckSinCos<::std::ratio<1, 2>>();
+ CheckSinCos<::std::ratio<1, 4>>();
+ CheckSinCos<::std::ratio<1, 8>>();
+ CheckSinCos<::std::ratio<1, 128>>();
+ CheckSinCos<::std::ratio<1, 1024>>();
+}
+
+TEST_F(SinCosIntTest, Numerator5) {
+ CheckSinCos<::std::ratio<5, 8>>();
+ CheckSinCos<::std::ratio<5, 128>>();
+ CheckSinCos<::std::ratio<5, 1024>>();
+}
+
+class SinCosFloatTest : public ::testing::Test {
+ public:
+ void SetUp() override {
+ MathInit();
+ }
+
+ void CheckSinCos(float theta) {
+ ASSERT_GE(theta, -0.2f);
+ ASSERT_LE(theta, 0.2f);
+
+ static constexpr float kTolerance = 0.002;
+ EXPECT_THAT(FastSinFloat(theta),
+ ::testing::FloatNear(sin(theta), kTolerance));
+ EXPECT_THAT(FastCosFloat(theta),
+ ::testing::FloatNear(cos(theta), kTolerance));
+ }
+};
+
+TEST_F(SinCosFloatTest, Endpoints) {
+ CheckSinCos(0);
+ CheckSinCos(-0.2);
+ CheckSinCos(0.2);
+}
+
+} // namespace testing
+} // namespace salsa
+} // namespace frc971
diff --git a/motors/medium_salsa.cc b/motors/medium_salsa.cc
index 1fbe9a6..144fc1e 100644
--- a/motors/medium_salsa.cc
+++ b/motors/medium_salsa.cc
@@ -2,15 +2,35 @@
#include <stdio.h>
+#include <atomic>
+
#include "motors/core/time.h"
+#include "motors/motor.h"
+#include "motors/motor_controls.h"
+#include "motors/peripheral/adc.h"
+#include "motors/peripheral/can.h"
#include "motors/usb/usb_serial.h"
#include "motors/util.h"
namespace frc971 {
namespace salsa {
+namespace {
+
+::std::atomic<Motor *> global_motor{nullptr};
extern "C" {
+
void *__stack_chk_guard = (void *)0x67111971;
+void __stack_chk_fail(void) {
+ while (true) {
+ GPIOC_PSOR = (1 << 5);
+ printf("Stack corruption detected\n");
+ delay(1000);
+ GPIOC_PCOR = (1 << 5);
+ delay(1000);
+ }
+}
+
extern void usb_init();
int _write(int file, char *ptr, int len) {
(void)file;
@@ -24,7 +44,12 @@
extern uint32_t __heap_start__[];
extern uint32_t __stack_end__[];
+void ftm0_isr(void) {
+ global_motor.load(::std::memory_order_relaxed)->HandleInterrupt();
+}
+
} // extern "C"
+} // namespace
extern "C" int main(void) {
// for background about this startup delay, please see these conversations
@@ -39,38 +64,63 @@
// relative to each other, which means centralizing them here makes it a lot
// more manageable.
NVIC_SET_SANE_PRIORITY(IRQ_USBOTG, 0x7);
+ NVIC_SET_SANE_PRIORITY(IRQ_FTM0, 0x3);
// Set the LED's pin to output mode.
GPIO_BITBAND(GPIOC_PDDR, 5) = 1;
PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ GPIO_BITBAND(GPIOA_PDDR, 15) = 1;
+ PORTA_PCR15 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ DMA_CR = DMA_CR_EMLM;
usb_serial_init();
usb_descriptor_set_product_id(0x0490);
usb_init();
+ AdcInit();
+ MathInit();
+ delay(1000);
+ can_init();
+
+ MotorControlsImplementation controls;
+
+ delay(1000);
+ Motor motor(FTM0, FTM1, &controls);
+ motor.Init();
+ global_motor.store(&motor, ::std::memory_order_relaxed);
+ // Output triggers to things like the PDBs on initialization.
+ FTM0_EXTTRIG = FTM_EXTTRIG_INITTRIGEN;
+ // Don't let any memory accesses sneak past here, because we actually
+ // need everything to be starting up.
+ __asm__("" :: : "memory");
// Give everything a chance to get going.
delay(100);
+#if 0
printf("Ram start: %p\n", __bss_ram_start__);
printf("Heap start: %p\n", __heap_start__);
printf("Heap end: %p\n", __brkval);
printf("Stack start: %p\n", __stack_end__);
+#endif
- GPIOC_PSOR = 1 << 5;
+ printf("Going silent to zero motors...\n");
+ // Give the print a chance to make it out.
+ delay(1000);
+ motor.Zero();
+
+ printf("Zeroed motor!\n");
+ // Give stuff a chance to recover from interrupts-disabled.
+ delay(100);
+ motor.Start();
+
+ GPIOA_PCOR = 1 << 15;
+
+ // TODO(Brian): Use SLEEPONEXIT to reduce interrupt latency?
while (true) {}
return 0;
}
-void __stack_chk_fail(void) {
- while (true) {
- GPIOC_PSOR = (1 << 5);
- printf("Stack corruption detected\n");
- delay(1000);
- GPIOC_PCOR = (1 << 5);
- delay(1000);
- }
-}
-
} // namespace salsa
} // namespace frc971
diff --git a/motors/motor.cc b/motors/motor.cc
new file mode 100644
index 0000000..06f9f08
--- /dev/null
+++ b/motors/motor.cc
@@ -0,0 +1,556 @@
+#include "motors/motor.h"
+
+#include <limits.h>
+
+#include <array>
+
+#include "motors/peripheral/configuration.h"
+
+#include <stdio.h>
+#include <inttypes.h>
+#include "motors/core/time.h"
+#include "motors/usb/usb_serial.h"
+#include "motors/peripheral/can.h"
+
+namespace frc971 {
+namespace salsa {
+namespace {
+
+constexpr int kDeadtimeCompensation = 9;
+
+uint32_t CalculateOnTime(uint32_t width, bool flip) {
+ if (width > 0) {
+ width += kDeadtimeCompensation;
+ if (flip) {
+ width += 1;
+ }
+ }
+ return 1500 - width / 2;
+}
+
+uint32_t CalculateOffTime(uint32_t width, bool flip) {
+ if (width > 0) {
+ width += kDeadtimeCompensation;
+ if (!flip) {
+ width += 1;
+ }
+ }
+ return 1500 + width / 2;
+}
+
+} // namespace
+
+Motor::Motor(BigFTM *pwm_ftm, LittleFTM *encoder_ftm, MotorControls *controls)
+ : pwm_ftm_(pwm_ftm), encoder_ftm_(encoder_ftm), controls_(controls) {}
+
+static_assert((BUS_CLOCK_FREQUENCY % SWITCHING_FREQUENCY) == 0,
+ "Switching frequency needs to divide the bus clock frequency");
+
+static_assert(BUS_CLOCK_FREQUENCY / SWITCHING_FREQUENCY < UINT16_MAX,
+ "Need to prescale");
+
+void Motor::Init() {
+ PORTE_PCR24 = PORT_PCR_MUX(1);
+ PORTB_PCR0 = PORT_PCR_MUX(6);
+ PORTB_PCR1 = PORT_PCR_MUX(6);
+
+ PORTC_PCR1 = PORT_PCR_MUX(4);
+ PORTC_PCR2 = PORT_PCR_MUX(4);
+ PORTC_PCR3 = PORT_PCR_MUX(4);
+ PORTC_PCR4 = PORT_PCR_MUX(4);
+ PORTD_PCR4 = PORT_PCR_MUX(4);
+ PORTD_PCR5 = PORT_PCR_MUX(4);
+
+ // PWMSYNC doesn't matter because we set SYNCMODE down below.
+ pwm_ftm_->MODE = FTM_MODE_WPDIS;
+ pwm_ftm_->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
+ encoder_ftm_->MODE = FTM_MODE_WPDIS;
+ encoder_ftm_->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
+
+ pwm_ftm_->SC = FTM_SC_CLKS(0) /* Disable counting for now */;
+ encoder_ftm_->SC =
+ FTM_SC_CLKS(1) /* Use the system clock (not sure it matters) */ |
+ FTM_SC_PS(0) /* Don't prescale the clock (not sure it matters) */;
+
+ pwm_ftm_->CNTIN = encoder_ftm_->CNTIN = 0;
+ pwm_ftm_->CNT = encoder_ftm_->CNT = 0;
+
+ pwm_ftm_->MOD = (BUS_CLOCK_FREQUENCY / SWITCHING_FREQUENCY) - 1;
+ encoder_ftm_->MOD = controls_->counts_per_revolution() - 1;
+
+ // Put them all into combine active-high mode, and all the low ones staying on
+ // all the time by default.
+ // TODO: update comment
+ pwm_ftm_->C0SC = FTM_CSC_ELSA;
+ pwm_ftm_->C0V = 0;
+ pwm_ftm_->C1SC = FTM_CSC_ELSA;
+ pwm_ftm_->C1V = 0;
+ pwm_ftm_->C2SC = FTM_CSC_ELSA;
+ pwm_ftm_->C2V = 0;
+ pwm_ftm_->C3SC = FTM_CSC_ELSA;
+ pwm_ftm_->C3V = 0;
+ pwm_ftm_->C4SC = FTM_CSC_ELSA;
+ pwm_ftm_->C4V = 0;
+ pwm_ftm_->C5SC = FTM_CSC_ELSA;
+ pwm_ftm_->C5V = 0;
+
+ // I think you have to set this to something other than 0 for the quadrature
+ // encoder mode to actually work? This is "input capture on rising edge only",
+ // which should be fine.
+ encoder_ftm_->C0SC = FTM_CSC_ELSA;
+ encoder_ftm_->C1SC = FTM_CSC_ELSA;
+
+ // Initialize all the channels to 0.
+ // TODO(Brian): Is this really what we want?
+ pwm_ftm_->OUTINIT = 0;
+
+ pwm_ftm_->COMBINE = FTM_COMBINE_SYNCEN3 /* Synchronize updates usefully */ |
+ FTM_COMBINE_DTEN3 /* Enable deadtime */ |
+ FTM_COMBINE_COMP3 /* Make them complementary */ |
+ FTM_COMBINE_COMBINE3 /* Combine the channels */ |
+ FTM_COMBINE_SYNCEN2 /* Synchronize updates usefully */ |
+ FTM_COMBINE_DTEN2 /* Enable deadtime */ |
+ FTM_COMBINE_COMP2 /* Make them complementary */ |
+ FTM_COMBINE_COMBINE2 /* Combine the channels */ |
+ FTM_COMBINE_SYNCEN1 /* Synchronize updates usefully */ |
+ FTM_COMBINE_DTEN1 /* Enable deadtime */ |
+ FTM_COMBINE_COMP1 /* Make them complementary */ |
+ FTM_COMBINE_COMBINE1 /* Combine the channels */ |
+ FTM_COMBINE_SYNCEN0 /* Synchronize updates usefully */ |
+ FTM_COMBINE_DTEN0 /* Enable deadtime */ |
+ FTM_COMBINE_COMP0 /* Make them complementary */ |
+ FTM_COMBINE_COMBINE0 /* Combine the channels */;
+
+ // Set a deadtime of 500ns.
+ // TODO: update comment
+ pwm_ftm_->DEADTIME =
+ FTM_DEADTIME_DTPS(0) /* Prescaler of 1 */ | FTM_DEADTIME_DTVAL(9);
+
+ // All of the channels are active high.
+ pwm_ftm_->POL = 0;
+
+ encoder_ftm_->FILTER = FTM_FILTER_CH0FVAL(0) /* No filter */ |
+ FTM_FILTER_CH1FVAL(0) /* No filter */;
+
+ // Could set PHAFLTREN and PHBFLTREN here to enable the filters.
+ encoder_ftm_->QDCTRL = FTM_QDCTRL_QUADEN;
+
+ pwm_ftm_->SYNCONF =
+ FTM_SYNCONF_HWWRBUF /* Hardware trigger flushes switching points */ |
+ FTM_SYNCONF_SWWRBUF /* Software trigger flushes switching points */ |
+ FTM_SYNCONF_SWRSTCNT /* Software trigger resets the count */ |
+ FTM_SYNCONF_SYNCMODE /* Use the new synchronization mode */;
+ encoder_ftm_->SYNCONF =
+ FTM_SYNCONF_SWWRBUF /* Software trigger flushes MOD */ |
+ FTM_SYNCONF_SWRSTCNT /* Software trigger resets the count */ |
+ FTM_SYNCONF_SYNCMODE /* Use the new synchronization mode */;
+
+ // Don't want any intermediate loading points.
+ pwm_ftm_->PWMLOAD = 0;
+
+ // This has to happen after messing with SYNCONF, and should happen after
+ // messing with various other things so the values can get flushed out of the
+ // buffers.
+ pwm_ftm_->SYNC =
+ FTM_SYNC_SWSYNC /* Flush everything out right now */ |
+ FTM_SYNC_CNTMAX /* Load new values at the end of the cycle */;
+ encoder_ftm_->SYNC = FTM_SYNC_SWSYNC /* Flush everything out right now */;
+
+ // Wait for the software synchronization to finish.
+ while (pwm_ftm_->SYNC & FTM_SYNC_SWSYNC) {
+ }
+ while (encoder_ftm_->SYNC & FTM_SYNC_SWSYNC) {
+ }
+}
+
+void Motor::Zero() {
+#if 0
+ while (true) {
+ if (GPIO_BITBAND(GPIOE_PDIR, 24)) {
+ encoder_ftm_->CNT = 0;
+ break;
+ }
+ }
+#else
+ uint32_t scratch;
+ __disable_irq();
+ // Stuff all of this in an inline assembly statement so we can make sure the
+ // compiler doesn't decide sticking constant loads etc in the middle of
+ // the loop is a good idea, because that increases the latency of recognizing
+ // the index pulse edge which makes velocity affect the zeroing accuracy.
+ __asm__ __volatile__(
+ // A label to restart the loop.
+ "0:\n"
+ // Load the current PDIR value for the pin we care about.
+ "ldr %[scratch], [%[pdir_word]]\n"
+ // Terminate the loop if it's non-0.
+ "cbnz %[scratch], 1f\n"
+ // Go back around again.
+ "b 0b\n"
+ // A label to finish the loop.
+ "1:\n"
+ // Reset the count once we're down here. It doesn't actually matter what
+ // value we store because writing anything resets it to CNTIN (ie 0).
+ "str %[scratch], [%[cnt]]\n"
+ : [scratch] "=&l"(scratch)
+ : [pdir_word] "l"(&GPIO_BITBAND(GPIOE_PDIR, 24)),
+ [cnt] "l"(&encoder_ftm_->CNT));
+ __enable_irq();
+#endif
+}
+
+void Motor::Start() {
+ pwm_ftm_->SC = FTM_SC_TOIE /* Interrupt on overflow */ |
+ FTM_SC_CLKS(1) /* Use the system clock */ |
+ FTM_SC_PS(0) /* Don't prescale the clock */;
+ pwm_ftm_->MODE &= ~FTM_MODE_WPDIS;
+ encoder_ftm_->MODE &= ~FTM_MODE_WPDIS;
+
+ NVIC_ENABLE_IRQ(IRQ_FTM0);
+}
+
+void Motor::HandleInterrupt() {
+ const uint32_t starting_sc = pwm_ftm_->SC;
+ pwm_ftm_->SC = starting_sc & ~FTM_SC_TOF;
+
+ const uint32_t start_count = pwm_ftm_->CNT;
+ __asm__("":::"memory");
+ const MediumAdcReadings adc_readings = AdcReadMedium();
+
+ // Turn the light on if we're starting too late (this generally means a
+ // previous iteration took too long).
+ if (start_count > 100) {
+ GPIOC_PSOR = 1 << 5;
+ }
+
+ ReadingsToBalance to_balance{{0, 0, 0}, {0, 0, 0}};
+ {
+ for (int reading = 0; reading < 2; ++reading) {
+ for (int phase = 0; phase < 3; ++phase) {
+ to_balance.Add(phase, adc_readings.motor_currents[phase][reading]);
+ }
+ }
+ }
+ const BalancedReadings balanced = BalanceReadings(to_balance);
+
+ static float current_command = 0;
+ static uint32_t last_command_receive_time = 0;
+ {
+ unsigned char command_data[8];
+ int command_length;
+ can_receive_command(command_data, &command_length);
+ if (command_length == 4) {
+ last_command_receive_time = micros();
+ uint32_t result = command_data[0] << 24 | command_data[1] << 16 |
+ command_data[2] << 8 | command_data[3];
+ current_command = static_cast<float>(result) / 1000.0f;
+ }
+ }
+ if (!time_after(time_add(last_command_receive_time, 100000), micros())) {
+ current_command = 0;
+ }
+
+ static bool high_gear = false;
+ if (controls_->estimated_velocity() < -2015) {
+ high_gear = true;
+ }
+ if (current_command < 1) {
+ high_gear = false;
+ }
+ float current_now = current_command;
+ if (!high_gear) {
+ current_now = current_now * -120.0f / 120.0f;
+ } else {
+ current_now = current_now * 115.0f / 120.0f;
+ }
+
+#if 0
+ static int status_send_counter = 0;
+ if (++status_send_counter == 1000) {
+ // can_send(uint32_t can_id, const unsigned char *data, unsigned int length)
+ unsigned char send_data[8];
+ can_send(9 << 8, send_data, 0);
+ status_send_counter = 0;
+ printf("sent\n");
+ }
+#endif
+
+#define DO_CONTROLS 1
+#if DO_CONTROLS
+ static constexpr int kEncoderOffset = 810;
+ const uint32_t adjusted_count = (encoder_ftm_->CNT + kEncoderOffset) % 1024;
+ const ::std::array<uint32_t, 3> switching_points =
+ controls_->DoIteration(balanced.readings, adjusted_count, current_now);
+ constexpr uint32_t kMax = 2945;
+ static bool done = false;
+ bool done_now = false;
+ if (switching_points[0] > kMax || switching_points[1] > kMax ||
+ switching_points[2] > kMax) {
+ done_now = true;
+ }
+#define USE_ABSOLUTE_CUTOFF 1
+#if USE_ABSOLUTE_CUTOFF
+ static unsigned int current_done_count = 0;
+ bool current_done = false;
+ for (int phase = 0; phase < 3; ++phase) {
+ const float scaled_reading =
+ MotorControls::scale_current_reading(balanced.readings[0]);
+ static constexpr float kMaxBalancedCurrent = 190.0f;
+ if (scaled_reading > kMaxBalancedCurrent ||
+ scaled_reading < -kMaxBalancedCurrent) {
+ current_done = true;
+ }
+ }
+ if (current_done) {
+ if (current_done_count > 5) {
+ done_now = true;
+ }
+ ++current_done_count;
+ } else {
+ current_done_count = 0;
+ }
+#endif
+ if (done_now && !done) {
+ printf("done now\n");
+ printf("switching_points %" PRIu32 " %" PRIu32 " %" PRIu32 "\n",
+ switching_points[0], switching_points[1], switching_points[2]);
+ printf("raw %" PRIu16 " %" PRIu16 " %" PRIu16 " %" PRIu16 " %" PRIu16
+ " %" PRIu16 "\n",
+ adc_readings.motor_currents[0][0], adc_readings.motor_currents[0][1],
+ adc_readings.motor_currents[1][0], adc_readings.motor_currents[1][1],
+ adc_readings.motor_currents[2][0],
+ adc_readings.motor_currents[2][1]);
+ printf("balanced %" PRIu16 " %" PRIu16 " %" PRIu16 "\n",
+ static_cast<uint16_t>(balanced.readings[0]),
+ static_cast<uint16_t>(balanced.readings[1]),
+ static_cast<uint16_t>(balanced.readings[2]));
+ done = true;
+ }
+ if (!done) {
+ pwm_ftm_->C0V = CalculateOnTime(switching_points[0], flip_time_offset_);
+ pwm_ftm_->C1V = CalculateOffTime(switching_points[0], flip_time_offset_);
+ pwm_ftm_->C2V = CalculateOnTime(switching_points[1], flip_time_offset_);
+ pwm_ftm_->C3V = CalculateOffTime(switching_points[1], flip_time_offset_);
+ pwm_ftm_->C4V = CalculateOnTime(switching_points[2], flip_time_offset_);
+ pwm_ftm_->C5V = CalculateOffTime(switching_points[2], flip_time_offset_);
+ flip_time_offset_ = !flip_time_offset_;
+ } else {
+ pwm_ftm_->C0V = 0;
+ pwm_ftm_->C1V = 0;
+ pwm_ftm_->C2V = 0;
+ pwm_ftm_->C3V = 0;
+ pwm_ftm_->C4V = 0;
+ pwm_ftm_->C5V = 0;
+ }
+#define DO_FIXED_PULSE 0
+#elif DO_FIXED_PULSE
+ // An on-width of 60 with 30V in means about 50A through the motor and about
+ // 30W total power dumped by the motor.
+#if 0
+ static int i = 0;
+ ++i;
+ if (i == 2) {
+ pwm_ftm_->C3V = 111;
+ i = 0;
+ } else {
+ pwm_ftm_->C3V = 0;
+ }
+#endif
+ pwm_ftm_->C0V = 0;
+ pwm_ftm_->C1V = 0;
+ pwm_ftm_->C2V = 0;
+ //pwm_ftm_->C3V = 100;
+ pwm_ftm_->C4V = 0;
+ pwm_ftm_->C5V = 0;
+#endif
+
+#define PRINT_READINGS 0
+#if PRINT_READINGS
+ static int i = 0;
+ if (i == 100) {
+ i = 0;
+ printf("i=%" PRIu16 "\n", adc_readings.input_voltage);
+ } else if (i == 20) {
+ printf("0=%" PRIu16 " r=%" PRIu16 "\n",
+ adc_readings.motor_currents[0][0], adc_readings.motor_current_ref);
+ } else if (i == 40) {
+ printf("1=%" PRIu16 " r=%" PRIu16 "\n",
+ adc_readings.motor_currents[1][0], adc_readings.motor_current_ref);
+ } else if (i == 60) {
+ printf("2=%" PRIu16 " r=%" PRIu16 "\n",
+ adc_readings.motor_currents[2][0], adc_readings.motor_current_ref);
+ } else {
+ //printf("%" PRIu32 " to %" PRIu32 "\n", start_count, end_count);
+ }
+ ++i;
+#define PRINT_ALL_READINGS 0
+#elif PRINT_ALL_READINGS
+ printf("ref=%" PRIu16 " 0.0=%" PRIu16 " 1.0=%" PRIu16 " 2.0=%" PRIu16
+ " in=%" PRIu16 " 0.1=%" PRIu16 " 1.1=%" PRIu16 " 2.1=%" PRIu16 "\n",
+ adc_readings.motor_current_ref, adc_readings.motor_currents[0][0],
+ adc_readings.motor_currents[1][0], adc_readings.motor_currents[2][0],
+ adc_readings.input_voltage, adc_readings.motor_currents[0][1],
+ adc_readings.motor_currents[1][1], adc_readings.motor_currents[2][1]);
+#define TAKE_SAMPLE 1
+#elif TAKE_SAMPLE
+#if 0
+ constexpr int kStartupWait = 50000;
+#elif 0
+ constexpr int kStartupWait = 0;
+#elif 0
+ constexpr int kStartupWait = 30000;
+#elif 1
+ constexpr int kStartupWait = 2 * 20000;
+#endif
+ constexpr int kSubsampling = 1;
+ constexpr int kPoints = 5000;
+ constexpr int kSamplingEnd = kStartupWait + kPoints * kSubsampling;
+ (void)kSamplingEnd;
+ static int j = 0;
+ static int16_t data[kPoints][11];
+ static int written = 0;
+ static_assert((kStartupWait % kSubsampling) == 0, "foo");
+ static_assert((kPoints % kSubsampling) == 0, "foo");
+ if (j < kStartupWait) {
+ // Wait to be started up.
+ ++j;
+#define SAMPLE_UNTIL_DONE 0
+#if !SAMPLE_UNTIL_DONE
+ } else if (j < kSamplingEnd && (j % kSubsampling) == 0) {
+#else
+ } else if (!done) {
+#endif
+ {
+ const int index = ((j - kStartupWait) / kSubsampling) % kPoints;
+ auto point = data[index];
+#if 0
+ point[0] = adc_readings.motor_currents[0][0];
+ point[1] = adc_readings.motor_currents[1][0];
+ point[2] = adc_readings.motor_currents[2][0];
+ point[3] = adc_readings.motor_currents[0][1];
+ point[4] = adc_readings.motor_currents[1][1];
+ point[5] = adc_readings.motor_currents[2][1];
+#else
+ point[0] = balanced.readings[0] / 2;
+ point[1] = balanced.readings[1] / 2;
+ point[2] = balanced.readings[2] / 2;
+#if 1
+ point[3] = controls_->Debug(0);
+ point[4] = controls_->Debug(1);
+ point[5] = controls_->Debug(2);
+ point[6] = controls_->Debug(3);
+ point[7] = controls_->Debug(4);
+ point[8] = controls_->Debug(5);
+ point[9] = controls_->Debug(6);
+ point[10] = controls_->Debug(7);
+#else
+ point[3] = adc_readings.motor_currents[0][0];
+ point[4] = adc_readings.motor_currents[1][0];
+ point[5] = adc_readings.motor_currents[2][0];
+ point[6] = adc_readings.motor_currents[0][1];
+ point[7] = adc_readings.motor_currents[1][1];
+ point[8] = adc_readings.motor_currents[2][1];
+ point[9] = temp1;
+ point[10] = temp2;
+#endif
+ (void)temp1;
+ (void)temp2;
+#if 0
+ point[3] = pwm_ftm_->C1V - pwm_ftm_->C0V;
+ point[4] = pwm_ftm_->C3V - pwm_ftm_->C2V;
+ point[5] = pwm_ftm_->C5V - pwm_ftm_->C4V;
+#endif
+#endif
+#if 0
+ point[6] = adc_readings.motor_current_ref;
+ point[7] = adc_readings.input_voltage;
+#else
+#endif
+ }
+
+#define DO_STEP_RESPONSE 0
+#if DO_STEP_RESPONSE
+ // Step response
+ if (j > 25000) {
+ pwm_ftm_->C1V = 20;
+ }
+#define DO_PULSE_SWEEP 0
+#elif DO_PULSE_SWEEP
+ // Sweep the pulse through the ADC sampling points.
+ static constexpr int kMax = 2500;
+ static constexpr int kExtraWait = 1500;
+ if (j > kStartupWait && j < kStartupWait + kExtraWait) {
+ pwm_ftm_->C4V = 0;
+ pwm_ftm_->C5V = 60;
+ } else if (j < kStartupWait + kMax + kExtraWait) {
+ uint32_t start = j - kStartupWait - kExtraWait;
+ pwm_ftm_->C4V = start;
+ pwm_ftm_->C5V = start + 60;
+ } else {
+ pwm_ftm_->C4V = 0;
+ pwm_ftm_->C5V = 0;
+ }
+#endif
+
+ ++j;
+#if !SAMPLE_UNTIL_DONE
+ } else if (j < kSamplingEnd) {
+ ++j;
+ } else if (j == kSamplingEnd) {
+#else
+ } else if (false) {
+#endif
+ printf("finished\n");
+ ++j;
+#if !SAMPLE_UNTIL_DONE
+ } else {
+#else
+ } else if (done) {
+#endif
+ // Time to write the data out.
+ if (written < (int)sizeof(data)) {
+ int to_write = sizeof(data) - written;
+#if 1
+ if (to_write > 20) {
+ to_write = 20;
+ }
+ // TODO(Brian): Fix usb_serial_write so we can write more than 1 byte at a
+ // time.
+ if (to_write > 1) {
+ to_write = 1;
+ }
+ int result =
+ usb_serial_write(1, ((const char *)data) + written, to_write);
+#else
+ if (to_write > 8) {
+ to_write = 8;
+ }
+ int result =
+ can_send(97, ((const unsigned char *)data) + written, to_write);
+#endif
+ if (result >= 0) {
+ written += to_write;
+ } else {
+ //printf("error\n");
+ }
+ if (written == (int)sizeof(data)) {
+ printf("done writing %d\n", written);
+ }
+ }
+ }
+#endif
+ (void)adc_readings;
+ (void)start_count;
+ (void)balanced;
+
+ // Tell the hardware to use the new switching points.
+ pwm_ftm_->PWMLOAD = FTM_PWMLOAD_LDOK;
+
+ // If another cycle has already started, turn the light on right now.
+ if (pwm_ftm_->SC & FTM_SC_TOF) {
+ GPIOC_PSOR = 1 << 5;
+ }
+}
+
+} // namespace salsa
+} // namespace frc971
diff --git a/motors/motor.h b/motors/motor.h
new file mode 100644
index 0000000..f5b731f
--- /dev/null
+++ b/motors/motor.h
@@ -0,0 +1,111 @@
+#ifndef MOTORS_MOTOR_H_
+#define MOTORS_MOTOR_H_
+
+#include <limits.h>
+
+#include <array>
+
+#include "motors/algorithms.h"
+#include "motors/core/kinetis.h"
+#include "motors/peripheral/adc.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace salsa {
+
+class MotorControls {
+ public:
+ MotorControls() = default;
+ virtual ~MotorControls() = default;
+
+ MotorControls(const MotorControls &) = delete;
+ void operator=(const MotorControls &) = delete;
+
+ static constexpr float scale_current_reading(float reading) {
+ return reading *
+ static_cast<float>(1.0 / 4096.0 /* Full-scale ADC reading */ *
+ 3.3 /* ADC reference voltage */ /
+ (1.47 / (0.768 + 1.47)) /* 5V -> 3.3V divider */ /
+ 50.0 /* Current sense amplification */ /
+ 0.0003 /* Sense resistor */);
+ }
+
+ static constexpr int counts_per_revolution() { return 2048 / 2; }
+
+ // raw_currents are in amps for each phase.
+ // theta is in electrical counts, which will be less than
+ // counts_per_revolution().
+ virtual ::std::array<uint32_t, 3> DoIteration(
+ const float raw_currents[3], uint32_t theta,
+ const float command_current) = 0;
+
+ virtual int16_t Debug(uint32_t theta) = 0;
+
+ virtual float estimated_velocity() const = 0;
+};
+
+// Controls a single motor.
+class Motor {
+ public:
+ // pwm_ftm is used to drive the PWM outputs.
+ // encoder_ftm is used for reading the encoder.
+ Motor(BigFTM *pwm_ftm, LittleFTM *encoder_ftm, MotorControls *controls);
+
+ Motor(const Motor &) = delete;
+ void operator=(const Motor &) = delete;
+
+ // Sets up everything but doesn't actually start the timers.
+ //
+ // This assumes the global time base configuration happens outside so the
+ // timers for both motors (if applicable) are synced up.
+ // TODO(Brian): "40.4.28.1 Enabling the global time base (GTB)" says something
+ // about needing to do stuff in a specific order, so this API might need
+ // revising.
+ void Init();
+
+ // Zeros the encoder. This involves blocking for an arbitrary length of time
+ // with interrupts disabled.
+ void Zero();
+
+ // Starts the timers.
+ void Start();
+
+ void HandleInterrupt();
+
+ private:
+ // Represents the ADC reading which is closest to an edge.
+ struct CloseAdcReading {
+ // Adds a new reading to the readings to balance or pushes the previous
+ // closest one there and saves off this one.
+ //
+ // Returns true if it saves off the new reading.
+ bool MaybeUse(int new_distance, const MediumAdcReadings &adc_readings,
+ int phase, int sample, ReadingsToBalance *to_balance) {
+ if (new_distance < distance) {
+ if (distance != INT_MAX) {
+ to_balance->Add(index, value);
+ }
+ distance = new_distance;
+ value = adc_readings.motor_currents[phase][sample];
+ index = phase;
+ return true;
+ }
+ return false;
+ }
+
+ int distance = INT_MAX;
+ int32_t value = 0;
+ int index = 0;
+ };
+
+ bool flip_time_offset_ = false;
+
+ BigFTM *const pwm_ftm_;
+ LittleFTM *const encoder_ftm_;
+ MotorControls *const controls_;
+};
+
+} // namespace salsa
+} // namespace frc971
+
+#endif // MOTORS_MOTOR_H_
diff --git a/motors/motor_controls.cc b/motors/motor_controls.cc
new file mode 100644
index 0000000..187a781
--- /dev/null
+++ b/motors/motor_controls.cc
@@ -0,0 +1,240 @@
+#include "motors/motor_controls.h"
+
+#include "motors/peripheral/configuration.h"
+
+namespace frc971 {
+namespace salsa {
+namespace {
+
+template <int kRows, int kCols>
+using ComplexMatrix = MotorControlsImplementation::ComplexMatrix<kRows, kCols>;
+
+using Complex = ::std::complex<float>;
+
+constexpr int kCountsPerRevolution = MotorControls::counts_per_revolution();
+
+#if 1
+constexpr double kMaxDutyCycle = 0.98;
+#elif 1
+constexpr double kMaxDutyCycle = 0.6;
+#elif 0
+constexpr double kMaxDutyCycle = 0.2;
+#endif
+
+constexpr int kPhaseBOffset = kCountsPerRevolution / 3;
+constexpr int kPhaseCOffset = 2 * kCountsPerRevolution / 3;
+
+constexpr double K1_unscaled = 1.81e04;
+constexpr double K2_unscaled = -2.65e03;
+
+// Make the amplitude of the fundamental 1 for ease of math.
+constexpr double K2 = K2_unscaled / K1_unscaled;
+constexpr double K1 = 1;
+
+// volts
+constexpr double vcc = 30.0;
+
+constexpr double Kv = 22000.0 * 2.0 * M_PI / 60.0 / 30.0 * 2.0;
+
+// Henries
+constexpr double L = 1e-05;
+
+constexpr double M = 1e-06;
+
+// ohms for system
+constexpr double R = 0.008;
+
+::Eigen::Matrix<float, 3, 3> A_discrete() {
+ ::Eigen::Matrix<float, 3, 3> r;
+ static constexpr float kDiagonal = 0.960091f;
+ static constexpr float kOffDiagonal = 0.00356245f;
+ r << kDiagonal, kOffDiagonal, kOffDiagonal, kOffDiagonal, kDiagonal,
+ kOffDiagonal, kOffDiagonal, kOffDiagonal, kDiagonal;
+ return r;
+}
+
+::Eigen::Matrix<float, 3, 3> B_discrete_inverse() {
+ return ::Eigen::Matrix<float, 1, 3>::Constant(0.18403f).asDiagonal();
+}
+
+// The number to divide the product of the unit BEMF and the per phase current
+// by to get motor current.
+constexpr double kOneAmpScalar = 1.46785;
+
+constexpr double kMaxOneAmpDrivingVoltage = 0.0361525;
+
+// Use FluxLinkageTable() to access this with a const so you don't accidentally
+// modify it.
+float flux_linkage_table[kCountsPerRevolution];
+
+void MakeFluxLinkageTable() {
+ for (int i = 0; i < kCountsPerRevolution; ++i) {
+ const double theta = static_cast<double>(i) /
+ static_cast<double>(kCountsPerRevolution) * 2.0 * M_PI;
+ flux_linkage_table[i] = K1 * sin(theta) - K2 * sin(theta * 5);
+ }
+}
+
+// theta doesn't have to be less than kCountsPerRevolution.
+::Eigen::Matrix<float, 3, 1> FluxLinkageAt(uint32_t theta) {
+ ::Eigen::Matrix<float, 3, 1> r;
+ r(0) = flux_linkage_table[theta % kCountsPerRevolution];
+ r(1) = flux_linkage_table[(theta + kPhaseBOffset) % kCountsPerRevolution];
+ r(2) = flux_linkage_table[(theta + kPhaseCOffset) % kCountsPerRevolution];
+ return r;
+}
+
+::Eigen::Matrix<float, 3, 3> MakeK() {
+ ::Eigen::Matrix<float, 3, 3> Vconv;
+ Vconv << 2.0f, -1.0f, -1.0f, -1.0f, 2.0f, -1.0f, -1.0f, -1.0f, 2.0f;
+ static constexpr float kControllerGain = 0.07f;
+ return kControllerGain * (Vconv / 3.0f);
+}
+
+ComplexMatrix<3, 1> MakeE1Unrotated() {
+ ComplexMatrix<3, 1> rotation;
+ rotation << Complex(0, -1), Complex(::std::sqrt(3) / 2, 0.5),
+ Complex(-::std::sqrt(3) / 2, 0.5);
+ return K1 * rotation;
+}
+
+ComplexMatrix<3, 1> MakeE2Unrotated() {
+ ComplexMatrix<3, 1> rotation;
+ rotation << Complex(0, -1), Complex(-::std::sqrt(3) / 2, 0.5),
+ Complex(::std::sqrt(3) / 2, 0.5);
+ return K2 * rotation;
+}
+
+ComplexMatrix<3, 3> Hn(float omega, int scalar) {
+ const Complex a(static_cast<float>(R),
+ omega * static_cast<float>(scalar * L));
+ const Complex b(0, omega * static_cast<float>(scalar * M));
+ const Complex temp1 = a + b;
+ const Complex temp2 = -b;
+ ComplexMatrix<3, 3> matrix;
+ matrix << temp1, temp2, temp2, temp2, temp1, temp2, temp2, temp2, temp1;
+ return matrix *
+ -(omega / static_cast<float>(Kv) / (a * a + a * b - 2.0f * b * b));
+}
+
+} // namespace
+
+MotorControlsImplementation::MotorControlsImplementation()
+ : E1Unrotated_(MakeE1Unrotated()), E2Unrotated_(MakeE2Unrotated()) {
+ MakeFluxLinkageTable();
+}
+
+::std::array<uint32_t, 3> MotorControlsImplementation::DoIteration(
+ const float raw_currents[3], const uint32_t theta_in,
+ const float command_current) {
+ static constexpr float kCurrentSlewRate = 0.05f;
+ if (command_current > filtered_current_ + kCurrentSlewRate) {
+ filtered_current_ += kCurrentSlewRate;
+ } else if (command_current < filtered_current_ - kCurrentSlewRate) {
+ filtered_current_ -= kCurrentSlewRate;
+ } else {
+ filtered_current_ = command_current;
+ }
+ const float goal_current_in = filtered_current_;
+ const float max_current =
+ (static_cast<float>(vcc * kMaxDutyCycle) -
+ estimated_velocity_ / static_cast<float>(Kv / 2.0)) /
+ static_cast<float>(kMaxOneAmpDrivingVoltage);
+ const float min_current =
+ (-static_cast<float>(vcc * kMaxDutyCycle) -
+ estimated_velocity_ / static_cast<float>(Kv / 2.0)) /
+ static_cast<float>(kMaxOneAmpDrivingVoltage);
+ const float goal_current =
+ ::std::max(min_current, ::std::min(max_current, goal_current_in));
+
+#if 0
+ const uint32_t theta =
+ (theta_in + static_cast<uint32_t>(estimated_velocity_ * 1.0f)) % 1024;
+#elif 0
+ const uint32_t theta =
+ (theta_in + kCountsPerRevolution - 160u) % kCountsPerRevolution;
+#elif 1
+ const uint32_t theta =
+ (theta_in + kCountsPerRevolution +
+ ((estimated_velocity_ > 0) ? (kCountsPerRevolution - 10u) : 60u)) %
+ kCountsPerRevolution;
+#elif 0
+ const uint32_t theta = theta_in;
+#endif
+
+ const ::Eigen::Matrix<float, 3, 1> measured_current =
+ (::Eigen::Matrix<float, 3, 1>() << scale_current_reading(raw_currents[0]),
+ scale_current_reading(raw_currents[1]),
+ scale_current_reading(raw_currents[2])).finished();
+
+ const ComplexMatrix<3, 1> E1 =
+ E1Unrotated_ *
+ ImaginaryExpInt<::std::ratio<1, counts_per_revolution()>>(theta);
+ const ComplexMatrix<3, 1> E2 =
+ E2Unrotated_ *
+ ImaginaryExpInt<::std::ratio<5, counts_per_revolution()>>(theta);
+
+ const float overall_measured_current =
+ ((E1 + E2).real().transpose() * measured_current /
+ static_cast<float>(kOneAmpScalar))(0);
+ const float current_error = goal_current - overall_measured_current;
+ estimated_velocity_ += current_error * 0.1f;
+ debug_[3] = theta;
+ const float omega = estimated_velocity_;
+
+ debug_[4] = max_current * 10;
+
+ const ::Eigen::Matrix<float, 3, 1> I_now = I_last_;
+ const ::Eigen::Matrix<float, 3, 1> I_next =
+ FluxLinkageAt(theta) * goal_current;
+
+ const ComplexMatrix<3, 3> H1 = Hn(omega, 1);
+ const ComplexMatrix<3, 3> H2 = Hn(omega, 5);
+
+ const ComplexMatrix<3, 1> p_imaginary = H1 * E1 + H2 * E2;
+ const ComplexMatrix<3, 1> p_next_imaginary =
+ ImaginaryExpFloat(omega / SWITCHING_FREQUENCY) * H1 * E1 +
+ ImaginaryExpFloat(omega * 5 / SWITCHING_FREQUENCY) * H2 * E2;
+ const ::Eigen::Matrix<float, 3, 1> p = p_imaginary.real();
+ const ::Eigen::Matrix<float, 3, 1> p_next = p_next_imaginary.real();
+
+ const ::Eigen::Matrix<float, 3, 1> Vn_ff =
+ B_discrete_inverse() * (I_next - A_discrete() * (I_now - p) - p_next);
+ const ::Eigen::Matrix<float, 3, 1> Vn =
+ Vn_ff + MakeK() * (I_now - measured_current);
+
+ debug_[0] = (I_next)(0) * 100;
+ debug_[1] = (I_next)(1) * 100;
+ debug_[2] = (I_next)(2) * 100;
+
+ debug_[5] = Vn(0) * 100;
+ debug_[6] = Vn(1) * 100;
+ debug_[7] = Vn(2) * 100;
+
+ ::Eigen::Matrix<float, 3, 1> times = Vn / vcc;
+ {
+ const float min_time = times.minCoeff();
+ times -= ::Eigen::Matrix<float, 3, 1>::Constant(min_time);
+ }
+ {
+ const float max_time = times.maxCoeff();
+ const float scalar =
+ static_cast<float>(kMaxDutyCycle) /
+ ::std::max(static_cast<float>(kMaxDutyCycle), max_time);
+ times *= scalar;
+ }
+
+ I_last_ = I_next;
+
+ // TODO(Austin): Figure out why we need the min here.
+ return {static_cast<uint32_t>(::std::max(0.0f, times(0)) * 3000.0f),
+ static_cast<uint32_t>(::std::max(0.0f, times(1)) * 3000.0f),
+ static_cast<uint32_t>(::std::max(0.0f, times(2)) * 3000.0f)};
+}
+
+int16_t MotorControlsImplementation::Debug(uint32_t theta) {
+ return debug_[theta];
+}
+
+} // namespace salsa
+} // namespace frc971
diff --git a/motors/motor_controls.h b/motors/motor_controls.h
new file mode 100644
index 0000000..e0fa28c
--- /dev/null
+++ b/motors/motor_controls.h
@@ -0,0 +1,45 @@
+#ifndef MOTORS_MOTOR_CONTROLS_H_
+#define MOTORS_MOTOR_CONTROLS_H_
+
+#include <array>
+#include <complex>
+
+#include "motors/math.h"
+#include "motors/motor.h"
+
+#include "Eigen/Dense"
+
+namespace frc971 {
+namespace salsa {
+
+class MotorControlsImplementation : public MotorControls {
+ public:
+ template <int kRows, int kCols>
+ using ComplexMatrix = ::Eigen::Matrix<::std::complex<float>, kRows, kCols>;
+
+ MotorControlsImplementation();
+ ~MotorControlsImplementation() override = default;
+
+ ::std::array<uint32_t, 3> DoIteration(const float raw_currents[3],
+ uint32_t theta,
+ const float command_current) override;
+
+ int16_t Debug(uint32_t theta) override;
+
+ float estimated_velocity() const override { return estimated_velocity_; }
+
+ private:
+ const ComplexMatrix<3, 1> E1Unrotated_, E2Unrotated_;
+
+ float estimated_velocity_ = 0;
+ float filtered_current_ = 0;
+
+ ::Eigen::Matrix<float, 3, 1> I_last_ = ::Eigen::Matrix<float, 3, 1>::Zero();
+
+ int16_t debug_[9];
+};
+
+} // namespace salsa
+} // namespace frc971
+
+#endif // MOTORS_MOTOR_CONTROLS_H_
diff --git a/motors/peripheral/BUILD b/motors/peripheral/BUILD
new file mode 100644
index 0000000..10fef28
--- /dev/null
+++ b/motors/peripheral/BUILD
@@ -0,0 +1,43 @@
+load("//tools:environments.bzl", "mcu_cpus")
+
+cc_library(
+ name = 'adc',
+ visibility = ['//visibility:public'],
+ hdrs = [
+ 'adc.h',
+ ],
+ srcs = [
+ 'adc.cc',
+ ],
+ deps = [
+ ':configuration',
+ '//motors:util',
+ '//motors/core',
+ ],
+ restricted_to = mcu_cpus,
+)
+
+cc_library(
+ name = 'configuration',
+ visibility = ['//visibility:public'],
+ hdrs = [
+ 'configuration.h',
+ ],
+ restricted_to = mcu_cpus,
+)
+
+cc_library(
+ name = 'can',
+ visibility = ['//visibility:public'],
+ hdrs = [
+ 'can.h',
+ ],
+ srcs = [
+ 'can.c',
+ ],
+ deps = [
+ '//motors/core',
+ '//motors:util',
+ ],
+ restricted_to = mcu_cpus,
+)
diff --git a/motors/peripheral/adc.cc b/motors/peripheral/adc.cc
new file mode 100644
index 0000000..ba2b247
--- /dev/null
+++ b/motors/peripheral/adc.cc
@@ -0,0 +1,118 @@
+#include "motors/peripheral/adc.h"
+
+#include "motors/core/kinetis.h"
+
+namespace frc971 {
+namespace salsa {
+
+#define ADC_SC2_BASE (ADC_SC2_REFSEL(0) /* Use the external reference pins. */)
+
+#define ADC_FINISH_CALIBRATION(n, PM) \
+ do { \
+ uint16_t variable = 0; \
+ variable += ADC##n##_CL##PM##0; \
+ variable += ADC##n##_CL##PM##1; \
+ variable += ADC##n##_CL##PM##2; \
+ variable += ADC##n##_CL##PM##3; \
+ variable += ADC##n##_CL##PM##4; \
+ variable += ADC##n##_CL##PM##S; \
+ variable /= 2; \
+ variable |= 0x8000; \
+ ADC##n##_##PM##G = variable; \
+ } while (0);
+
+#define ADC_INIT_SINGLE(n) \
+ do { \
+ ADC##n##_CFG1 = ADC_CFG1_ADIV(2) /* Divide clock by 4 to get 15MHz. */ | \
+ ADC_CFG1_MODE(1) /* 12 bit mode. */ | \
+ ADC_CFG1_ADICLK(0) /* Use the bus clock (60MHz). */; \
+ ADC##n##_CFG2 = ADC_CFG2_MUXSEL /* Use the b channels. */ | \
+ ADC_CFG2_ADHSC /* Support higher ADC clock speeds. */; \
+ ADC##n##_SC1A = 0; /* Clear SC1A's COCO flag. */ \
+ ADC##n##_SC2 = ADC_SC2_BASE; \
+ do { \
+ ADC##n##_SC3 = ADC_SC3_CAL | ADC_SC3_AVGE | \
+ ADC_SC3_AVGS(3) /* Average 32 samples (max). */; \
+ /* Wait for calibration to complete. */ \
+ while (!(ADC##n##_SC1A & ADC_SC1_COCO)) { \
+ } \
+ } while (ADC##n##_SC3 & ADC_SC3_CALF); \
+ ADC_FINISH_CALIBRATION(n, P); \
+ ADC_FINISH_CALIBRATION(n, M); \
+ \
+ ADC##n##_SC3 = 0 /* Disable hardware averaging. */; \
+ } while (0)
+
+void AdcInit() {
+ SIM_SCGC3 |= SIM_SCGC3_ADC1;
+ SIM_SCGC6 |= SIM_SCGC6_ADC0;
+ // TODO(Brian): Mess with SIM_SOPT7 to reconfigure ADC trigger input source?
+ ADC_INIT_SINGLE(0);
+ ADC_INIT_SINGLE(1);
+
+ // M_CH2V/M1_CH2F ADC0_SE14
+ PORTC_PCR0 = PORT_PCR_MUX(0);
+
+ // M_CH0V/M1_CH0F ADC0_SE13
+ PORTB_PCR3 = PORT_PCR_MUX(0);
+
+ // M_CH1V/M1_CH1F ADC0_SE12
+ PORTB_PCR2 = PORT_PCR_MUX(0);
+
+ // M0_CH0F/M_CH0F ADC1_SE14
+ PORTB_PCR10 = PORT_PCR_MUX(0);
+
+ // M0_CH1F/M_CH1F ADC1_SE15
+ PORTB_PCR11 = PORT_PCR_MUX(0);
+
+ // WHEEL_ABS/M_VREF ADC0_SE18
+ PORTE_PCR25 = PORT_PCR_MUX(0);
+
+ // VIN/VIN ADC1_SE5B
+ PORTC_PCR9 = PORT_PCR_MUX(0);
+
+ // M0_CH2F/M_CH2F ADC1_SE17
+ PORTA_PCR17 = PORT_PCR_MUX(0);
+}
+
+MediumAdcReadings AdcReadMedium() {
+ MediumAdcReadings r;
+
+ ADC1_SC1A = 14;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 15;
+ r.motor_currents[0][0] = ADC1_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 17;
+ ADC0_SC1A = 18;
+ r.motor_currents[1][0] = ADC1_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 5;
+ r.motor_currents[2][0] = ADC1_RA;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ r.motor_current_ref = ADC0_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 14;
+ r.input_voltage = ADC1_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 15;
+ r.motor_currents[0][1] = ADC1_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 17;
+ r.motor_currents[1][1] = ADC1_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ r.motor_currents[2][1] = ADC1_RA;
+
+ return r;
+}
+
+} // namespace salsa
+} // namespace frc971
diff --git a/motors/peripheral/adc.h b/motors/peripheral/adc.h
new file mode 100644
index 0000000..4aee769
--- /dev/null
+++ b/motors/peripheral/adc.h
@@ -0,0 +1,22 @@
+#ifndef MOTORS_PERIPHERAL_ADC_H_
+#define MOTORS_PERIPHERAL_ADC_H_
+
+#include <stdint.h>
+
+namespace frc971 {
+namespace salsa {
+
+struct MediumAdcReadings {
+ uint16_t motor_currents[3][2];
+ uint16_t motor_current_ref;
+ uint16_t input_voltage;
+};
+
+void AdcInit();
+
+MediumAdcReadings AdcReadMedium();
+
+} // namespace salsa
+} // namespace frc971
+
+#endif // MOTORS_PERIPHERAL_ADC_H_
diff --git a/motors/peripheral/can.c b/motors/peripheral/can.c
new file mode 100644
index 0000000..7187ac9
--- /dev/null
+++ b/motors/peripheral/can.c
@@ -0,0 +1,189 @@
+#include "motors/peripheral/can.h"
+
+#include <stddef.h>
+#include <string.h>
+
+#include "motors/core/kinetis.h"
+#include "motors/util.h"
+
+#include <stdio.h>
+#include <inttypes.h>
+
+// General note: this peripheral is really weird about accessing its memory. It
+// goes much farther than normal memory-mapped device semantics. In particular,
+// it "locks" various regions of memory under complicated conditions. Because of
+// this, all the code in here touching the device memory is fairly paranoid
+// about how it does that.
+
+// The number of message buffers we're actually going to use. The chip only has
+// 16. Using fewer means less for the CAN module (and CPU) to go through looking
+// for actual data.
+// 0 is for sending and 1 is for receiving commands.
+#define NUMBER_MESSAGE_BUFFERS 2
+
+#if NUMBER_MESSAGE_BUFFERS > 16
+#error Only have 16 message buffers on this part.
+#endif
+
+// TODO(Brian): Do something about CAN errors and warnings (enable interrupts?).
+
+// Flags for the interrupt to process which don't actually come from the
+// hardware. Currently, only used for tx buffers.
+static volatile uint32_t can_manual_flags = 0;
+
+void can_init(void) {
+ printf("can_init\n");
+ PORTB_PCR18 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+
+ SIM_SCGC6 |= SIM_SCGC6_FLEXCAN0;
+
+ // Put it into freeze mode and wait for it to actually do that.
+ // Don't OR these bits in because it starts in module-disable mode, which
+ // isn't what we want. It will ignore the attempt to change some of the bits
+ // because it's not in freeze mode, but whatever.
+ CAN0_MCR = CAN_MCR_FRZ | CAN_MCR_HALT;
+ while (!(CAN0_MCR & CAN_MCR_FRZACK)) {}
+
+ // Initializing this before touching the mailboxes because the reference
+ // manual slightly implies you have to, and the registers and RAM on this
+ // thing are weird (get locked sometimes) so it actually might matter.
+ CAN0_MCR =
+ CAN_MCR_FRZ | CAN_MCR_HALT /* Stay in freeze mode. */ |
+ CAN_MCR_SRXDIS /* Don't want to see our own frames at all. */ |
+ CAN_MCR_IRMQ /* Use individual masks for each filter. */ |
+ CAN_MCR_LPRIOEN /* Let us prioritize TX mailboxes. */ |
+ (0 << 8) /* No need to pack IDs tightly, so it's easier not to. */ |
+ (NUMBER_MESSAGE_BUFFERS - 1);
+
+ // Initialize all the buffers and RX filters we're enabling.
+
+ // Just in case this does anything...
+ CAN0_RXIMRS[0] = 0;
+ CAN0_MESSAGES[0].prio_id = 0;
+ CAN0_MESSAGES[0].control_timestamp =
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_TX_INACTIVE) | CAN_MB_CONTROL_IDE;
+
+ CAN0_RXIMRS[1] = (1 << 31) /* Want to filter out RTRs. */ |
+ (1 << 30) /* Want to only get extended frames. */ |
+ 0xFF /* Filter on the 1-byte VESC id. */;
+ CAN0_MESSAGES[1].prio_id = 0;
+ CAN0_MESSAGES[1].control_timestamp =
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_RX_EMPTY) | CAN_MB_CONTROL_IDE;
+
+ // Using the oscillator clock directly because it's a reasonable frequency and
+ // more stable than the PLL-based peripheral clock, which matters.
+ // We're going with a sample point fraction of 0.875 because that's what
+ // SocketCAN defaults to.
+ CAN0_CTRL1 = CAN_CTRL1_PRESDIV(
+ 1) /* Divide the crystal frequency by 2 to get 8 MHz. */ |
+ CAN_CTRL1_RJW(0) /* RJW/SJW of 1, which is most common. */ |
+ CAN_CTRL1_PSEG1(7) /* 8 time quanta before sampling. */ |
+ CAN_CTRL1_PSEG2(1) /* 2 time quanta after sampling. */ |
+ CAN_CTRL1_SMP /* Use triple sampling. */ |
+ CAN_CTRL1_PROPSEG(4) /* 5 time quanta before sampling. */;
+ CAN0_CTRL2 = CAN_CTRL2_TASD(25) /* We have so few mailboxes and */
+ /* such a fast peripheral clock, this has lots of margin. */ |
+ CAN_CTRL2_EACEN /* Match on IDE and RTR. */;
+
+ // Enable interrupts for the RX mailbox.
+ CAN0_IMASK1 = 1 << 1;
+
+ // Now take it out of freeze mode.
+ CAN0_MCR &= ~CAN_MCR_HALT;
+
+ //NVIC_ENABLE_IRQ(IRQ_CAN_MESSAGE);
+}
+
+static void can_vesc_process_rx(volatile CanMessageBuffer *buffer,
+ unsigned char *data_out, int *length_out) {
+ // Wait until the buffer is marked as not being busy. The reference manual
+ // says to do this, although it's unclear how we could get an interrupt
+ // asserted while it's still busy. Maybe if the interrupt was slow and now
+ // it's being overwritten?
+ uint32_t control_timestamp;
+ do {
+ control_timestamp = buffer->control_timestamp;
+ } while (control_timestamp & CAN_MB_CONTROL_CODE_BUSY_MASK);
+ // The message buffer is now locked, so it won't be modified by the hardware.
+
+ const uint32_t prio_id = buffer->prio_id;
+ // Making sure to access the data 32 bits at a time, copy it out. It's
+ // ambiguous whether you're allowed to access the individual bytes, and this
+ // memory is weird enough to not make sense risking it. Also, it's only 2
+ // cycles, which is pretty hard to beat by doing anything with the length...
+ // Also, surprise!: the hardware stores the data big-endian.
+ uint32_t data[2];
+ data[0] = __builtin_bswap32(buffer->data[0]);
+ data[1] = __builtin_bswap32(buffer->data[1]);
+
+ // Yes, it might actually matter that we clear the interrupt flag before
+ // unlocking it...
+ CAN0_IFLAG1 = 1 << (buffer - CAN0_MESSAGES);
+
+ // Now read the timer to unlock the message buffer. Want to do this ASAP
+ // rather than waiting until we get to processing the next buffer, plus we
+ // might want to write to the next one, which results in weird, bad things.
+ {
+ uint16_t dummy = CAN0_TIMER;
+ (void)dummy;
+ }
+
+ // The message buffer is now unlocked and "serviced", but its control word
+ // code is still CAN_MB_CODE_RX_FULL. However, said code will stay
+ // CAN_MB_CODE_RX_FULL the next time a message is received into it (the code
+ // won't change to CAN_MB_CODE_RX_OVERRUN because it has been "serviced").
+ // Yes, really...
+
+ memcpy(data_out, data, 8);
+ *length_out = CAN_MB_CONTROL_EXTRACT_DLC(control_timestamp);
+ (void)prio_id;
+}
+
+int can_send(uint32_t can_id, const unsigned char *data, unsigned int length) {
+ volatile CanMessageBuffer *const message_buffer = &CAN0_MESSAGES[0];
+
+ if (CAN_MB_CONTROL_EXTRACT_CODE(message_buffer->control_timestamp) ==
+ CAN_MB_CODE_TX_DATA) {
+ return -1;
+ }
+
+ // Yes, it might actually matter that we clear the interrupt flag before
+ // doing stuff...
+ CAN0_IFLAG1 = 1 << (message_buffer - CAN0_MESSAGES);
+ message_buffer->prio_id = can_id;
+ // Copy only the bytes from data that we're supposed to onto the stack, and
+ // then move it into the message buffer 32 bits at a time (because it might
+ // get unhappy about writing individual bytes). Plus, we have to byte-swap
+ // each 32-bit word because this hardware is weird...
+ {
+ uint32_t data_words[2] = {0, 0};
+ for (uint8_t *dest = (uint8_t *)&data_words[0];
+ dest - (uint8_t *)&data_words[0] < (ptrdiff_t)length; ++dest) {
+ *dest = *data;
+ ++data;
+ }
+ message_buffer->data[0] = __builtin_bswap32(data_words[0]);
+ message_buffer->data[1] = __builtin_bswap32(data_words[1]);
+ }
+ message_buffer->control_timestamp =
+ CAN_MB_CONTROL_INSERT_DLC(length) | CAN_MB_CONTROL_SRR |
+ CAN_MB_CONTROL_IDE | CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_TX_DATA);
+ return 0;
+}
+
+void can_receive_command(unsigned char *data, int *length) {
+ if (0) {
+ static int i = 0;
+ if (i++ == 13) {
+ printf("IFLAG1=%" PRIx32 " ESR=%" PRIx32 " ESR1=%" PRIx32 "\n",
+ CAN0_IFLAG1, CAN0_ECR, CAN0_ESR1);
+ i = 0;
+ }
+ }
+ if ((CAN0_IFLAG1 & (1 << 1)) == 0) {
+ *length = -1;
+ return;
+ }
+ can_vesc_process_rx(&CAN0_MESSAGES[1], data, length);
+}
diff --git a/motors/peripheral/can.h b/motors/peripheral/can.h
new file mode 100644
index 0000000..acd6cce
--- /dev/null
+++ b/motors/peripheral/can.h
@@ -0,0 +1,24 @@
+#ifndef PERIPHERAL_CAN_H_
+#define PERIPHERAL_CAN_H_
+
+#include <stdint.h>
+
+// The code defined here calls functions in vesc/vesc_can.h from various
+// interrupts and expects them to call back into this file to do something.
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void can_init(void);
+
+int can_send(uint32_t can_id, const unsigned char *data, unsigned int length);
+
+// Sets *length to -1 if there isn't a new piece of data to receive.
+void can_receive_command(unsigned char *data, int *length);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // PERIPHERAL_CAN_H_
diff --git a/motors/peripheral/configuration.h b/motors/peripheral/configuration.h
new file mode 100644
index 0000000..3fa686a
--- /dev/null
+++ b/motors/peripheral/configuration.h
@@ -0,0 +1,24 @@
+#ifndef MOTORS_PERIPHERAL_CONFIGURATION_H_
+#define MOTORS_PERIPHERAL_CONFIGURATION_H_
+
+// We're just going to leave the default DMA priorities which correspond to the
+// channel numbers and fixed priority mode, so channel 0 is the lowest priority
+// and 15 is the highest.
+// We're also going to leave DMA_CR alone except for setting EMLM.
+
+// The frequency of the peripheral bus(es) in hz.
+#define BUS_CLOCK_FREQUENCY (F_CPU / 2)
+
+// The frequency we switch the motor FETs at in hz.
+#define SWITCHING_FREQUENCY 20000
+
+#if 0
+// Which PDB the ADC triggering uses.
+#define ADC_TRIGGER_PDB 0
+// The DMA channel which copies ADC results.
+#define ADC_RESULT_DMA_CHANNEL 7
+// The DMA channel which reconfigures the ADCs to take the next samples.
+#define ADC_RECONFIGURE_DMA_CHANNEL 8
+#endif
+
+#endif // MOTORS_PERIPHERAL_CONFIGURATION_H_
diff --git a/motors/plot.py b/motors/plot.py
new file mode 100755
index 0000000..013f308
--- /dev/null
+++ b/motors/plot.py
@@ -0,0 +1,25 @@
+#!/usr/bin/python3
+
+import numpy
+from matplotlib import pylab
+
+data = numpy.loadtxt('/tmp/dump3.csv',
+ delimiter=',',
+ skiprows=1)
+x = range(len(data))
+
+pylab.subplot(1, 1, 1)
+pylab.plot(x, [d[0] for d in data], 'ro', label='ia')
+pylab.plot(x, [d[1] for d in data], 'go', label='ib')
+pylab.plot(x, [d[2] for d in data], 'bo', label='ic')
+pylab.plot(x, [d[3] for d in data], 'r--', label='ia_goal')
+pylab.plot(x, [d[4] for d in data], 'g--', label='ib_goal')
+pylab.plot(x, [d[5] for d in data], 'b--', label='ic_goal')
+pylab.plot(x, [d[6] for d in data], 'rx', label='i_overall')
+pylab.plot(x, [d[7] for d in data], 'gx', label='omega')
+pylab.plot(x, [d[8] for d in data], 'r', label='van')
+pylab.plot(x, [d[9] for d in data], 'g', label='vbn')
+pylab.plot(x, [d[10] for d in data], 'b', label='vcn')
+pylab.legend()
+
+pylab.show()
diff --git a/motors/usb/usb_serial.h b/motors/usb/usb_serial.h
index 2512adc..40e7c36 100644
--- a/motors/usb/usb_serial.h
+++ b/motors/usb/usb_serial.h
@@ -56,7 +56,7 @@
// Drops any unread input until the most recent packet sent.
void usb_serial_flush_input(int port);
-// Writes data. Returns -1 if it times out or size if it succeeds.
+// Writes data. Returns -1 if it times out or 0 if it succeeds.
//
// NOTE: This does not send immediately. The data is buffered
int usb_serial_write(int port, const void *buffer, uint32_t size);
diff --git a/motors/util.h b/motors/util.h
index e4a0271..d1eaf53 100644
--- a/motors/util.h
+++ b/motors/util.h
@@ -1,6 +1,11 @@
#ifndef MOTORS_UTIL_H_
#define MOTORS_UTIL_H_
+#include <stdint.h>
+#include <stddef.h>
+
+#include "motors/core/kinetis.h"
+
#ifdef __cplusplus
extern "C"
{
@@ -97,6 +102,7 @@
#define CAN_MB_CONTROL_IDE ((uint32_t)(1 << 21))
#define CAN_MB_CONTROL_SRR ((uint32_t)(1 << 22))
#define CAN_MB_CONTROL_INSERT_CODE(n) ((uint32_t)(((n) & 0xF) << 24))
+#define CAN_MB_CONTROL_EXTRACT_CODE(n) ((uint32_t)(((n) >> 24) & 0xF))
#define CAN_MB_CONTROL_CODE_BUSY_MASK CAN_MB_CONTROL_INSERT_CODE(1)
#define CAN_MB_PRIO_ID_PRIORITY_MASK ((uint32_t)((1 << 29) - 1))
#define CAN_MB_CODE_RX_INACTIVE 0
@@ -161,6 +167,116 @@
};
#endif // __cplusplus
+typedef struct {
+ uint32_t saddr;
+ uint16_t soff;
+ uint16_t attr;
+ uint32_t nbytes_mlno;
+ uint32_t slast;
+ uint32_t daddr;
+ uint16_t doff;
+ uint16_t citer;
+ uint32_t dlastsga;
+ uint32_t _reserved;
+} DmaTcd __attribute__((aligned(0x20)));
+#ifdef __cplusplus
+static_assert(sizeof(DmaTcd) == 0x20, "DMA TCD is the wrong size");
+#endif
+
+#define ALL_FTM_REGISTERS \
+ FOR_BOTH_FTM_REGISTER(SC) \
+ FOR_BOTH_FTM_REGISTER(CNT) \
+ FOR_BOTH_FTM_REGISTER(MOD) \
+ FOR_BOTH_FTM_REGISTER(C0SC) \
+ FOR_BOTH_FTM_REGISTER(C0V) \
+ FOR_BOTH_FTM_REGISTER(C1SC) \
+ FOR_BOTH_FTM_REGISTER(C1V) \
+ FOR_BIG_FTM_REGISTER(C2SC) \
+ FOR_BIG_FTM_REGISTER(C2V) \
+ FOR_BIG_FTM_REGISTER(C3SC) \
+ FOR_BIG_FTM_REGISTER(C3V) \
+ FOR_BIG_FTM_REGISTER(C4SC) \
+ FOR_BIG_FTM_REGISTER(C4V) \
+ FOR_BIG_FTM_REGISTER(C5SC) \
+ FOR_BIG_FTM_REGISTER(C5V) \
+ FOR_BIG_FTM_REGISTER(C6SC) \
+ FOR_BIG_FTM_REGISTER(C6V) \
+ FOR_BIG_FTM_REGISTER(C7SC) \
+ FOR_BIG_FTM_REGISTER(C7V) \
+ FOR_BOTH_FTM_REGISTER(CNTIN) \
+ FOR_BOTH_FTM_REGISTER(STATUS) \
+ FOR_BOTH_FTM_REGISTER(MODE) \
+ FOR_BOTH_FTM_REGISTER(SYNC) \
+ FOR_BOTH_FTM_REGISTER(OUTINIT) \
+ FOR_BOTH_FTM_REGISTER(OUTMASK) \
+ FOR_BOTH_FTM_REGISTER(COMBINE) \
+ FOR_BOTH_FTM_REGISTER(DEADTIME) \
+ FOR_BOTH_FTM_REGISTER(EXTTRIG) \
+ FOR_BOTH_FTM_REGISTER(POL) \
+ FOR_BOTH_FTM_REGISTER(FMS) \
+ FOR_BOTH_FTM_REGISTER(FILTER) \
+ FOR_BOTH_FTM_REGISTER(FLTCTRL) \
+ FOR_LITTLE_FTM_REGISTER(QDCTRL) \
+ FOR_BOTH_FTM_REGISTER(CONF) \
+ FOR_BOTH_FTM_REGISTER(FLTPOL) \
+ FOR_BOTH_FTM_REGISTER(SYNCONF) \
+ FOR_BOTH_FTM_REGISTER(INVCTRL) \
+ FOR_BOTH_FTM_REGISTER(SWOCTRL) \
+ FOR_BOTH_FTM_REGISTER(PWMLOAD)
+
+typedef struct {
+#define FOR_BIG_FTM_REGISTER(name) volatile uint32_t name;
+#define FOR_BOTH_FTM_REGISTER(name) volatile uint32_t name;
+#define FOR_LITTLE_FTM_REGISTER(name) const uint32_t _reserved_##name;
+ ALL_FTM_REGISTERS
+#undef FOR_BIG_FTM_REGISTER
+#undef FOR_LITTLE_FTM_REGISTER
+} BigFTM;
+
+typedef struct {
+#define FOR_BIG_FTM_REGISTER(name) const uint32_t _reserved_##name;
+#define FOR_LITTLE_FTM_REGISTER(name) volatile uint32_t name;
+ ALL_FTM_REGISTERS
+#undef FOR_BIG_FTM_REGISTER
+#undef FOR_LITTLE_FTM_REGISTER
+#undef FOR_BOTH_FTM_REGISTER
+} LittleFTM;
+
+#define FTM0 ((BigFTM *)0x40038000)
+#define FTM1 ((LittleFTM *)0x40039000)
+#define FTM2 ((LittleFTM *)0x400B8000)
+#define FTM3 ((BigFTM *)0x400B9000)
+
+#ifdef __cplusplus
+#define FOR_BIG_FTM_REGISTER(name) \
+ static_assert(offsetof(BigFTM, name) == \
+ (reinterpret_cast<const volatile char *>(&FTM0_##name) - \
+ reinterpret_cast<volatile char *>(FTM0)), \
+ #name " is at the wrong place"); \
+ static_assert(offsetof(BigFTM, name) == \
+ (reinterpret_cast<const volatile char *>(&FTM3_##name) - \
+ reinterpret_cast<volatile char *>(FTM3)), \
+ #name " is at the wrong place");
+#define FOR_LITTLE_FTM_REGISTER(name) \
+ static_assert(offsetof(LittleFTM, name) == \
+ (reinterpret_cast<const volatile char *>(&FTM1_##name) - \
+ reinterpret_cast<volatile char *>(FTM1)), \
+ #name " is at the wrong place"); \
+ static_assert(offsetof(LittleFTM, name) == \
+ (reinterpret_cast<const volatile char *>(&FTM2_##name) - \
+ reinterpret_cast<volatile char *>(FTM2)), \
+ #name " is at the wrong place");
+#define FOR_BOTH_FTM_REGISTER(name) \
+ FOR_BIG_FTM_REGISTER(name) \
+ FOR_LITTLE_FTM_REGISTER(name)
+ALL_FTM_REGISTERS
+#undef FOR_BIG_FTM_REGISTER
+#undef FOR_LITTLE_FTM_REGISTER
+#undef FOR_BOTH_FTM_REGISTER
+#endif
+
+#undef ALL_FTM_REGISTERS
+
#ifdef __cplusplus
}
#endif
diff --git a/third_party/eigen/BUILD b/third_party/eigen/BUILD
index 3f4fc4c..4a33fd9 100644
--- a/third_party/eigen/BUILD
+++ b/third_party/eigen/BUILD
@@ -1,5 +1,7 @@
licenses(['notice'])
+load("//tools:environments.bzl", "mcu_cpus")
+
cc_library(
name = 'eigen',
visibility = ['//visibility:public'],
@@ -16,4 +18,5 @@
'Eigen/UmfPackSupport',
]) + ['unsupported/Eigen/MatrixFunctions'] +
glob(['unsupported/Eigen/src/MatrixFunctions/*.h']),
+ compatible_with = mcu_cpus,
)