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_