Merge "Move over to ABSL logging and flags."
diff --git a/motors/BUILD b/motors/BUILD
index b606bf7..d4ad63d 100644
--- a/motors/BUILD
+++ b/motors/BUILD
@@ -103,11 +103,10 @@
     ],
 )
 
-cc_binary(
-    name = "button_board.elf",
-    srcs = [
-        "button_board.cc",
-    ],
+cc_library(
+    name = "driver_station_lib",
+    srcs = ["driver_station.cc"],
+    hdrs = ["driver_station.h"],
     target_compatible_with = ["@platforms//os:none"],
     deps = [
         ":util",
@@ -121,10 +120,20 @@
     ],
 )
 
-hex_from_elf(
-    name = "button_board",
-    target_compatible_with = ["@platforms//os:none"],
-)
+[
+    cc_binary(
+        name = "driver_station.elf",
+        target_compatible_with = ["@platforms//os:none"],
+        deps = [":driver_station_lib"],
+    ),
+]
+
+[
+    hex_from_elf(
+        name = "driver_station",
+        target_compatible_with = ["@platforms//os:none"],
+    ),
+]
 
 cc_binary(
     name = "simple_receiver.elf",
diff --git a/motors/README.md b/motors/README.md
new file mode 100644
index 0000000..c71c657
--- /dev/null
+++ b/motors/README.md
@@ -0,0 +1,22 @@
+# Driver station
+## Setup
+1. Install Teensy Loader [here](https://www.pjrc.com/teensy/loader_mac.html).
+
+## Make changes
+1. Most of the code lives in `/motors/driver_station*`. 
+
+## Deploy
+1. Build driver station. This outputs a file `bazel-bin/motors/driver_station.hex`.
+    ```shell
+    bazel build -c opt --config=cortex-m4f //motors:driver_station
+    ```
+
+2. Send the file to your local development machiine. 
+    ```shell
+    scp <user@build.frc971.org>:917-Robot-Code/bazel-bin/motors/driver_station.hex ~/Downloads/driver_station.hex
+    ```
+
+3. Open hex file in Teensy loader. Press button on Teensy. 
+
+## References
+- Electrical schematics [here](https://github.com/frc971/electrical/blob/main/robots/2023/driver-station/DriverStation-8Apr2023.pdf).
\ No newline at end of file
diff --git a/motors/button_board.cc b/motors/button_board.cc
deleted file mode 100644
index e509ea6..0000000
--- a/motors/button_board.cc
+++ /dev/null
@@ -1,407 +0,0 @@
-// This file has the main for the Teensy on the button board.
-
-#include <inttypes.h>
-#include <stdio.h>
-
-#include <atomic>
-#include <cmath>
-
-#include "motors/core/kinetis.h"
-#include "motors/core/time.h"
-#include "motors/peripheral/adc.h"
-#include "motors/peripheral/can.h"
-#include "motors/print/print.h"
-#include "motors/usb/cdc.h"
-#include "motors/usb/hid.h"
-#include "motors/usb/usb.h"
-#include "motors/util.h"
-
-namespace frc971::motors {
-namespace {
-
-struct JoystickAdcReadings {
-  uint16_t analog0, analog1, analog2, analog3;
-};
-
-void AdcInitJoystick() {
-  AdcInitCommon();
-
-  // ANALOG0 ADC0_SE5b
-  PORTD_PCR1 = PORT_PCR_MUX(0);
-  // ANALOG1 ADC0_SE14
-  PORTC_PCR0 = PORT_PCR_MUX(0);
-  // ANALOG2 ADC0_SE13
-  PORTB_PCR3 = PORT_PCR_MUX(0);
-  // ANALOG3 ADC0_SE12
-  PORTB_PCR2 = PORT_PCR_MUX(0);
-}
-
-JoystickAdcReadings AdcReadJoystick(const DisableInterrupts &) {
-  JoystickAdcReadings r;
-
-  ADC0_SC1A = 5;
-  while (!(ADC0_SC1A & ADC_SC1_COCO)) {
-  }
-  ADC0_SC1A = 14;
-  r.analog0 = ADC0_RA;
-  while (!(ADC0_SC1A & ADC_SC1_COCO)) {
-  }
-  ADC0_SC1A = 13;
-  r.analog1 = ADC0_RA;
-  while (!(ADC0_SC1A & ADC_SC1_COCO)) {
-  }
-  ADC0_SC1A = 12;
-  r.analog2 = ADC0_RA;
-  while (!(ADC0_SC1A & ADC_SC1_COCO)) {
-  }
-  r.analog3 = ADC0_RA;
-
-  return r;
-}
-
-// The HID report descriptor we use.
-constexpr char kReportDescriptor1[] = {
-    0x05, 0x01,        // Usage Page (Generic Desktop),
-    0x09, 0x04,        // Usage (Joystick),
-    0xA1, 0x01,        // Collection (Application),
-    0x75, 0x08,        //     Report Size (8),
-    0x95, 0x04,        //     Report Count (4),
-    0x15, 0x00,        //     Logical Minimum (0),
-    0x26, 0xFF, 0x00,  //     Logical Maximum (255),
-    0x35, 0x00,        //     Physical Minimum (0),
-    0x46, 0xFF, 0x00,  //     Physical Maximum (255),
-    0x09, 0x30,        //     Usage (X),
-    0x09, 0x31,        //     Usage (Y),
-    0x09, 0x32,        //     Usage (Z),
-    0x09, 0x33,        //     Usage (Rz),
-    0x81, 0x02,        //     Input (Variable),
-    0x75, 0x01,        //     Report Size (1),
-    0x95, 0x10,        //     Report Count (16),
-    0x25, 0x01,        //     Logical Maximum (1),
-    0x45, 0x01,        //     Physical Maximum (1),
-    0x05, 0x09,        //     Usage Page (Button),
-    0x19, 0x01,        //     Usage Minimum (01),
-    0x29, 0x10,        //     Usage Maximum (16),
-    0x81, 0x02,        //     Input (Variable),
-    0xC0               // End Collection
-};
-
-constexpr uint16_t report_size() { return 1 * 4 + 2; }
-
-char DecodeAnalog(int analog) {
-  // None: 132
-  // Far: 71
-  // Near: 103
-  // Both: 0
-  if (analog < 30) {
-    return 0x3;
-  } else if (::std::abs(analog - 71) < 10) {
-    return 0x2;
-  } else if (::std::abs(analog - 103) < 10) {
-    return 0x1;
-  } else {
-    return 0x0;
-  }
-}
-
-void SendJoystickData(teensy::HidFunction *joystick0,
-                      teensy::HidFunction *joystick1) {
-  uint32_t start = micros();
-  while (true) {
-    JoystickAdcReadings adc;
-    char report0[report_size()];
-    char report1[report_size()];
-    {
-      DisableInterrupts disable_interrupts;
-      adc = AdcReadJoystick(disable_interrupts);
-    }
-
-    FTM0->C1V = adc.analog0 / 4;
-    FTM0->C0V = adc.analog1 / 4;
-    FTM0->C4V = adc.analog2 / 4;
-    FTM0->C3V = adc.analog3 / 4;
-    FTM0->PWMLOAD = FTM_PWMLOAD_LDOK;
-    report0[0] = report1[0] = adc.analog0 / 16;
-    report0[1] = report1[1] = adc.analog1 / 16;
-    report0[2] = report1[2] = adc.analog2 / 16;
-    report0[3] = report1[3] = adc.analog3 / 16;
-
-    report0[4] = ((PERIPHERAL_BITBAND(GPIOD_PDIR, 5) << 0) |
-                  (PERIPHERAL_BITBAND(GPIOD_PDIR, 6) << 1) |
-                  (PERIPHERAL_BITBAND(GPIOB_PDIR, 0) << 2) |
-                  (PERIPHERAL_BITBAND(GPIOB_PDIR, 1) << 3) |
-                  (PERIPHERAL_BITBAND(GPIOA_PDIR, 14) << 4) |
-                  (PERIPHERAL_BITBAND(GPIOE_PDIR, 26) << 5) |
-                  (PERIPHERAL_BITBAND(GPIOA_PDIR, 16) << 6) |
-                  (PERIPHERAL_BITBAND(GPIOA_PDIR, 15) << 7)) ^
-                 0xff;
-
-    report0[5] = ((PERIPHERAL_BITBAND(GPIOE_PDIR, 25) << 0) |
-                  (PERIPHERAL_BITBAND(GPIOE_PDIR, 24) << 1) |
-                  (PERIPHERAL_BITBAND(GPIOC_PDIR, 3) << 2) |
-                  (PERIPHERAL_BITBAND(GPIOC_PDIR, 7) << 3) |
-                  (PERIPHERAL_BITBAND(GPIOD_PDIR, 3) << 4) |
-                  (PERIPHERAL_BITBAND(GPIOD_PDIR, 2) << 5) |
-                  (PERIPHERAL_BITBAND(GPIOD_PDIR, 7) << 6) |
-                  (PERIPHERAL_BITBAND(GPIOA_PDIR, 13) << 7)) ^
-                 0xff;
-
-    report1[4] =
-        ((PERIPHERAL_BITBAND(GPIOA_PDIR, 12) << 0) |
-         (PERIPHERAL_BITBAND(GPIOD_PDIR, 0) << 1) |
-         (PERIPHERAL_BITBAND(GPIOB_PDIR, 17) << 2) |
-         (PERIPHERAL_BITBAND(GPIOB_PDIR, 16) << 3) |
-         (DecodeAnalog(report1[0]) << 4) | (DecodeAnalog(report1[1]) << 6)) ^
-        0x0f;
-    report1[5] = (DecodeAnalog(report1[2])) | (DecodeAnalog(report1[3]) << 2);
-
-    {
-      DisableInterrupts disable_interrupts;
-      joystick0->UpdateReport(report0, sizeof(report0), disable_interrupts);
-      joystick1->UpdateReport(report1, sizeof(report1), disable_interrupts);
-    }
-
-    start = delay_from(start, 1);
-  }
-}
-
-void SetupLedFtm(BigFTM *ftm) {
-  // PWMSYNC doesn't matter because we set SYNCMODE down below.
-  ftm->MODE = FTM_MODE_WPDIS;
-  ftm->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
-  ftm->SC = FTM_SC_CLKS(0) /* Disable counting for now */;
-
-  // Use center-aligned high-true for all the channels.
-  ftm->C0SC = FTM_CSC_ELSB;
-  ftm->C0V = 0;
-  ftm->C1SC = FTM_CSC_ELSB;
-  ftm->C1V = 0;
-  ftm->C2SC = FTM_CSC_ELSB;
-  ftm->C2V = 0;
-  ftm->C3SC = FTM_CSC_ELSB;
-  ftm->C3V = 0;
-  ftm->C4SC = FTM_CSC_ELSB;
-  ftm->C4V = 0;
-  ftm->C5SC = FTM_CSC_ELSB;
-  ftm->C5V = 0;
-  ftm->C6SC = FTM_CSC_ELSB;
-  ftm->C6V = 0;
-  ftm->C7SC = FTM_CSC_ELSB;
-  ftm->C7V = 0;
-
-  ftm->COMBINE = FTM_COMBINE_SYNCEN3 /* Synchronize updates usefully */ |
-                 FTM_COMBINE_SYNCEN2 /* Synchronize updates usefully */ |
-                 FTM_COMBINE_SYNCEN1 /* Synchronize updates usefully */ |
-                 FTM_COMBINE_SYNCEN0 /* Synchronize updates usefully */;
-
-  ftm->CNTIN = 0;
-  ftm->CNT = 0;
-  ftm->MOD = 1024;
-  ftm->OUTINIT = 0;
-  ftm->POL = 0;
-  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 */;
-  // Don't want any intermediate loading points.
-  ftm->PWMLOAD = 0;
-
-  ftm->SYNC = FTM_SYNC_SWSYNC /* Flush everything out right now */;
-  // Wait for the software synchronization to finish.
-  while (ftm->SYNC & FTM_SYNC_SWSYNC) {
-  }
-  ftm->SC = FTM_SC_CPWMS /* Center-aligned PWM */ |
-            FTM_SC_CLKS(1) /* Use the system clock */ |
-            FTM_SC_PS(6) /* Prescaler=64 */;
-
-  ftm->MODE &= ~FTM_MODE_WPDIS;
-}
-
-}  // namespace
-
-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 "C"
-
-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);
-
-  // Set all the LED pins to output, slew rate controlled, high drive strength.
-  // Builtin
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 1;
-  PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1);
-  PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1;
-  // LED0 FTM0_CH1
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 2) = 0;
-  PORTC_PCR2 = PORT_PCR_DSE | PORT_PCR_ODE | PORT_PCR_SRE | PORT_PCR_MUX(4);
-  PERIPHERAL_BITBAND(GPIOC_PDDR, 2) = 1;
-  // LED1 FTM0_CH0
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 1) = 0;
-  PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_ODE | PORT_PCR_SRE | PORT_PCR_MUX(4);
-  PERIPHERAL_BITBAND(GPIOC_PDDR, 1) = 1;
-  // LED2 FTM0_CH4
-  PERIPHERAL_BITBAND(GPIOD_PDOR, 4) = 0;
-  PORTD_PCR4 = PORT_PCR_DSE | PORT_PCR_ODE | PORT_PCR_SRE | PORT_PCR_MUX(4);
-  PERIPHERAL_BITBAND(GPIOD_PDDR, 4) = 1;
-  // LED3 FTM0_CH3
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 4) = 0;
-  PORTC_PCR4 = PORT_PCR_DSE | PORT_PCR_ODE | PORT_PCR_SRE | PORT_PCR_MUX(4);
-  PERIPHERAL_BITBAND(GPIOC_PDDR, 4) = 1;
-  // LED4 FTM3_CH4 yellow
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = 0;
-  PORTC_PCR8 = PORT_PCR_DSE | PORT_PCR_ODE | PORT_PCR_SRE | PORT_PCR_MUX(1);
-  PERIPHERAL_BITBAND(GPIOC_PDDR, 8) = 1;
-  // LED5 FTM3_CH5 green
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 9) = 0;
-  PORTC_PCR9 = PORT_PCR_DSE | PORT_PCR_ODE | PORT_PCR_SRE | PORT_PCR_MUX(1);
-  PERIPHERAL_BITBAND(GPIOC_PDDR, 9) = 1;
-  // LED6 FTM3_CH6 red
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = 0;
-  PORTC_PCR10 = PORT_PCR_DSE | PORT_PCR_ODE | PORT_PCR_SRE | PORT_PCR_MUX(1);
-  PERIPHERAL_BITBAND(GPIOC_PDDR, 10) = 1;
-
-  // Set up the CAN pins.
-  PORTB_PCR18 = PORT_PCR_DSE | PORT_PCR_MUX(2);
-  PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(2);
-
-  // .1ms filter time.
-  PORTA_DFWR = 6000;
-  PORTB_DFWR = 6000;
-  PORTC_DFWR = 6000;
-  PORTD_DFWR = 6000;
-  PORTE_DFWR = 6000;
-
-  // Set up the buttons. The LEDs pull them up to 5V, so the Teensy needs to not
-  // be set to pull up.
-  // BTN0
-  PORTD_PCR5 = PORT_PCR_MUX(1);
-  PORTD_DFER |= 1 << 5;
-  // BTN1
-  PORTD_PCR6 = PORT_PCR_MUX(1);
-  PORTD_DFER |= 1 << 6;
-  // BTN2
-  PORTB_PCR0 = PORT_PCR_MUX(1);
-  PORTB_DFER |= 1 << 0;
-  // BTN3
-  PORTB_PCR1 = PORT_PCR_MUX(1);
-  PORTB_DFER |= 1 << 1;
-  // BTN4
-  PORTA_PCR14 = PORT_PCR_MUX(1);
-  PORTA_DFER |= 1 << 14;
-  // BTN5
-  PORTE_PCR26 = PORT_PCR_MUX(1);
-  PORTE_DFER |= 1 << 26;
-  // BTN6
-  PORTA_PCR16 = PORT_PCR_MUX(1);
-  PORTA_DFER |= 1 << 16;
-  // BTN7
-  PORTA_PCR15 = PORT_PCR_MUX(1);
-  PORTA_DFER |= 1 << 15;
-  // BTN8
-  PORTE_PCR25 = PORT_PCR_MUX(1);
-  PORTE_DFER |= 1 << 25;
-  // BTN9
-  PORTE_PCR24 = PORT_PCR_MUX(1);
-  PORTE_DFER |= 1 << 24;
-  // BTN10
-  PORTC_PCR3 = PORT_PCR_MUX(1);
-  PORTC_DFER |= 1 << 3;
-  // BTN11
-  PORTC_PCR7 = PORT_PCR_MUX(1);
-  PORTC_DFER |= 1 << 7;
-  // BTN12
-  PORTD_PCR3 = PORT_PCR_MUX(1);
-  PORTD_DFER |= 1 << 3;
-  // BTN13
-  PORTD_PCR2 = PORT_PCR_MUX(1);
-  PORTD_DFER |= 1 << 2;
-  // BTN14
-  PORTD_PCR7 = PORT_PCR_MUX(1);
-  PORTD_DFER |= 1 << 7;
-  // BTN15
-  PORTA_PCR13 = PORT_PCR_MUX(1);
-  PORTA_DFER |= 1 << 13;
-  // BTN16
-  PORTA_PCR12 = PORT_PCR_MUX(1);
-  PORTA_DFER |= 1 << 12;
-  // BTN17
-  PORTD_PCR0 = PORT_PCR_MUX(1);
-  PORTD_DFER |= 1 << 0;
-  // BTN18
-  PORTB_PCR17 = PORT_PCR_MUX(1);
-  PORTB_DFER |= 1 << 17;
-  // BTN19
-  PORTB_PCR16 = PORT_PCR_MUX(1);
-  PORTB_DFER |= 1 << 16;
-
-  delay(100);
-
-  teensy::UsbDevice usb_device(0, 0x16c0, 0x0492);
-  usb_device.SetManufacturer("FRC 971 Spartan Robotics");
-  usb_device.SetProduct("Spartan Joystick Board");
-
-  teensy::HidFunction joystick0(&usb_device, report_size());
-  joystick0.set_report_descriptor(
-      ::std::string(kReportDescriptor1, sizeof(kReportDescriptor1)));
-
-  teensy::HidFunction joystick1(&usb_device, report_size());
-  joystick1.set_report_descriptor(
-      ::std::string(kReportDescriptor1, sizeof(kReportDescriptor1)));
-
-  teensy::AcmTty tty1(&usb_device);
-  PrintingParameters printing_parameters;
-  printing_parameters.stdout_tty = &tty1;
-
-  const ::std::unique_ptr<PrintingImplementation> printing =
-      CreatePrinting(printing_parameters);
-  usb_device.Initialize();
-  printing->Initialize();
-
-  can_init(0, 1);
-  AdcInitJoystick();
-  SetupLedFtm(FTM0);
-  SetupLedFtm(FTM3);
-
-  // Leave the LEDs on for a bit longer.
-  delay(300);
-  printf("Done starting up\n");
-
-  // Done starting up, now turn all the LEDs off.
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 0;
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 2) = 1;
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 1) = 1;
-  PERIPHERAL_BITBAND(GPIOD_PDOR, 4) = 1;
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 4) = 1;
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = 1;
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 9) = 1;
-  PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = 1;
-
-  SendJoystickData(&joystick0, &joystick1);
-
-  return 0;
-}
-
-}  // namespace frc971::motors
diff --git a/motors/driver_station.cc b/motors/driver_station.cc
new file mode 100644
index 0000000..287caf3
--- /dev/null
+++ b/motors/driver_station.cc
@@ -0,0 +1,1200 @@
+// This file has the main for the Teensy on the button board.
+
+// ButtonBoard is the main outline of how we communicate with the teensy, along
+// with what we tell it to do
+
+// ButtonBoardBB1 and ButtonBoardBB2 only differ in what teensy pins manage what
+// on the pcb
+// teensy (3.5) schematic: https://www.pjrc.com/teensy/schematic.html
+// datasheets for the chip (MK64FX512) used in the teensy:
+// https://www.pjrc.com/teensy/datasheets.html
+// comments (especially on BB2) inform which pins are used
+
+#include "motors/driver_station.h"
+
+#include <inttypes.h>
+#include <stdio.h>
+
+#include <atomic>
+#include <cmath>
+
+#include "motors/core/kinetis.h"
+#include "motors/core/time.h"
+#include "motors/peripheral/adc.h"
+#include "motors/peripheral/can.h"
+#include "motors/print/print.h"
+#include "motors/usb/cdc.h"
+#include "motors/usb/hid.h"
+#include "motors/usb/usb.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace motors {
+namespace {
+
+// The HID report descriptor we use.
+constexpr char kReportDescriptor1[] = {
+    0x05, 0x01,        // Usage Page (Generic Desktop),
+    0x09, 0x04,        // Usage (Joystick),
+    0xA1, 0x01,        // Collection (Application),
+    0x75, 0x08,        //     Report Size (8),
+    0x95, 0x05,        //     Report Count (5),
+    0x15, 0x00,        //     Logical Minimum (0),
+    0x26, 0xFF, 0x00,  //     Logical Maximum (255),
+    0x35, 0x00,        //     Physical Minimum (0),
+    0x46, 0xFF, 0x00,  //     Physical Maximum (255),
+    0x09, 0x30,        //     Usage (X),
+    0x09, 0x31,        //     Usage (Y),
+    0x09, 0x32,        //     Usage (Z),
+    0x09, 0x33,        //     Usage (Rx),
+    0x09, 0x34,        //     Usage (Ry),
+    0x81, 0x02,        //     Input (Variable),
+    0x75, 0x01,        //     Report Size (1),
+    0x95, 0x10,        //     Report Count (16),
+    0x25, 0x01,        //     Logical Maximum (1),
+    0x45, 0x01,        //     Physical Maximum (1),
+    0x05, 0x09,        //     Usage Page (Button),
+    0x19, 0x01,        //     Usage Minimum (01),
+    0x29, 0x10,        //     Usage Maximum (16),
+    0x81, 0x02,        //     Input (Variable),
+    0xC0               // End Collection
+};
+}  // namespace
+
+bool IsMyProcessorId(uint32_t id[4]) {
+  uint32_t my_uuid[4] = {SIM_UIDH, SIM_UIDMH, SIM_UIDML, SIM_UIDL};
+
+  for (int byte_index = 0; byte_index < 4; byte_index++) {
+    if (id[byte_index] != my_uuid[byte_index]) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+uint8_t ProcessorIndex() {
+  static uint32_t kPistolgripProcessorIds[PROCESSOR_ID_COUNT][4] = {
+      {0x00000000, 0x00000000, 0x00000000, 0x00000000},
+      {0x0036FFFF, 0xFFFFFFFF, 0x4E457285,
+       0x60110022},  // One with cable tie labels
+      {0x0035FFFF, 0xFFFFFFFF, 0x4E457285, 0x60130022},
+  };
+
+  for (int i = 0; i < PROCESSOR_ID_COUNT; i++) {
+    if (IsMyProcessorId(kPistolgripProcessorIds[i])) {
+      return i;
+    }
+  }
+
+  return 0;
+}
+
+uint16_t TareEncoder(ENCODER_DATA_S *encoder_data) {
+  return static_cast<uint16_t>((static_cast<uint32_t>(encoder_data->angle) -
+                                static_cast<uint32_t>(encoder_data->enc_trim) +
+                                0x800) &
+                               0xfff);
+}
+
+// Scale and center controller readings.
+//
+// The encoder has a larger range of motion in the + direction than the -
+// direction. We scale the readings so that min (-) maps to 0x0000, max (+) maps
+// to 0xFFFF and the center is at 0x8000.
+uint16_t ScaleEncoder(ENCODER_DATA_S *encoder_data, uint16_t adjusted_angle) {
+  uint16_t result = 0x8000;
+  if (adjusted_angle > 0x800) {
+    const float scaled_angle =
+        static_cast<float>(static_cast<int>(adjusted_angle) - 0x800) /
+        static_cast<float>(encoder_data->enc_max - 0x800);
+    result += std::min<int>(0xffff - 0x8000,
+                            static_cast<int>(scaled_angle * (0xffff - 0x8000)));
+  } else {
+    const float scaled_angle =
+        static_cast<float>(static_cast<int>(adjusted_angle) - 0x800) /
+        static_cast<float>(0x800 - encoder_data->enc_min);
+    result -= std::min<int>(0x8000, static_cast<int>(-scaled_angle * 0x8000));
+  }
+
+  return result;
+}
+
+uint16_t filterIIR(uint32_t *filterValue, uint16_t currentValue,
+                   uint16_t filterDepth, bool initFilter) {
+  uint32_t localValue = 0;
+
+  if (initFilter) {
+    *filterValue = currentValue << filterDepth;
+    return currentValue;
+  }
+
+  localValue = *filterValue - (*filterValue >> filterDepth);
+
+  localValue += currentValue;
+
+  *filterValue = localValue;
+
+  return localValue >> filterDepth;
+}
+
+int DriverStation::DetermineEncoderValues(ENCODER_DATA_S *enc,
+                                          ABS_POSITION_S *absAngle,
+                                          int encoderNum,
+                                          uint16_t resetTime_ms) {
+  uint16_t currentAngle = 0;
+
+  if (enc->angle > ENCODER_COUNTS_PER_REV) {
+    enc->angle = 0;
+  }
+
+  if (ReadQuadrature(encoderNum, &currentAngle)) {
+    return -1;
+  }
+
+  if (MeasureAbsPosition(encoderNum, absAngle)) {
+    return -1;
+  }
+
+  if (!absAngle->intialized) {
+    return 0;
+  }
+
+  enc->resetTimer_ms++;
+  if (abs((int)(((currentAngle + enc->offset) & ENCODER_MOD) - enc->angle)) >
+      0) {
+    enc->resetTimer_ms = 0;
+    (void)filterIIR(&(enc->angle_filter), absAngle->dutycycle,
+                    ENCODER_FILTER_EXPONENT, true);
+  }
+
+  if (abs((int)(enc->angle - absAngle->dutycycle)) > MAX_FILTER_DELTA &&
+      enc->resetTimer_ms > 0) {
+    (void)filterIIR(&(enc->angle_filter), absAngle->dutycycle,
+                    ENCODER_FILTER_EXPONENT, true);
+  }
+
+  if (enc->resetTimer_ms > resetTime_ms) {
+    enc->filtered_duty_cycle =
+        filterIIR(&(enc->angle_filter), absAngle->dutycycle,
+                  ENCODER_FILTER_EXPONENT, false);
+    enc->offset =
+        enc->filtered_duty_cycle -
+        currentAngle; /* offset is calculated using filtered abs_position*/
+    enc->offset &= ENCODER_MOD;
+    enc->resetTimer_ms = resetTime_ms;
+  }
+
+  enc->angle = currentAngle + enc->offset;
+
+  enc->angle &= ENCODER_MOD;
+
+  return 0;
+}
+
+int DriverStation::MeasureAbsPosition(uint32_t encoder_id,
+                                      ABS_POSITION_S *abs_position) {
+  BigFTM *ftm = NULL;
+  volatile uint32_t *volatile stat_ctrl0 = NULL;  // status and control
+  volatile uint32_t *volatile stat_ctrl1 = NULL;
+  volatile uint32_t *volatile channel_val0 = NULL;  // channel value
+  volatile uint32_t *volatile channel_val1 = NULL;
+  uint32_t decap_mask = 0;
+  uint32_t decap_enable = 0;
+  uint32_t channel_filter = 0;
+
+  uint32_t initial = 0;
+  uint32_t final = 0;
+
+  switch (encoder_id) {
+    case 0:
+      ftm = FTM3;
+      stat_ctrl0 = &(FTM3_C0SC);
+      stat_ctrl1 = &(FTM3_C1SC);
+      channel_val0 = &(FTM3_C0V);
+      channel_val1 = &(FTM3_C1V);
+      decap_mask = FTM_COMBINE_DECAP0;
+      decap_enable = FTM_COMBINE_DECAPEN0;
+      channel_filter = FTM_FILTER_CH0FVAL(0);
+
+      break;
+    case 1:
+      ftm = FTM0;
+      stat_ctrl0 = &(FTM0_C6SC);
+      stat_ctrl1 = &(FTM0_C7SC);
+      channel_val0 = &(FTM0_C6V);
+      channel_val1 = &(FTM0_C7V);
+      decap_mask = FTM_COMBINE_DECAP3;
+      decap_enable = FTM_COMBINE_DECAPEN3;
+      channel_filter = FTM_FILTER_CH3FVAL(0);
+
+      break;
+
+    default:
+      return -1;
+      break;
+  }
+
+  switch (abs_position->state) {
+    case INIT_ABS:
+      ftm->MODE = FTM_MODE_WPDIS;
+      ftm->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
+
+      ftm->CNTIN = 0;
+      ftm->MOD = 0xFFFF;
+
+      // Disable filters
+      ftm->FILTER = channel_filter;
+
+      ftm->COMBINE = decap_enable;
+      ftm->COMBINE = decap_enable | decap_mask;
+
+      // Use PWM decoding rising and falling edges in CH0 and rising edges only
+      // in CH1.
+      *stat_ctrl0 = FTM_CSC_ELSA;
+      *stat_ctrl1 = FTM_CSC_ELSA;
+      *channel_val0 = 0;
+      *channel_val1 = 0;
+
+      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 */;
+
+      ftm->SYNC = FTM_SYNC_SWSYNC /* Flush everything out right now */;
+
+      // Wait for the software synchronization to finish.
+      while (ftm->SYNC & FTM_SYNC_SWSYNC) {
+      }
+
+      ftm->SC = FTM_SC_CLKS(1) /* Use the system clock */ |
+                FTM_SC_PS(4) /* Prescaler=16 */;
+
+      ftm->MODE &= ~FTM_MODE_WPDIS;
+      abs_position->state = START_PERIOD;
+      break;
+
+    case START_PERIOD:
+      if ((ftm->COMBINE & decap_mask) != decap_mask) {
+        ftm->COMBINE = decap_enable | decap_mask;
+        *stat_ctrl0 = FTM_CSC_ELSA;
+        *stat_ctrl1 = FTM_CSC_ELSA;
+        abs_position->state = WAIT_PERIOD_DONE;
+      }
+      break;
+
+    case WAIT_PERIOD_DONE:
+      initial = *channel_val0;
+      final = *channel_val1;
+      if ((*stat_ctrl0 & FTM_CSC_CHF) && (*stat_ctrl1 & FTM_CSC_CHF)) {
+        // Add 0x10000 to correct for rollover
+        abs_position->period = (final - initial) & 0xFFFF;
+
+        *stat_ctrl0 &= ~FTM_CSC_CHF;
+        *stat_ctrl1 &= ~FTM_CSC_CHF;
+
+        abs_position->state = START_WIDTH;
+      }
+      break;
+
+    case START_WIDTH:
+      if ((ftm->COMBINE & decap_mask) != decap_mask) {
+        ftm->COMBINE = decap_enable | decap_mask;
+        *stat_ctrl0 = FTM_CSC_ELSA;
+        *stat_ctrl1 = FTM_CSC_ELSB;
+        abs_position->state = WAIT_WIDTH_DONE;
+      }
+      break;
+
+    case WAIT_WIDTH_DONE:
+      initial = *channel_val0;
+      final = *channel_val1;
+      if ((*stat_ctrl0 & FTM_CSC_CHF) && (*stat_ctrl1 & FTM_CSC_CHF)) {
+        // Add 0x10000 to correct for rollover
+        abs_position->width = (final - initial) & 0xFFFF;
+
+        *stat_ctrl0 &= ~FTM_CSC_CHF;
+        *stat_ctrl1 &= ~FTM_CSC_CHF;
+
+        if (abs_position->period != 0) {
+          if (((abs_position->width * ENCODER_COUNTS_PER_REV) /
+               abs_position->period) > ENCODER_MOD) {
+            abs_position->state = START_PERIOD;
+            break;
+          }
+
+          // Handle duty cycle out of range. Reset at next reasonable
+          // measurement.
+          if (abs_position->last_erronious_dutycycle ==
+              (abs_position->width * ENCODER_COUNTS_PER_REV) /
+                  abs_position->period) {
+            abs_position->dutycycle =
+                (abs_position->width * ENCODER_COUNTS_PER_REV) /
+                abs_position->period;
+
+            abs_position->state = START_PERIOD;
+            break;
+          }
+
+          if (abs((int)((abs_position->width * ENCODER_COUNTS_PER_REV) /
+                        abs_position->period) -
+                  (int)abs_position->dutycycle) > MAX_DUTY_CYCLE_DELTA) {
+            abs_position->last_erronious_dutycycle =
+                (abs_position->width * ENCODER_COUNTS_PER_REV) /
+                abs_position->period;
+
+            abs_position->state = START_PERIOD;
+            break;
+          }
+
+          abs_position->dutycycle =
+              (abs_position->width * ENCODER_COUNTS_PER_REV) /
+              abs_position->period;
+          abs_position->intialized = true;
+        } else {
+          abs_position->period = 0xFFFF;
+        }
+
+        abs_position->state = START_PERIOD;
+      }
+      break;
+
+    default:
+      return -1;
+      break;
+  }
+
+  return 0;
+}
+
+std::array<ENCODER_DATA_S, 2> MakeEncoderData(uint32_t processor_index) {
+  switch (processor_index) {
+    case 0:
+    default:
+      return {ENCODER_DATA_S{
+                  .angle = 0,
+                  .offset = 0,
+                  .resetTimer_ms = 0,
+                  .filtered_duty_cycle = 0,
+                  .angle_filter = 0,
+                  .enc_trim = 0,
+                  .enc_min = 0,
+                  .enc_max = 0,
+              },
+              ENCODER_DATA_S{
+                  .angle = 0,
+                  .offset = 0,
+                  .resetTimer_ms = 0,
+                  .filtered_duty_cycle = 0,
+                  .angle_filter = 0,
+                  .enc_trim = 0,
+                  .enc_min = 0,
+                  .enc_max = 0,
+              }};
+    case 1:
+      // Both enc_min and enc_max should come from the "trimmed" enc print
+      // enc_min should be < 0x7FF and enc_max > 0x800
+      // for encoder 0 they should be ~ +/- 0x050 and encoder 1 ~ +/- 0x700 from
+      // center
+      return {ENCODER_DATA_S{
+                  .angle = 0,
+                  .offset = 0,
+                  .resetTimer_ms = 0,
+                  .filtered_duty_cycle = 0,
+                  .angle_filter = 0,
+                  .enc_trim = 0x0568,
+                  .enc_min = 0x0741,
+                  .enc_max = 0x08EB,
+              },
+              ENCODER_DATA_S{
+                  .angle = 0,
+                  .offset = 0,
+                  .resetTimer_ms = 0,
+                  .filtered_duty_cycle = 0,
+                  .angle_filter = 0,
+                  .enc_trim = 0x0987,
+                  .enc_min = 0x00EA,
+                  .enc_max = 0x0FD5,
+              }};
+    case 2:
+      return {ENCODER_DATA_S{
+                  .angle = 0,
+                  .offset = 0,
+                  .resetTimer_ms = 0,
+                  .filtered_duty_cycle = 0,
+                  .angle_filter = 0,
+                  .enc_trim = 0x02CD,
+                  .enc_min = 0x0746,
+                  .enc_max = 0x0900,
+              },
+              ENCODER_DATA_S{
+                  .angle = 0,
+                  .offset = 0,
+                  .resetTimer_ms = 0,
+                  .filtered_duty_cycle = 0,
+                  .angle_filter = 0,
+                  .enc_trim = 0x0589,
+                  .enc_min = 0x00FF,
+                  .enc_max = 0x0FC0,
+              }};
+  }
+}
+
+void DriverStation::SendJoystickData(teensy::HidFunction *joystick0,
+                                     teensy::HidFunction *joystick1,
+                                     teensy::HidFunction *joystick2,
+                                     uint32_t processor_index) {
+  std::array<ENCODER_DATA_S, NUM_ENCODERS> encoder_data =
+      MakeEncoderData(processor_index);
+
+  static ABS_POSITION_S abs_position[NUM_ENCODERS];
+  memset(abs_position, 0, NUM_ENCODERS * sizeof(ABS_POSITION_S));
+
+  uint8_t can_data_out[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+
+  uint8_t can_data_in[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+  static int length = 0;
+
+  MEASUREMENT_DATA_S measurements[BUTTON_BOARD_COUNT];
+
+  uint32_t start = micros();
+  uint16_t can_timer_1 = 0;
+  uint16_t can_timer_2 = 0;
+  while (true) {
+    JoystickAdcReadings adc;
+    char report[3][kReportSize];
+    {
+      DisableInterrupts disable_interrupts;
+      adc = AdcReadJoystick(disable_interrupts);
+    }
+
+    uint16_t tared_encoders[NUM_ENCODERS];
+    for (int i = 0; i < NUM_ENCODERS; i++) {
+      (void)DetermineEncoderValues(&encoder_data[i], &abs_position[i], i,
+                                   ENCODER_RESET_TIME_MS);
+      tared_encoders[i] = TareEncoder(&encoder_data[i]);
+    }
+
+    measurements[board_config.board_id - 1].buttons = ReadButtons();
+    measurements[board_config.board_id - 1].abs0 = adc.analog0;
+    measurements[board_config.board_id - 1].abs1 = adc.analog1;
+    measurements[board_config.board_id - 1].abs2 = adc.analog2;
+    measurements[board_config.board_id - 1].abs3 = adc.analog3;
+    measurements[board_config.board_id - 1].enc0 =
+        ScaleEncoder(&encoder_data[0], tared_encoders[0]);
+    measurements[board_config.board_id - 1].enc1 =
+        ScaleEncoder(&encoder_data[1], tared_encoders[1]);
+
+#if PRINT_OFFSETS
+    static int counter = 0;
+    counter++;
+
+    if (counter % 100 == 0) {
+      printf(
+          "Processor_ID: 0x%08lX 0x%08lX 0x%08lX 0x%08lX, ENC0 Max: 0x%03X, "
+          "ENC0 Angle: 0x%03X, "
+          "ENC0 Tared: 0x%03X, ENC0 Result: 0x%04X, ENC1 Max: 0x%03X, ENC1 "
+          "Angle: 0x%03X, ENC1 "
+          "Tared: 0x%03X, ENC1 Result: 0x%04X\n\r",
+          SIM_UIDH, SIM_UIDMH, SIM_UIDML, SIM_UIDL, encoder_data[0].enc_max,
+          encoder_data[0].angle, tared_encoders[0], measurements[2].enc0,
+          encoder_data[1].enc_max, encoder_data[1].angle, tared_encoders[1],
+          measurements[2].enc1);
+    }
+#endif
+
+    can_receive(can_data_in, &length, 0);
+
+    if (length == -1) {
+      if (can_timer_1 < 2000) {
+        can_timer_1++;
+      }
+    }
+
+    if (can_timer_1 >= 2000) {
+      ZeroMeasurements(measurements, board_config.can_id0 - 1);
+    }
+
+    if (length == 8) {
+      UpdateMeasurementsFromCAN(measurements, can_data_in);
+      can_timer_1 = 0;
+    }
+
+    can_receive(can_data_in, &length, 1);
+
+    if (length == -1) {
+      if (can_timer_2 < 2000) {
+        can_timer_2++;
+      }
+    }
+
+    if (can_timer_2 >= 2000) {
+      ZeroMeasurements(measurements, board_config.can_id1 - 1);
+    }
+
+    if (length == 8) {
+      UpdateMeasurementsFromCAN(measurements, can_data_in);
+      can_timer_2 = 0;
+    }
+
+    static int counter = 0;
+    counter++;
+
+    if (counter % 100 == 0) {
+      printf("CAN Timer 1: %d, from BB%ld + CAN Timer 2: %d, from BB%ld\n\r",
+             can_timer_1, board_config.can_id0, can_timer_2,
+             board_config.can_id1);
+      printf("Report 1: 0x%04X + Report 2: 0x%04X + Report 3: 0x%04X\n\r",
+             report[0][4], report[1][4], report[2][4]);
+    }
+
+    PackMeasurementsToCAN(&measurements[board_config.board_id - 1],
+                          can_data_out);
+
+    can_send(board_config.board_id, can_data_out, sizeof(can_data_out), 2);
+
+    ComposeReport(report, measurements, board_config.board_id,
+                  (can_timer_1 >= 2000) ? -1 : board_config.can_id0 - 1,
+                  (can_timer_2 >= 2000) ? -1 : board_config.can_id1 - 1);
+
+    {
+      DisableInterrupts disable_interrupts;
+      joystick0->UpdateReport(report[0], sizeof(report[0]), disable_interrupts);
+      joystick1->UpdateReport(report[1], sizeof(report[1]), disable_interrupts);
+      joystick2->UpdateReport(report[2], sizeof(report[2]), disable_interrupts);
+    }
+
+    start = delay_from(start, 1);
+  }
+}
+
+void DriverStation::ZeroMeasurements(MEASUREMENT_DATA_S *bbMeasurements,
+                                     uint32_t board) {
+  bbMeasurements[board].buttons = 0;
+  bbMeasurements[board].abs0 = 0;
+  bbMeasurements[board].abs1 = 0;
+  bbMeasurements[board].abs2 = 0;
+  bbMeasurements[board].abs3 = 0;
+  bbMeasurements[board].enc0 = 0;
+  bbMeasurements[board].enc1 = 0;
+}
+
+// clang-format off
+// CAN Packet Format
+//       |                                Bit                                 |
+// Byte	  7 	      6 	     5 	        4       	3       	2        1    	  0
+// 0	  BTN7	    BTN6	   BTN5	      BTN4	    BTN3  	  BTN2  	  BTN1     BTN0
+// 1	  BTN15	    BTN14	   BTN13  	  BTN12	    BTN11 	  BTN10	    BTN9	   BTN8
+// 2	  -     	  -	        -	        -	        BTN19	    BTN18     BTN17    BTN16 
+// 3	  ENC0:7  	ENC0:6  	ENC0:5  	ENC0:4  	ENC0:3    ENC0_2 	  ENC0:1   ENC0:0
+// 4	  ENC1:3	  ENC1:2  	ENC1:1  	ENC1:0  	ENC0:11   ENC0:10   ENC0:9   ENC0:8 
+// 5	  ENC1:11  	ENC1:10 	ENC1:9  	ENC1:8  	ENC1:7    ENC1:6  	ENC1:5   ENC1:4 
+// 6	  ABS2:7	  ABS2:6	  ABS2:5	  ABS2:4	  ABS2:3    ABS2:2	  ABS2:1   ABS2:0 
+// 7	  ABS3:7	  ABS3:6	  ABS3:5	  ABS3:4	  ABS3:3    ABS3:2  	ABS3:1   ABS3:0
+// clang-format on
+int DriverStation::UpdateMeasurementsFromCAN(MEASUREMENT_DATA_S *bbMeasurements,
+                                             uint8_t *canRX_data) {
+  int bb_id = 0;
+  uint32_t buttons = 0;
+  uint16_t enc0 = 0;
+  uint16_t enc1 = 0;
+  uint16_t abs2 = 0;
+  uint16_t abs3 = 0;
+
+  // Extract BB_id
+  bb_id = (int)((canRX_data[2] >> 2) & 0x03);
+  bb_id--;
+
+  if (bb_id == -1) {
+    return -1;
+  }
+
+  buttons = (uint32_t)canRX_data[0];
+  buttons |= (uint32_t)canRX_data[1] << 8;
+  buttons |= ((uint32_t)canRX_data[2] & 0x0F) << 16;
+
+  bbMeasurements[bb_id].buttons = buttons;
+
+  enc0 = (uint16_t)canRX_data[3];
+  enc0 |= ((uint16_t)canRX_data[4] & 0x0F) << 8;
+
+  bbMeasurements[bb_id].enc0 = enc0 << 4;
+
+  enc1 = ((uint16_t)canRX_data[4] & 0xF0) >> 4;
+  enc1 |= ((uint16_t)canRX_data[5]) << 4;
+
+  bbMeasurements[bb_id].enc1 = enc1 << 4;
+
+  abs2 = ((uint16_t)canRX_data[6]) << 8;
+
+  bbMeasurements[bb_id].abs2 = abs2;
+
+  abs3 = ((uint16_t)canRX_data[7]) << 8;
+
+  bbMeasurements[bb_id].abs3 = abs3;
+
+  return bb_id;
+}
+
+void DriverStation::PackMeasurementsToCAN(MEASUREMENT_DATA_S *bbMeasurements,
+                                          uint8_t *canTX_data) {
+  uint16_t encoder_measurements_0 = bbMeasurements->enc0 >> 4;
+  uint16_t encoder_measurements_1 = bbMeasurements->enc1 >> 4;
+
+  // taking button data
+  canTX_data[2] = (uint8_t)((bbMeasurements->buttons >> 16) & 0x0F);
+  canTX_data[1] = (uint8_t)((bbMeasurements->buttons >> 8) & 0xFF);
+  canTX_data[0] = (uint8_t)(bbMeasurements->buttons & 0xFF);
+
+  // taking encdoer data
+  canTX_data[3] = (uint8_t)(encoder_measurements_0 & 0xFF);
+  canTX_data[4] = (uint8_t)((encoder_measurements_0 >> 8) & 0x0F);
+
+  canTX_data[4] |= (uint8_t)((encoder_measurements_1 & 0x0F) << 4);
+  canTX_data[5] = (uint8_t)((encoder_measurements_1 >> 4) & 0xFF);
+
+  // taking abs data
+  canTX_data[6] = (uint8_t)(bbMeasurements->abs2 >> 8);
+  canTX_data[7] = (uint8_t)(bbMeasurements->abs3 >> 8);
+}
+
+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 "C"
+
+// clang-format off
+
+//		                          BB1/BB3 HID			                                BB2 HID		
+//	  HID Word	  JS0	            JS1	            JS2	            JS3	            JS4	            JS5
+// 0	AXIS0	      BB3:ENC0[15:8]	BB3:ENC1[15:8]  BB2:ADC0        BB3:ENC0[15:8]	BB3:ENC1[15:8]  BB2:ADC0
+// 1	AXIS1	      0x7F		        0x7F         	  BB2:ADC1        0x7F	          0x7F            BB2:ADC1
+// 2	AXIS2	      0x7F	          0x7F	          BB2:ADC2        0x7F	          0x7F            BB2:ADC2
+// 3	AXIS3	      BB3:ENC0[7:0]	  BB3:ENC1[7:0]	  BB2:ADC3        BB3:ENC0[7:0]	  BB3:ENC1[7:0]   BB2:ADC3
+// 4	AXIS4[0]	  BB1 _OK	        BB1 _OK	        BB1 _OK	        BB1 _OK	        BB1 _OK	        BB1 _OK
+// 4	AXIS4[1]	  BB2 _OK	        BB2 _OK	        BB2 _OK	        BB2 _OK	        BB2 _OK	        BB2 _OK
+// 4	AXIS4[2]	  BB3_OK	        BB3_OK	        BB3_OK	        BB3_OK	        BB3_OK	        BB3_OK
+// 4	AXIS4[3:4]  0b01	          0b10	          0b11	          0b01	          0b10	          0b11
+// 4	AXIS4[5]	  0	              0	              0	              1	              1	              1
+// 4	AXIS4[6:7]  0	              0	              0	              0	              0	              0
+// 5	B0	        BB3:B0	        BB1:B8	        BB2:B4	        BB3:B0	        BB1:B8	        BB2:B4
+// 5	B1	        BB3:B1	        BB1:B9	        BB2:B5	        BB3:B1	        BB1:B9	        BB2:B5
+// 5	B2	        BB3:B2	        BB1:B10	        BB2:B6	        BB3:B2	        BB1:B10	        BB2:B6
+// 5	B3	        BB3:B3	        BB1:B11	        BB2:B7	        BB3:B3	        BB1:B11	        BB2:B7
+// 5	B4	        BB3:B4	        BB1:B12	        BB2:B8	        BB3:B4	        BB1:B12	        BB2:B8
+// 5	B5	        BB1:B0	        BB1:B13	        BB2:B9	        BB1:B0	        BB1:B13	        BB2:B9
+// 5	B6	        BB1:B1	        BB1:B14	        BB2:B10	        BB1:B1	        BB1:B14	        BB2:B10
+// 5	B7	        BB1:B2	        BB1:B15	        BB2:B11	        BB1:B2	        BB1:B15	        BB2:B11
+// 6	B8	        BB1:B3	        BB1:B16	        BB2:B12	        BB1:B3	        BB1:B16	        BB2:B12
+// 6	B9	        BB1:B4	        BB2:B0	        BB2:B13	        BB1:B4	        BB2:B0	        BB2:B13
+// 6	B10	        BB1:B5	        BB2:B1	        BB2:B14	        BB1:B5	        BB2:B1	        BB2:B14
+// 6	B11	        BB1:B6	        BB2:B2	        BB2:B15	        BB1:B6	        BB2:B2	        BB2:B15
+// 6	B12	        BB1:B7	        BB2:B3	        BB2:B16	        BB1:B7	        BB2:B3	        BB2:B16
+// 6	B[14:13]	  0b01	          0b10	          0b11	          0b01	          0b10	          0b11
+// 6	B15	        0	              0	              0	              1	              1	              1
+
+// clang-format on
+void DriverStation::ComposeReport(char report[][kReportSize],
+                                  MEASUREMENT_DATA_S *bbMeasurements,
+                                  uint8_t board_id, int can_1_board,
+                                  int can_2_board) {
+  memset(report, 0, 3 * sizeof(*report));
+
+  report[0][0] = (char)(bbMeasurements[2].enc0 >> 8 & 0xFF);
+  report[0][1] = (char)((0x7F) & 0xFF);
+  report[0][2] = (char)((0x7F) & 0xFF);
+  report[0][3] = (char)(bbMeasurements[2].enc0 & 0xFF);
+  report[0][4] |= (char)(1 << (board_id - 1));
+  if (can_1_board != -1) {
+    report[0][4] |= (char)(1 << (can_1_board));
+  }
+  if (can_2_board != -1) {
+    report[0][4] |= (char)(1 << (can_2_board));
+  }
+  report[0][4] |= (char)(1 << 3);
+  report[0][5] = (char)(bbMeasurements[2].buttons & 0x1F);  // BB1 BTN[7:0]
+  report[0][5] |= (char)((bbMeasurements[0].buttons << 5) & 0xFF);
+  report[0][6] =
+      (char)((bbMeasurements[0].buttons >> 3) & 0x1F);  // BB1 BTN[12:8]
+  report[0][6] |= (char)(1 << 5);                       // BB1 BTN[14:13]
+
+  report[1][0] = (char)(((bbMeasurements[2].enc1) >> 8) & 0xFF);
+  report[1][1] = (char)((0x7F) & 0xFF);
+  report[1][2] = (char)((0x7F) & 0xFF);
+  report[1][3] = (char)((bbMeasurements[2].enc1) & 0xFF);
+  report[1][4] |= (char)(1 << (board_id - 1));
+  if (can_1_board != -1) {
+    report[1][4] |= (char)(1 << (can_1_board));
+  }
+  if (can_2_board != -1) {
+    report[1][4] |= (char)(1 << (can_2_board));
+  }
+  report[1][4] |= (char)(2 << 3);
+  report[1][5] =
+      (char)((bbMeasurements[0].buttons >> 8) & 0xFF);        // BB1 BTN[16:13]
+  report[1][6] = (char)((bbMeasurements[1].buttons) & 0x1F);  // BB2 BTN[3:0]
+  report[1][6] |= (char)(2 << 5);                             // BB2 BTN[14:13]
+
+  report[2][0] = (char)((bbMeasurements[1].abs0 >> 8) & 0xFF);
+  report[2][1] = (char)((bbMeasurements[1].abs1 >> 8) & 0xFF);
+  report[2][2] = (char)((bbMeasurements[1].abs2 >> 8) & 0xFF);
+  report[2][3] = (char)((bbMeasurements[1].abs3 >> 8) & 0xFF);
+  report[2][4] |= (char)(1 << (board_id - 1));
+  if (can_1_board != -1) {
+    report[2][4] |= (char)(1 << (can_1_board));
+  }
+  if (can_2_board != -1) {
+    report[2][4] |= (char)(1 << (can_2_board));
+  }
+  report[2][4] |= (char)(3 << 3);
+  report[2][5] =
+      (char)((bbMeasurements[1].buttons >> 5) & 0xFF);  // BB2 BTN[8:4]
+  report[2][6] =
+      (char)((bbMeasurements[1].buttons >> 13) & 0x1F);  // BB2 BTN[16:9]
+  // report[2][5] = (char)(bbMeasurements[2].buttons & 0x1F);  // BB3 BTN[4:0]
+  report[2][6] |= (char)(3 << 5);  // BB3 BTN[14:13]
+
+  if (board_id == 2) {
+    report[0][4] |= (char)(1 << 5);
+    report[1][4] |= (char)(1 << 5);
+    report[2][4] |= (char)(1 << 5);
+
+    report[0][6] |= (char)(1 << 7);  // BB1 BTN[15]
+    report[1][6] |= (char)(1 << 7);  // BB2 BTN[15]
+    report[2][6] |= (char)(1 << 7);  // BB3 BTN[15]
+  }
+
+  /*for ( int i = 0; i < 3; i++ ) {
+   printf("Report %d: 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X    ", i,
+  report[i][0], report[i][1], report[i][2], report[i][3], report[i][4],
+  report[i][5]);
+  }
+  printf("\n\r\n\r");*/
+}
+
+uint32_t DriverStation::ReadButtons() {
+  uint32_t buttons = 0;
+
+  buttons = ((PERIPHERAL_BITBAND(GPIOD_PDIR, 5) << 0) |     // BTN0	PTD5
+             (PERIPHERAL_BITBAND(GPIOC_PDIR, 0) << 1) |     // BTN1	PTC0
+             (PERIPHERAL_BITBAND(GPIOB_PDIR, 3) << 2) |     // BTN2	PTB3
+             (PERIPHERAL_BITBAND(GPIOB_PDIR, 2) << 3) |     // BTN3	PTB2
+             (PERIPHERAL_BITBAND(GPIOA_PDIR, 14) << 4) |    // BTN4	PTA14
+             (PERIPHERAL_BITBAND(GPIOE_PDIR, 26) << 5) |    // BTN5	PTE26
+             (PERIPHERAL_BITBAND(GPIOA_PDIR, 16) << 6) |    // BTN6	PTA16
+             (PERIPHERAL_BITBAND(GPIOA_PDIR, 15) << 7) |    // BTN7	PTA15
+             (PERIPHERAL_BITBAND(GPIOE_PDIR, 25) << 8) |    // BTN8	PTE25
+             (PERIPHERAL_BITBAND(GPIOA_PDIR, 5) << 9) |     // BTN9	PTA5
+             (PERIPHERAL_BITBAND(GPIOC_PDIR, 3) << 10) |    // BTN10	PTC3
+             (PERIPHERAL_BITBAND(GPIOC_PDIR, 7) << 11) |    // BTN11	PTC7
+             (PERIPHERAL_BITBAND(GPIOD_PDIR, 3) << 12) |    // BTN12	PTD3
+             (PERIPHERAL_BITBAND(GPIOD_PDIR, 2) << 13) |    // BTN13	PTD2
+             (PERIPHERAL_BITBAND(GPIOD_PDIR, 7) << 14) |    // BTN14	PTD7
+             (PERIPHERAL_BITBAND(GPIOB_PDIR, 10) << 15) |   // BTN15	PTB10
+             (PERIPHERAL_BITBAND(GPIOB_PDIR, 11) << 16) |   // BTN16	PTB11
+             (PERIPHERAL_BITBAND(GPIOD_PDIR, 4) << 17) |    // BTN17	PTD4
+             (PERIPHERAL_BITBAND(GPIOB_PDIR, 17) << 18) |   // BTN18	PTB17
+             (PERIPHERAL_BITBAND(GPIOB_PDIR, 16) << 19)) ^  // BTN19	PTB16
+            0x000FFFFF;
+
+  return buttons;
+}
+
+JoystickAdcReadings DriverStation::AdcReadJoystick(const DisableInterrupts &) {
+  JoystickAdcReadings r;
+
+  // ENC1_ABS_ADC	(PTE24) ADC0_SE17
+  ADC0_SC1A = 17;
+  while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+  }
+  // ABS2_ADC	(PTC1) ADC0_SE15
+  ADC0_SC1A = 15;
+  r.analog1 = ADC0_RA << 4;
+  while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+  }
+  // ABS3_ADC	(PTC2) ADC0_SE4b
+  ADC0_SC1A = 4;
+  r.analog2 = ADC0_RA << 4;
+  while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+  }
+  // ENC0_ABS_ADC	(PTD1) ADC0_SE5b
+  ADC0_SC1A = 5;
+  r.analog3 = ADC0_RA << 4;
+  while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+  }
+  r.analog0 = ADC0_RA << 4;
+
+  return r;
+}
+
+void DriverStation::AdcInitJoystick() {
+  AdcInitCommon();
+  // ENC1_ABS_ADC	(PTE24) ADC0_SE17
+  PORTE_PCR24 = PORT_PCR_MUX(0);
+  // ABS2_ADC	(PTC1) ADC0_SE15
+  PORTC_PCR1 = PORT_PCR_MUX(0);
+  // ABS3_ADC	(PTC2) ADC0_SE4b
+  PORTC_PCR2 = PORT_PCR_MUX(0);
+  // ENC0_ABS_ADC	(PTD1) ADC0_SE5b
+  PORTD_PCR1 = PORT_PCR_MUX(0);
+}
+
+void DriverStation::EnableLeds() {
+  // Set all the LED pins to output, drive strength enable (high drive since its
+  // an output), slew rate enable (slow since output) LED (on board)	PTC5
+  PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 1;
+  PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1);
+  PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1;
+  // LED0R	PTC6
+  PERIPHERAL_BITBAND(GPIOC_PDOR, 6) = 1;
+  PORTC_PCR6 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1);
+  PERIPHERAL_BITBAND(GPIOC_PDDR, 6) = 1;
+  // LED0Y	PTA17
+  PERIPHERAL_BITBAND(GPIOA_PDOR, 17) = 1;
+  PORTA_PCR17 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1);
+  PERIPHERAL_BITBAND(GPIOA_PDDR, 17) = 1;
+  // LED0G	PTC11
+  PERIPHERAL_BITBAND(GPIOC_PDOR, 11) = 1;
+  PORTC_PCR11 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1);
+  PERIPHERAL_BITBAND(GPIOC_PDDR, 11) = 1;
+  // LED1R	PTC10
+  PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = 1;
+  PORTC_PCR10 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1);
+  PERIPHERAL_BITBAND(GPIOC_PDDR, 10) = 1;
+  // LED1Y	PTC4
+  PERIPHERAL_BITBAND(GPIOC_PDOR, 4) = 1;
+  PORTC_PCR4 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1);
+  PERIPHERAL_BITBAND(GPIOC_PDDR, 4) = 1;
+  // LED1G	PTC8
+  PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = 1;
+  PORTC_PCR8 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1);
+  PERIPHERAL_BITBAND(GPIOC_PDDR, 8) = 1;
+}
+
+void DriverStation::EnableCan() {
+  // Set up the CAN pins.
+  // (PTA12) CAN0_TX
+  PORTA_PCR12 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+  // (PTA13) CAN0_RX
+  PORTA_PCR13 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+}
+
+void DriverStation::EnableEncoders() {
+  // Set up the encoder inputs
+  // ENC0_A	(PTB18) FTM2_QD_PHA
+  PORTB_PCR18 = PORT_PCR_MUX(6) /*| PORT_PCR_PE | PORT_PCR_PS*/;
+
+  // ENC0_B	(PTB19) FTM2_QD_PHB
+  PORTB_PCR19 = PORT_PCR_MUX(6) /*| PORT_PCR_PE | PORT_PCR_PS*/;
+
+  // ENC0_ABS	(PTD0) FTM3_CH0
+  PORTD_PCR0 = PORT_PCR_MUX(4) /*| PORT_PCR_PE | PORT_PCR_PS*/;
+
+  // ENC1_A	(PTB1) FTM1_QD_PHB
+  PORTB_PCR1 = PORT_PCR_MUX(6) /*| PORT_PCR_PE | PORT_PCR_PS*/;
+
+  // ENC1_B	(PTB0) FTM1_QD_PHA
+  PORTB_PCR0 = PORT_PCR_MUX(6) /*| PORT_PCR_PE | PORT_PCR_PS*/;
+
+  // ENC1_ABS	(PTD6) FTM0_CH6
+  PORTD_PCR6 = PORT_PCR_MUX(4) /*| PORT_PCR_PE | PORT_PCR_PS*/;
+}
+
+// Enable quadrature encoding.
+void DriverStation::EnableQD(LittleFTM *ftm, int encoder) {
+  ftm->MODE = FTM_MODE_WPDIS;
+  ftm->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
+
+  ftm->CNTIN = 0;
+  ftm->CNT = 0;
+  ftm->MOD = ENCODER_COUNTS_PER_REV;
+
+  if (encoder == 1) {
+    ftm->QDCTRL = FTM_QDCTRL_PHBPOL;
+  }
+
+  ftm->C0SC = FTM_CSC_ELSA;
+  ftm->C1SC = FTM_CSC_ELSA;
+
+  // Disable filters
+  ftm->FILTER = FTM_FILTER_CH0FVAL(0) | FTM_FILTER_CH1FVAL(0) |
+                FTM_FILTER_CH2FVAL(0) | FTM_FILTER_CH3FVAL(0);
+
+  // Use Quadrature Encoding.
+  ftm->QDCTRL |= FTM_QDCTRL_QUADEN;
+
+  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 */;
+
+  ftm->SYNC = FTM_SYNC_SWSYNC /* Flush everything out right now */;
+
+  // Wait for the software synchronization to finish.
+  while (ftm->SYNC & FTM_SYNC_SWSYNC) {
+  }
+
+  ftm->SC = FTM_SC_CLKS(1) /* Use the system clock */ |
+            FTM_SC_PS(0) /* Prescaler=64 */;
+
+  ftm->MODE &= ~FTM_MODE_WPDIS;
+}
+
+int DriverStation::ReadQuadrature(int encoderNum, uint16_t *encoderAngle) {
+  switch (encoderNum) {
+    case 0:
+      *encoderAngle = FTM2_CNT;
+      break;
+
+    case 1:
+      *encoderAngle = FTM1_CNT;
+      break;
+
+    default:
+      return -1;
+      break;
+  }
+  return 0;
+}
+
+void DriverStation::EnableGlitchFilter() {
+  // Enable 1KHz filter clock
+  PORTA_DFCR = 1;
+  PORTB_DFCR = 1;
+  PORTC_DFCR = 1;
+  PORTD_DFCR = 1;
+  PORTE_DFCR = 1;
+  // 10ms filter time
+  PORTA_DFWR = 10;
+  PORTB_DFWR = 10;
+  PORTC_DFWR = 10;
+  PORTD_DFWR = 10;
+  PORTE_DFWR = 10;
+  // TODO: validate the 10ms gitch filter, seems to work
+}
+
+void DriverStation::EnableButtons() {
+  // Set up the buttons. The LEDs pull them up to 5V, so the Teensy needs to not
+  // be set to pull up.
+  // BTN0	PTD5
+  PORTD_PCR5 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTD_DFER |= 1 << 5;
+
+  // BTN1	PTC0
+  PORTC_PCR0 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTC_DFER |= 1 << 0;
+
+  // BTN2	PTB3
+  PORTB_PCR3 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTB_DFER |= 1 << 3;
+
+  // BTN3	PTB2
+  PORTB_PCR2 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTB_DFER |= 1 << 2;
+
+  // BTN4	PTA14
+  PORTA_PCR14 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTA_DFER |= 1 << 14;
+
+  // BTN5	PTE26
+  PORTE_PCR26 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTE_DFER |= 1 << 26;
+
+  // BTN6	PTA16
+  PORTA_PCR16 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTA_DFER |= 1 << 16;
+
+  // BTN7	PTA15
+  PORTA_PCR15 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTA_DFER |= 1 << 15;
+
+  // BTN8	PTE25
+  PORTE_PCR25 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTE_DFER |= 1 << 25;
+
+  // BTN9	PTA5
+  PORTA_PCR5 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTA_DFER |= 1 << 5;
+
+  // BTN10	PTC3
+  PORTC_PCR3 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTC_DFER |= 1 << 3;
+
+  // BTN11	PTC7
+  PORTC_PCR7 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTC_DFER |= 1 << 7;
+
+  // BTN12	PTD3
+  PORTD_PCR3 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTD_DFER |= 1 << 3;
+
+  // BTN13	PTD2
+  PORTD_PCR2 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTD_DFER |= 1 << 2;
+
+  // BTN14	PTD7
+  PORTD_PCR7 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTD_DFER |= 1 << 7;
+
+  // BTN15	PTB10
+  PORTB_PCR10 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTB_DFER |= 1 << 10;
+
+  // BTN16	PTB11
+  PORTB_PCR11 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTB_DFER |= 1 << 11;
+
+  // BTN17	PTD4
+  PORTD_PCR4 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTD_DFER |= 1 << 4;
+  (PERIPHERAL_BITBAND(GPIOD_PDIR, 4) =
+       1);  // BTN17	PTD4 is being forced to be an input
+
+  // BTN18	PTB17
+  PORTB_PCR17 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTB_DFER |= 1 << 17;
+
+  // BTN19	PTB16
+  PORTB_PCR16 = PORT_PCR_MUX(1) | PORT_PCR_PFE;
+  PORTB_DFER |= 1 << 16;
+}
+
+void DriverStation::DisableLeds() {
+  // LED (on board)	PTC5
+  PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 0;
+  // LED0R	PTC6
+  PERIPHERAL_BITBAND(GPIOC_PDOR, 6) = 0;
+  // LED0Y	PTA17
+  PERIPHERAL_BITBAND(GPIOA_PDOR, 17) = 0;
+  // LED0G	PTC11
+  PERIPHERAL_BITBAND(GPIOC_PDOR, 11) = 0;
+  // LED1R	PTC10
+  PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = 0;
+  // LED1Y	PTC4
+  PERIPHERAL_BITBAND(GPIOC_PDOR, 4) = 0;
+  // LED1G	PTC8
+  PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = 1;
+}
+
+int DriverStation::Run() {
+  // 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);
+
+  EnableLeds();
+
+  EnableGlitchFilter();
+
+  EnableCan();
+
+  EnableEncoders();
+
+  EnableButtons();
+
+  delay(100);
+
+  teensy::UsbDevice usb_device(0, 0x16c0, 0x0492);
+  usb_device.SetManufacturer("FRC 971 Spartan Robotics");
+  usb_device.SetProduct("Spartan Joystick Board");
+
+  teensy::HidFunction joystick0(&usb_device, kReportSize);
+  joystick0.set_report_descriptor(
+      ::std::string(kReportDescriptor1, sizeof(kReportDescriptor1)));
+
+  teensy::HidFunction joystick1(&usb_device, kReportSize);
+  joystick1.set_report_descriptor(
+      ::std::string(kReportDescriptor1, sizeof(kReportDescriptor1)));
+
+  teensy::HidFunction joystick2(&usb_device, kReportSize);
+  joystick2.set_report_descriptor(
+      ::std::string(kReportDescriptor1, sizeof(kReportDescriptor1)));
+
+  teensy::AcmTty tty1(&usb_device);
+  PrintingParameters printing_parameters;
+  printing_parameters.stdout_tty = &tty1;
+
+  const ::std::unique_ptr<PrintingImplementation> printing =
+      CreatePrinting(printing_parameters);
+  usb_device.Initialize();
+  printing->Initialize();
+
+  AdcInitJoystick();
+
+  EnableQD(FTM1, 1);
+  EnableQD(FTM2, 0);
+
+  // Leave the LEDs on for a bit longer.
+  delay(300);
+  printf("Done starting up\n");
+
+  // Done starting up, now turn all the LEDs off.
+  DisableLeds();
+
+  board_config.board_id = 0;
+  board_config.processor_index = 0;
+  board_config.can_id0 = 0;
+  board_config.can_id1 = 0;
+
+  uint32_t button18 =
+      PERIPHERAL_BITBAND(GPIOB_PDIR, 17) ^ 0x1;  // BTN18	PTB17
+  uint32_t button19 =
+      PERIPHERAL_BITBAND(GPIOB_PDIR, 16) ^ 0x1;  // BTN19	PTB16
+
+  board_config.board_id = (button19 << 1) | button18;
+
+  board_config.processor_index = ProcessorIndex();
+
+  switch (board_config.board_id) {
+    case 1:
+      board_config.can_id0 = 2;
+      board_config.can_id1 = 3;
+      break;
+
+    case 2:
+      board_config.can_id0 = 1;
+      board_config.can_id1 = 3;
+      break;
+
+    case 3:
+      board_config.can_id0 = 2;
+      board_config.can_id1 = 1;
+      break;
+
+    default:
+      board_config.board_id = 0;
+      break;
+  }
+
+  can_init(board_config.can_id0, board_config.can_id1);
+
+  SendJoystickData(&joystick0, &joystick1, &joystick2,
+                   board_config.processor_index);
+
+  return 0;
+}
+
+extern "C" int main(void) {
+  frc971::motors::DriverStation driverStation;
+  return driverStation.Run();
+}
+
+}  // namespace motors
+}  // namespace frc971
\ No newline at end of file
diff --git a/motors/driver_station.h b/motors/driver_station.h
new file mode 100644
index 0000000..244fe3f
--- /dev/null
+++ b/motors/driver_station.h
@@ -0,0 +1,132 @@
+#ifndef MOTORS_BUTTON_BOARD_H_
+#define MOTORS_BUTTON_BOARD_H_
+
+#include "motors/peripheral/adc.h"
+#include "motors/peripheral/can.h"
+#include "motors/usb/cdc.h"
+#include "motors/usb/hid.h"
+#include "motors/util.h"
+
+#define NUM_ENCODERS 2
+#define ENCODER_COUNTS_PER_REV 0x1000
+#define ENCODER_MOD (ENCODER_COUNTS_PER_REV - 1)
+#define ENCODER_RESET_TIME_MS 500
+#define ENCODER_FILTER_EXPONENT 6  // depth of IIR filtering where val 2^n
+#define MAX_FILTER_DELTA \
+  200  // reset encoder filter when the input value exceeds filtered value by
+       // this amount
+#define MAX_DUTY_CYCLE_DELTA 500
+#define BUTTON_BOARD_COUNT 3
+#define PROCESSOR_ID_COUNT 3  // number of driver stations
+#define PRINT_OFFSETS 0       // used to debug the zero's of the joystick
+#define SCALED_VALUE_MAX 0xFFFF
+
+namespace frc971 {
+namespace motors {
+
+struct JoystickAdcReadings {
+  uint16_t analog0, analog1, analog2, analog3;
+};
+
+enum abs_Measurement_State {
+  INIT_ABS,      // Absolute position of encoder on startup, which is treated as
+                 // center
+  START_PERIOD,  // Period reading needs to stay for to be considered stable.
+                 // Rise-to-rise time of PWM
+  WAIT_PERIOD_DONE,
+  START_WIDTH,  // Rise-to-fall time of PWM
+  WAIT_WIDTH_DONE
+};
+
+typedef struct {
+  uint16_t enc0_trim;
+  uint16_t enc1_trim;
+} ENCODER_TRIMS_S;
+
+typedef struct {
+  abs_Measurement_State state;
+  uint32_t period;
+  uint32_t width;
+  uint32_t dutycycle;
+  uint32_t last_erronious_dutycycle;  // Tracks out-of-range measurements so
+                                      // that duty cycle can be reset
+  bool intialized;
+} ABS_POSITION_S;
+
+typedef struct {
+  uint16_t angle;
+  uint16_t offset;
+  uint16_t resetTimer_ms;
+  uint16_t filtered_duty_cycle;
+  uint32_t angle_filter;
+  // The encoder value at center.
+  uint16_t enc_trim;
+  // The tared min and max values of the limits.
+  uint16_t enc_min;
+  uint16_t enc_max;
+} ENCODER_DATA_S;
+
+// Identifies which board is used.
+typedef struct {
+  uint32_t board_id;  // There are three boards, one for each Teensy
+  uint32_t
+      processor_index;  // There are two processors, one for each driver station
+  uint32_t can_id0;
+  uint32_t can_id1;
+} BOARD_CONFIG_S;
+
+typedef struct {
+  uint32_t buttons;
+  uint16_t enc0;
+  uint16_t enc1;
+  uint16_t abs0;
+  uint16_t abs1;
+  uint16_t abs2;
+  uint16_t abs3;
+} MEASUREMENT_DATA_S;
+
+class DriverStation {
+ public:
+  DriverStation() = default;
+  int Run();
+  // Number of bytes per CAN packet
+  static constexpr uint16_t kReportSize = 1 * 5 + 2;
+
+ private:
+  BOARD_CONFIG_S board_config;
+
+  // Contains the while loop to read inputs, pack data, and send it via CAN and
+  // USB
+  void SendJoystickData(teensy::HidFunction *joystick0,
+                        teensy::HidFunction *joystick1,
+                        teensy::HidFunction *joystick2,
+                        uint32_t processor_index);
+  void EnableLeds();
+  void EnableCan();
+  void EnableEncoders();
+  void EnableQD(LittleFTM *ftm, int encoder);
+  void EnableGlitchFilter();
+  void EnableButtons();
+  void DisableLeds();
+  void AdcInitJoystick();
+  JoystickAdcReadings AdcReadJoystick(const DisableInterrupts &);
+  void ComposeReport(char report[][kReportSize],
+                     MEASUREMENT_DATA_S *bbMeasurements, uint8_t board_id,
+                     int can_1_board, int can_2_board);
+  int ReadQuadrature(int encoderNum, uint16_t *encoderAngle);
+  int MeasureAbsPosition(uint32_t encoder_id, ABS_POSITION_S *abs_position);
+  int DetermineEncoderValues(ENCODER_DATA_S *enc, ABS_POSITION_S *absAngle,
+                             int encoderNum, uint16_t resetTime_ms);
+  void ZeroMeasurements(MEASUREMENT_DATA_S *bbMeasurements, uint32_t board);
+  int UpdateMeasurementsFromCAN(MEASUREMENT_DATA_S *bbMeasurements,
+                                uint8_t *canRX_data);
+  void PackMeasurementsToCAN(MEASUREMENT_DATA_S *bbMeasurements,
+                             uint8_t *canTX_data);
+  uint32_t ReadButtons();
+
+  int MeasurementsToJoystick(MEASUREMENT_DATA_S bbMeasurement);
+};
+}  // namespace motors
+}  // namespace frc971
+
+#endif  //  MOTORS_BUTTON_BOARD_H_
\ No newline at end of file