Merge "Add initial fet12 code"
diff --git a/motors/fet12/BUILD b/motors/fet12/BUILD
index a242f8e..3787e24 100644
--- a/motors/fet12/BUILD
+++ b/motors/fet12/BUILD
@@ -2,6 +2,29 @@
load("//tools:environments.bzl", "mcu_cpus")
cc_binary(
+ name = "fet12.elf",
+ srcs = [
+ "fet12.cc",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ ":motor_controls",
+ "//motors:motor",
+ "//motors:util",
+ "//motors/core",
+ "//motors/peripheral:adc",
+ "//motors/peripheral:can",
+ "//motors/usb",
+ "//motors/usb:cdc",
+ ],
+)
+
+hex_from_elf(
+ name = "fet12",
+ restricted_to = mcu_cpus,
+)
+
+cc_binary(
name = "power_wheels.elf",
srcs = [
"power_wheels.cc",
@@ -20,3 +43,20 @@
name = "power_wheels",
restricted_to = mcu_cpus,
)
+
+cc_library(
+ name = "motor_controls",
+ srcs = [
+ "motor_controls.cc",
+ ],
+ hdrs = [
+ "motor_controls.h",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ "//motors:math",
+ "//motors:motor",
+ "//motors/peripheral:configuration",
+ "//third_party/eigen",
+ ],
+)
diff --git a/motors/fet12/fet12.cc b/motors/fet12/fet12.cc
new file mode 100644
index 0000000..6b48436
--- /dev/null
+++ b/motors/fet12/fet12.cc
@@ -0,0 +1,349 @@
+#include "motors/core/kinetis.h"
+
+#include <inttypes.h>
+#include <stdio.h>
+
+#include <atomic>
+
+#include "motors/core/time.h"
+#include "motors/fet12/motor_controls.h"
+#include "motors/motor.h"
+#include "motors/peripheral/adc.h"
+#include "motors/peripheral/can.h"
+#include "motors/usb/cdc.h"
+#include "motors/usb/usb.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace motors {
+namespace {
+
+struct Fet12AdcReadings {
+ uint16_t motor_currents[3];
+};
+
+void AdcInitFet12() {
+ AdcInitCommon();
+
+ // M_CH0V ADC1_SE13
+ PORTB_PCR7 = PORT_PCR_MUX(0);
+
+ // M_CH1V ADC1_SE14
+ PORTB_PCR10 = PORT_PCR_MUX(0);
+
+ // M_CH2V ADC1_SE4b
+ PORTC_PCR10 = PORT_PCR_MUX(0);
+
+ // M_CH0F ADC0_SE16
+ // dedicated
+
+ // M_CH1F ADC0_SE18
+ PORTE_PCR24 = PORT_PCR_MUX(0);
+
+ // M_CH2F ADC0_SE23
+ // dedicated
+}
+
+Fet12AdcReadings AdcReadFet12(const DisableInterrupts &) {
+ Fet12AdcReadings r;
+
+ ADC0_SC1A = 16;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC0_SC1A = 18;
+ r.motor_currents[0] = ADC0_RA;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC0_SC1A = 23;
+ r.motor_currents[1] = ADC0_RA;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ r.motor_currents[2] = ADC0_RA;
+
+ return r;
+}
+
+::std::atomic<Motor *> global_motor{nullptr};
+::std::atomic<teensy::AcmTty *> global_stdout{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);
+ }
+}
+
+int _write(int /*file*/, char *ptr, int len) {
+ teensy::AcmTty *const tty = global_stdout.load(::std::memory_order_acquire);
+ if (tty != nullptr) {
+ return tty->Write(ptr, len);
+ }
+ return 0;
+}
+
+void __stack_chk_fail(void);
+
+extern char *__brkval;
+extern uint32_t __bss_ram_start__[];
+extern uint32_t __heap_start__[];
+extern uint32_t __stack_end__[];
+
+void ftm0_isr(void) {
+ Fet12AdcReadings adc_readings;
+ {
+ DisableInterrupts disable_interrupts;
+ adc_readings = AdcReadFet12(disable_interrupts);
+ }
+#if 1
+ printf("%" PRIu16 " %" PRIu16 " %" PRIu16 "\n",
+ adc_readings.motor_currents[0], adc_readings.motor_currents[1],
+ adc_readings.motor_currents[2]);
+#endif
+ const BalancedReadings balanced =
+ BalanceSimpleReadings(adc_readings.motor_currents);
+
+#if 0
+ global_motor.load(::std::memory_order_relaxed)->HandleInterrupt(
+ balanced,
+ global_motor.load(::std::memory_order_relaxed)->wrapped_encoder());
+#else
+ (void)balanced;
+#endif
+ FTM0->SC &= ~FTM_SC_TOF;
+ FTM0->C0V = 0;
+ FTM0->C1V = 0;
+ FTM0->C2V = 0;
+ FTM0->C3V = 0;
+ FTM0->C4V = 0;
+ FTM0->C5V = 80;
+ FTM0->PWMLOAD = FTM_PWMLOAD_LDOK;
+}
+
+} // extern "C"
+
+void ConfigurePwmFtm(BigFTM *pwm_ftm) {
+ // Put them all into combine active-high mode, and all the low ones staying on
+ // all the time by default.
+ 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;
+ pwm_ftm->C6SC = FTM_CSC_ELSA;
+ pwm_ftm->C6V = 0;
+ pwm_ftm->C7SC = FTM_CSC_ELSA;
+ pwm_ftm->C7V = 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 the deadtime.
+ pwm_ftm->DEADTIME =
+ FTM_DEADTIME_DTPS(0) /* Prescaler of 1 */ | FTM_DEADTIME_DTVAL(9);
+}
+
+// Zeros the encoder. This involves blocking for an arbitrary length of time
+// with interrupts disabled.
+void ZeroMotor() {
+#if 0
+ while (true) {
+ if (PERIPHERAL_BITBAND(GPIOA_PDIR, 7)) {
+ 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"(&PERIPHERAL_BITBAND(GPIOA_PDIR, 7)),
+ [cnt] "l"(&FTM1->CNT));
+ __enable_irq();
+#endif
+}
+
+} // namespace
+
+extern "C" int main(void) {
+ // for background about this startup delay, please see these conversations
+ // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980
+ // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273
+ delay(400);
+
+ // Set all interrupts to the second-lowest priority to start with.
+ for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD);
+
+ // Now set priorities for all the ones we care about. They only have meaning
+ // 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.
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1;
+ PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+#if 0
+ PERIPHERAL_BITBAND(GPIOA_PDDR, 15) = 1;
+ PORTA_PCR15 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+#endif
+
+ // Set up the CAN pins.
+ PORTA_PCR12 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ PORTA_PCR13 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+
+ DMA_CR = DMA_CR_EMLM;
+
+ teensy::UsbDevice usb_device(0, 0x16c0, 0x0490);
+ usb_device.SetManufacturer("FRC 971 Spartan Robotics");
+ usb_device.SetProduct("Pistol Grip Controller debug");
+ teensy::AcmTty tty1(&usb_device);
+ teensy::AcmTty tty2(&usb_device);
+ global_stdout.store(&tty1, ::std::memory_order_release);
+ usb_device.Initialize();
+
+ AdcInitFet12();
+ MathInit();
+ delay(1000);
+ can_init(0, 1);
+
+#if 0
+ GPIOD_PCOR = 1 << 3;
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 3) = 1;
+ PORTD_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ delay(1000);
+ GPIOD_PSOR = 1 << 3;
+ delay(1000);
+ GPIOD_PCOR = 1 << 3;
+ delay(1000);
+#endif
+
+ MotorControlsImplementation controls;
+
+ delay(1000);
+
+ // Index pin
+ PORTA_PCR7 = PORT_PCR_MUX(1);
+ // FTM1_QD_PH{A,B}
+ PORTB_PCR0 = PORT_PCR_MUX(6);
+ PORTB_PCR1 = PORT_PCR_MUX(6);
+
+ // FTM0_CH[0-5]
+ PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTC_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTC_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTC_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTD_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+
+ Motor motor(FTM0, FTM1, &controls, {&FTM0->C0V, &FTM0->C2V, &FTM0->C4V});
+ motor.set_encoder_offset(810);
+ motor.set_deadtime_compensation(9);
+ ConfigurePwmFtm(FTM0);
+ 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);
+
+ 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__);
+
+ printf("Going silent to zero motors...\n");
+ // Give the print a chance to make it out.
+ delay(1000);
+#if 0
+ ZeroMotor();
+#endif
+
+ printf("Zeroed motor!\n");
+ // Give stuff a chance to recover from interrupts-disabled.
+ delay(100);
+ motor.Start();
+ NVIC_ENABLE_IRQ(IRQ_FTM0);
+ GPIOC_PSOR = 1 << 5;
+
+ float current_command = 0;
+ while (true) {
+ unsigned char command_data[8];
+ int command_length;
+ can_receive(command_data, &command_length, 0);
+ if (command_length == 4) {
+ uint32_t result = command_data[0] << 24 | command_data[1] << 16 |
+ command_data[2] << 8 | command_data[3];
+ float current = static_cast<float>(result) / 1000.0f;
+
+ static bool high_gear = false;
+ if (controls.estimated_velocity() < -2015) {
+ high_gear = true;
+ }
+ if (current < 1) {
+ high_gear = false;
+ }
+ if (!high_gear) {
+ current = current_command * -120.0f / 120.0f;
+ } else {
+ current = current_command * 115.0f / 120.0f;
+ }
+ motor.SetGoalCurrent(current);
+ current_command = current;
+ }
+ }
+
+ return 0;
+}
+
+} // namespace motors
+} // namespace frc971
diff --git a/motors/fet12/motor_controls.cc b/motors/fet12/motor_controls.cc
new file mode 100644
index 0000000..cd048f4
--- /dev/null
+++ b/motors/fet12/motor_controls.cc
@@ -0,0 +1,243 @@
+#include "motors/fet12/motor_controls.h"
+
+#include "motors/peripheral/configuration.h"
+
+namespace frc971 {
+namespace motors {
+namespace {
+
+template <int kRows, int kCols>
+using ComplexMatrix = MotorControlsImplementation::ComplexMatrix<kRows, kCols>;
+
+using Complex = ::std::complex<float>;
+
+constexpr int kCountsPerRevolution =
+ MotorControlsImplementation::constant_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, constant_counts_per_revolution()>>(theta);
+ const ComplexMatrix<3, 1> E2 =
+ E2Unrotated_ *
+ ImaginaryExpInt<::std::ratio<5, constant_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> H1E1 = H1 * E1;
+ const ComplexMatrix<3, 1> H2E2 = H2 * E2;
+ const ComplexMatrix<3, 1> p_imaginary = H1E1 + H2E2;
+ const ComplexMatrix<3, 1> p_next_imaginary =
+ ImaginaryExpFloat(omega / SWITCHING_FREQUENCY) * H1E1 +
+ ImaginaryExpFloat(omega * 5 / SWITCHING_FREQUENCY) * H2E2;
+ 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 motors
+} // namespace frc971
diff --git a/motors/fet12/motor_controls.h b/motors/fet12/motor_controls.h
new file mode 100644
index 0000000..b9514fd
--- /dev/null
+++ b/motors/fet12/motor_controls.h
@@ -0,0 +1,62 @@
+#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 motors {
+
+class MotorControlsImplementation : public MotorControls {
+ public:
+ template <int kRows, int kCols>
+ using ComplexMatrix = ::Eigen::Matrix<::std::complex<float>, kRows, kCols>;
+
+ MotorControlsImplementation();
+ ~MotorControlsImplementation() override = default;
+
+ static constexpr int constant_counts_per_revolution() { return 1024; }
+
+ int mechanical_counts_per_revolution() const override {
+ return constant_counts_per_revolution();
+ }
+ int electrical_counts_per_revolution() const override {
+ return constant_counts_per_revolution();
+ }
+ float scale_current_reading(float reading) const override {
+ 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 */);
+ }
+
+ ::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 motors
+} // namespace frc971
+
+#endif // MOTORS_MOTOR_CONTROLS_H_