Merge "Import apache2 for testing"
diff --git a/frc971/imu/ADIS16505.cc b/frc971/imu/ADIS16505.cc
new file mode 100644
index 0000000..dfc09cb
--- /dev/null
+++ b/frc971/imu/ADIS16505.cc
@@ -0,0 +1,694 @@
+/**
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdio.h>
+
+#include <algorithm>
+#include <cstring>
+
+#include "hardware/dma.h"
+#include "hardware/gpio.h"
+#include "hardware/irq.h"
+#include "hardware/pio.h"
+#include "hardware/pwm.h"
+#include "hardware/spi.h"
+#include "hardware/timer.h"
+#include "pico/binary_info.h"
+#include "pico/bootrom.h"
+#include "pico/double.h"
+#include "pico/stdlib.h"
+#include "quadrature_encoder.pio.h"
+
+// Pinout definitions for the imu
+#define RST_IMU 22
+#define DR_IMU 20
+#define SYNC_IMU 21
+#define DIN_IMU 19
+#define DOUT_IMU 16
+#define SCLK_IMU 18
+#define CS_IMU 17
+
+// Pinout definitions for spi to the pi through the differential drivers
+#define DR_PI 14
+#define MOSI_PI 12
+#define MISO_PI 11
+#define SCK_PI 10
+#define CS_PI 13
+
+// The two drivetrain encoders
+#define ENC1_A 6
+#define ENC1_B 7
+#define ENC2_A 0
+#define ENC2_B 1
+
+// Backup outputs to the roborio
+#define RATE_PWM 2
+#define HEADING_PWM 4
+
+#define PWM_FREQ_HZ 200
+// PWM counts to this before wrapping
+#define PWM_TOP 62499
+
+#define SPI_IMU spi0
+#define SPI_PI spi1
+#define WRITE_BIT 0x8000
+
+// length in half-words of the buffer for communicating with the IMU
+// includes 2 non-data fields
+// the first element is used for recieving zeros while the initial request is
+// made the last element is the checksum
+#define IMU_NUM_ITEMS 17
+
+// length in bytes of the packet to the pi
+#define PI_NUM_ITEMS 42
+
+// number of samples for zeroing the z-gyro axis
+#define YAW_BUF_LEN 5000
+
+#define EXPECTED_PROD_ID 0x4079
+
+// Registers on the ADIS16505
+#define GLOB_CMD 0x68
+#define DIAG_STAT 0x02
+#define FIRM_REV 0x6C
+#define FIRM_DM 0x6E
+#define FIRM_Y 0x70
+#define PROD_ID 0x72
+#define SERIAL_NUM 0x74
+#define FILT_CTRL 0x5C
+#define MSC_CTRL 0x60
+#define DEC_RATE 0x64
+
+// TODO: replace with aos/events/logging/crc32.h
+const uint32_t kCrc32Table[256] = {
+    0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f,
+    0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
+    0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2,
+    0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+    0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9,
+    0xfa0f3d63, 0x8d080df5, 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
+    0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 0x35b5a8fa, 0x42b2986c,
+    0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+    0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423,
+    0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
+    0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 0x76dc4190, 0x01db7106,
+    0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+    0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d,
+    0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
+    0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950,
+    0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+    0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7,
+    0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
+    0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa,
+    0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+    0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81,
+    0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
+    0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84,
+    0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+    0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb,
+    0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
+    0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e,
+    0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+    0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55,
+    0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
+    0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28,
+    0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+    0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f,
+    0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
+    0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242,
+    0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+    0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69,
+    0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
+    0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc,
+    0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+    0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693,
+    0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
+    0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d};
+
+// Buffer to start a burst read and recive 15 items + checksum
+static uint16_t imu_write_buf[IMU_NUM_ITEMS] = {0x6800, 0, 0, 0, 0, 0, 0, 0, 0,
+                                                0,      0, 0, 0, 0, 0, 0, 0};
+// recieves a byte of zeros followed by 15 items + checksum
+static uint16_t imu_data_buffer[IMU_NUM_ITEMS];
+
+static dma_channel_config imu_tx_config;
+static dma_channel_config imu_rx_config;
+static uint imu_dma_tx;
+static uint imu_dma_rx;
+
+// The packet to the pi contains the whole burst read from the IMU (30 bytes)
+// followed by a timestamp, encoder values, and checksum
+// DIAG_STAT, X_GYRO_LOW, X_GYRO_OUT, Y_GYRO_LOW, Y_GYRO_OUT, Z_GYRO_LOW,
+// Z_GYRO_OUT, X_ACCL_LOW, X_ACCL_OUT, Y_ACCL_LOW, Y_ACCL_OUT, Z_ACCL_LOW,
+// Z_ACCL_OUT, TEMP_OUT, DATA_CNTR, TIMESTAMP (32 bit), ENC1_POS, ENC2_POS,
+// CHECKSUM (32-bit)
+
+// the staging buffer gets updated everytime new data is received
+static uint8_t pi_staging_buffer[PI_NUM_ITEMS];
+// the data from the staging buffer is latched into the sending buffer while the
+// pi is reading
+static uint8_t pi_sending_buffer[PI_NUM_ITEMS];
+
+// for now just recieves zeros
+// but is useful because directing the dma to a nullptr messes up the transfer
+// finished interrupt
+static uint8_t pi_recieve_buffer[PI_NUM_ITEMS];
+
+static dma_channel_config pi_tx_config;
+static dma_channel_config pi_rx_config;
+static uint pi_dma_tx;
+static uint pi_dma_rx;
+
+// zeroed yaw from the latest message from the imu
+static double yaw = 0;
+static double yaw_rate = 0;
+
+// TODO: fields for the janky, not encapsulated zeroing function
+static double yaw_rate_offset = 0;
+static bool yaw_zeroed = false;
+
+static int16_t encoder1_count = 0;
+static int16_t encoder2_count = 0;
+
+static uint32_t data_collect_timestamp = 0;
+
+// useful debug counters
+static uint checksum_mismatch_count;
+static uint message_recieved_count;
+static uint message_sent_count;
+// the number of times we had to defer sending new data because another
+// transfer was still in progress
+static uint timing_overrun_count;
+
+// the time it takes from servicing DR_IMU to finishing the transfer to the pi
+static int send_time = 0;
+
+void data_ready();
+void maybe_send_pi_packet();
+
+void gpio_irq_handler(uint gpio, uint32_t events) {
+  if (gpio == DR_IMU && (events & GPIO_IRQ_EDGE_RISE)) {
+    data_ready();
+  }
+}
+
+static inline void cs_select() {
+  gpio_put(CS_IMU, 0);  // Active low
+  sleep_us(1);
+}
+
+static inline void cs_deselect() { gpio_put(CS_IMU, 1); }
+
+static uint16_t read_register(uint8_t reg_low) {
+  // For this particular device, we send the device the register we want to read
+  // first, then subsequently read from the device.
+  uint16_t output;
+
+  // The low byte of the register is first in the IMU's memory
+  uint16_t reg = ((uint16_t)reg_low) << 8;
+
+  cs_select();
+  spi_write16_blocking(SPI_IMU, &reg, 1);
+  cs_deselect();
+  sleep_us(20);  // wait the stall period
+  cs_select();
+  spi_read16_blocking(SPI_IMU, 0x0000, &output, 1);
+  cs_deselect();
+
+  return output;
+}
+
+static void write_register(uint8_t reg, uint16_t value) {
+  uint16_t low_byte = ((uint16_t)value) & 0xFF;
+  uint16_t high_byte = ((uint16_t)value) >> 8;
+
+  uint16_t command = (((uint16_t)reg) << 8) | low_byte | WRITE_BIT;
+
+  cs_select();
+  spi_write16_blocking(SPI_IMU, &command, 1);
+  cs_deselect();
+
+  sleep_us(20);  // wait the stall period
+
+  command = ((((uint16_t)reg) + 1) << 8) | high_byte | WRITE_BIT;
+
+  cs_select();
+  spi_write16_blocking(SPI_IMU, &command, 1);
+  cs_deselect();
+}
+
+static void adis16505_reset() {
+  gpio_put(RST_IMU, 0);  // Active low
+  sleep_ms(10);
+  gpio_put(RST_IMU, 1);  // Active low
+  sleep_ms(310);
+}
+
+static uint32_t calculate_crc32(uint8_t *data, size_t len) {
+  uint32_t crc = 0xFFFFFFFF;
+  for (size_t i = 0; i < len; i++) {
+    crc = kCrc32Table[(crc ^ data[i]) & 0xFF] ^ (crc >> 8);
+  }
+  return crc;
+}
+
+static bool check_checksum(uint16_t *buf) {
+  uint16_t sum = 0;
+  for (int i = 1; i < IMU_NUM_ITEMS - 1; i++) {
+    uint16_t low = buf[i] & 0xff;
+    uint16_t high = (buf[i] >> 8) & 0xff;
+
+    sum += high + low;
+  }
+
+  uint16_t checksum = buf[IMU_NUM_ITEMS - 1];
+  return sum == checksum;
+}
+
+static void zero_yaw(double yaw) {
+  static double yaw_buffer[YAW_BUF_LEN];
+  static int num_items;
+
+  if (num_items < YAW_BUF_LEN) {
+    yaw_buffer[num_items] = yaw;
+    num_items++;
+  } else if (!yaw_zeroed) {
+    double sum = 0;
+    for (int i = 0; i < YAW_BUF_LEN; i++) {
+      sum += yaw_buffer[i];
+    }
+    yaw_rate_offset = -sum / YAW_BUF_LEN;
+    yaw = 0;
+    printf("offset: %f\n", yaw_rate_offset);
+    yaw_zeroed = true;
+  }
+}
+
+void pack_pi_packet() {
+  // zero the buffer
+  for (int i = 0; i < PI_NUM_ITEMS; i++) {
+    pi_staging_buffer[i] = 0;
+  }
+
+  // skip empty item
+  uint8_t *imu_packet = (uint8_t *)(imu_data_buffer + 1);
+  // skip empty item and checksum and convert to bytes
+  const size_t imu_packet_len = (IMU_NUM_ITEMS - 2) * sizeof(uint16_t);
+
+  memcpy(pi_staging_buffer, imu_packet, imu_packet_len);
+  memcpy(pi_staging_buffer + imu_packet_len, &data_collect_timestamp, 4);
+  memcpy(pi_staging_buffer + imu_packet_len + 4, &encoder1_count, 2);
+  memcpy(pi_staging_buffer + imu_packet_len + 6, &encoder2_count, 2);
+
+  // exclude the part of the buffer that will be the checksum
+  uint32_t crc = calculate_crc32(pi_staging_buffer, PI_NUM_ITEMS - 4);
+  memcpy(pi_staging_buffer + imu_packet_len + 8, &crc, 4);
+
+  static_assert(PI_NUM_ITEMS == imu_packet_len +
+                                    sizeof(data_collect_timestamp) +
+                                    sizeof(encoder1_count) +
+                                    sizeof(encoder2_count) + sizeof(crc),
+                "PI_NUM_ITEMS needs to be able to hold the imu message + the "
+                "timestamp + the encoders + the checksum");
+}
+
+void data_ready() {
+  // save timestamp
+  data_collect_timestamp = time_us_32();
+
+  // read encoders
+  quadrature_encoder_request_count(pio0, 0);
+  quadrature_encoder_request_count(pio0, 1);
+
+  cs_select();
+  dma_channel_configure(imu_dma_tx, &imu_tx_config,
+                        &spi_get_hw(SPI_IMU)->dr,  // write address
+                        &imu_write_buf,            // read address
+                        IMU_NUM_ITEMS,  // element count (each element is of
+                                        // size transfer_data_size)
+                        false);         // don't start yet
+  dma_channel_configure(imu_dma_rx, &imu_rx_config,
+                        &imu_data_buffer,          // write address
+                        &spi_get_hw(SPI_IMU)->dr,  // read address
+                        IMU_NUM_ITEMS,  // element count (each element is of
+                                        // size transfer_data_size)
+                        false);         // don't start yet
+  dma_start_channel_mask((1u << imu_dma_tx) | (1u << imu_dma_rx));
+
+  encoder1_count = quadrature_encoder_fetch_count(pio0, 0);
+  encoder2_count = quadrature_encoder_fetch_count(pio0, 1);
+}
+
+void imu_read_finished() {
+  cs_deselect();
+
+  // TODO: check status and if necessary set flag to reset in main loop
+
+  message_recieved_count++;
+  bool checksum_passed = check_checksum(imu_data_buffer);
+  if (!checksum_passed) {
+    checksum_mismatch_count++;
+  } else {
+    static const double dt = 0.0005;  // seconds
+    int32_t z_gyro_out;
+    memcpy(&z_gyro_out, imu_data_buffer + 6, 4);
+    yaw_rate = (double)z_gyro_out / 655360.0 + yaw_rate_offset;  // degrees
+    yaw += yaw_rate * dt;
+
+    // 50% is 0; -2000 deg/sec to 2000 deg/sec
+    uint16_t rate_level =
+        (std::clamp(yaw_rate, -2000.0, 2000.0) / 4000.0 + 0.5) * PWM_TOP;
+    pwm_set_gpio_level(RATE_PWM, rate_level);
+
+    // 0 to 360
+    uint16_t heading_level = (int16_t)(fmod(yaw, 360) / 360.0 * PWM_TOP);
+    pwm_set_gpio_level(HEADING_PWM, heading_level);
+
+    // fill out message and add it to the queue
+    pack_pi_packet();
+  }
+
+  // TODO: this has a sleep in it
+  // stay in sync with the pi by resetting the spi fifos each transfer
+  spi_init(SPI_PI, 2000 * 1000);
+  spi_set_slave(SPI_PI, true);
+  // these settings were the most stable even though the PI is using
+  // polarity 1 and phase 1
+  spi_set_format(SPI_PI, 8, SPI_CPOL_0, SPI_CPHA_1, SPI_MSB_FIRST);
+
+  maybe_send_pi_packet();
+
+  // clear the interrupt
+  dma_hw->ints0 = 1u << imu_dma_rx;
+}
+
+void maybe_send_pi_packet() {
+  // active low; if the pi isn't connected/booted, this will
+  // also look active
+  bool cs_asserted_or_unplugged = !gpio_get(CS_PI);
+
+  if (cs_asserted_or_unplugged) {
+    // the pi is still recieving something else from us
+    timing_overrun_count++;
+    return;
+  }
+
+  gpio_put(DR_PI, 1);
+  memcpy(pi_sending_buffer, pi_staging_buffer, PI_NUM_ITEMS);
+
+  dma_channel_configure(pi_dma_tx, &pi_tx_config,
+                        &spi_get_hw(SPI_PI)->dr,  // write address
+                        &pi_sending_buffer,       // read address
+                        PI_NUM_ITEMS,  // element count (each element is
+                                       // of size transfer_data_size)
+                        false);        // don't start yet
+  dma_channel_configure(pi_dma_rx, &pi_rx_config,
+                        &pi_recieve_buffer,       // write address
+                        &spi_get_hw(SPI_PI)->dr,  // read address
+                        PI_NUM_ITEMS,  // element count (each element is of
+                                       // size transfer_data_size)
+                        false);        // don't start yet
+
+  // start hardware calculation of the CRC-32; currently calculated in software
+  dma_sniffer_enable(pi_dma_tx, 0x0, true);
+  dma_hw->sniff_data = 0;
+
+  // start both at exactly the same time
+  dma_start_channel_mask((1u << pi_dma_tx) | (1u << pi_dma_rx));
+}
+
+void pi_transfer_finished() {
+  message_sent_count++;
+  gpio_put(DR_PI, 0);
+
+  send_time = time_us_32() - data_collect_timestamp;
+
+  dma_hw->ints1 = 1u << pi_dma_rx;
+}
+
+int main() {
+  stdio_init_all();
+
+  // Use 1MHz with the IMU as that is the limit for burst mode.
+  spi_init(SPI_IMU, 1000 * 1000);
+  spi_set_format(SPI_IMU, 16, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
+  gpio_set_function(DOUT_IMU, GPIO_FUNC_SPI);
+  gpio_set_function(SCLK_IMU, GPIO_FUNC_SPI);
+  gpio_set_function(DIN_IMU, GPIO_FUNC_SPI);
+  // Make the SPI pins available to picotool
+  bi_decl(bi_3pins_with_func(DOUT_IMU, DIN_IMU, SCLK_IMU, GPIO_FUNC_SPI));
+
+  // Chip select is active-low, so we'll initialise it to a driven-high state
+  gpio_init(CS_IMU);
+  gpio_set_dir(CS_IMU, GPIO_OUT);
+  gpio_put(CS_IMU, 1);
+  // Make the CS pin available to picotool
+  bi_decl(bi_1pin_with_name(CS_IMU, "IMU CS"));
+
+  // Reset is active-low, so we'll initialise it to a driven-high state
+  gpio_init(RST_IMU);
+  gpio_set_dir(RST_IMU, GPIO_OUT);
+  gpio_put(RST_IMU, 1);
+  // Make the RST pin available to picotool
+  bi_decl(bi_1pin_with_name(RST_IMU, "IMU RESET"));
+
+  imu_dma_tx = dma_claim_unused_channel(true);
+  imu_dma_rx = dma_claim_unused_channel(true);
+
+  // We set the outbound DMA to transfer from a memory buffer to the SPI
+  // transmit FIFO paced by the SPI TX FIFO DREQ The default is for the read
+  // address to increment every element (in this case 2 bytes - DMA_SIZE_16) and
+  // for the write address to remain unchanged.
+
+  imu_tx_config = dma_channel_get_default_config(imu_dma_tx);
+  channel_config_set_transfer_data_size(&imu_tx_config, DMA_SIZE_16);
+  channel_config_set_dreq(&imu_tx_config, spi_get_dreq(SPI_IMU, true));
+  channel_config_set_read_increment(&imu_tx_config, true);
+  channel_config_set_write_increment(&imu_tx_config, false);
+
+  // We set the inbound DMA to transfer from the SPI receive FIFO to a memory
+  // buffer paced by the SPI RX FIFO DREQ We configure the read address to
+  // remain unchanged for each element, but the write address to increment (so
+  // data is written throughout the buffer)
+  imu_rx_config = dma_channel_get_default_config(imu_dma_rx);
+  channel_config_set_transfer_data_size(&imu_rx_config, DMA_SIZE_16);
+  channel_config_set_dreq(&imu_rx_config, spi_get_dreq(SPI_IMU, false));
+  channel_config_set_read_increment(&imu_rx_config, false);
+  channel_config_set_write_increment(&imu_rx_config, true);
+
+  // Tell the DMA to raise IRQ line 0 when the channel finishes a block
+  dma_channel_set_irq0_enabled(imu_dma_rx, true);
+
+  // Configure the processor to run dma_handler() when DMA IRQ 0 is asserted
+  irq_set_exclusive_handler(DMA_IRQ_0, imu_read_finished);
+  irq_set_enabled(DMA_IRQ_0, true);
+
+  // Use 2MHz with the PI as that is the maximum speed for the pico
+  gpio_set_function(MISO_PI, GPIO_FUNC_SPI);
+  gpio_set_function(MOSI_PI, GPIO_FUNC_SPI);
+  gpio_set_function(SCK_PI, GPIO_FUNC_SPI);
+  gpio_set_function(CS_PI, GPIO_FUNC_SPI);
+  // Make the SPI pins available to picotool
+  bi_decl(bi_3pins_with_func(DOUT_IMU, DIN_IMU, SCLK_IMU, GPIO_FUNC_SPI));
+
+  gpio_init(DR_PI);
+  gpio_set_dir(DR_PI, GPIO_OUT);
+  gpio_put(DR_PI, 0);
+  // Make the CS pin available to picotool
+  bi_decl(bi_1pin_with_name(DR_PI, "DATA READY PI"));
+
+  pi_dma_tx = dma_claim_unused_channel(true);
+  pi_dma_rx = dma_claim_unused_channel(true);
+
+  // We set the outbound DMA to transfer from a memory buffer to the SPI
+  // transmit FIFO paced by the SPI TX FIFO DREQ The default is for the read
+  // address to increment every element (in this case 2 bytes - DMA_SIZE_16) and
+  // for the write address to remain unchanged.
+
+  pi_tx_config = dma_channel_get_default_config(pi_dma_tx);
+  channel_config_set_transfer_data_size(&pi_tx_config, DMA_SIZE_8);
+  channel_config_set_dreq(&pi_tx_config, spi_get_dreq(SPI_PI, true));
+  channel_config_set_read_increment(&pi_tx_config, true);
+  channel_config_set_write_increment(&pi_tx_config, false);
+
+  // We set the inbound DMA to transfer from the SPI receive FIFO to a memory
+  // buffer paced by the SPI RX FIFO DREQ We configure the read address to
+  // remain unchanged for each element, but the write address to increment (so
+  // data is written throughout the buffer)
+  pi_rx_config = dma_channel_get_default_config(pi_dma_rx);
+  channel_config_set_transfer_data_size(&pi_rx_config, DMA_SIZE_8);
+  channel_config_set_dreq(&pi_rx_config, spi_get_dreq(SPI_PI, false));
+  channel_config_set_read_increment(&pi_rx_config, false);
+  channel_config_set_write_increment(&pi_rx_config, true);
+
+  // Tell the DMA to raise IRQ line 1 when the channel finishes a block
+  dma_channel_set_irq1_enabled(pi_dma_rx, true);
+
+  // Configure the processor to run dma_handler() when DMA IRQ 0 is asserted
+  irq_set_exclusive_handler(DMA_IRQ_1, pi_transfer_finished);
+  irq_set_enabled(DMA_IRQ_1, true);
+
+  /* All IRQ priorities are initialized to PICO_DEFAULT_IRQ_PRIORITY by the pico
+   * runtime at startup.
+   *
+   * Handler             | Interrupt    | Cause of interrupt
+   * --------------------|--------------|---------------------------------------
+   * imu_read_finished   | DMA_IRQ_0    | When the dma read from the imu is done
+   * pi_transfer_finished| DMA_IRQ_1    | When the dma read to the pi is
+   * done data_ready     | IO_IRQ_BANK0 | On the rising edge of DR_IMU
+   */
+
+  // Tell the GPIOs they are allocated to PWM
+  gpio_set_function(HEADING_PWM, GPIO_FUNC_PWM);
+  gpio_set_function(RATE_PWM, GPIO_FUNC_PWM);
+
+  // Find out which PWM slice is connected to each gpio pin
+  uint heading_slice = pwm_gpio_to_slice_num(HEADING_PWM);
+  uint rate_slice = pwm_gpio_to_slice_num(RATE_PWM);
+
+  /* frequency_pwm = f_sys / ((TOP + 1) * (CSR_PHASE_CORRECT + 1) * (DIV_INT +
+   * DIV_FRAC / 16))
+   *
+   * f_sys = 125 mhz
+   * CSR_PHASE_CORRECT = 0; no phase correct
+   * TARGET_FREQ = 200 hz
+   *
+   * 125 mhz / x = 200 hz * 62500
+   *
+   * TOP = 62499
+   * DIV_INT = 10
+   */
+
+  float divisor = clock_get_hz(clk_sys) / (PWM_TOP + 1) / PWM_FREQ_HZ;
+
+  pwm_config cfg = pwm_get_default_config();
+  pwm_config_set_clkdiv_mode(&cfg, PWM_DIV_FREE_RUNNING);
+  pwm_config_set_clkdiv(&cfg, divisor);
+  pwm_config_set_wrap(&cfg, PWM_TOP);
+
+  pwm_init(heading_slice, &cfg, true);
+  pwm_init(rate_slice, &cfg, true);
+
+  // the two state machine instances use the same program memory from pio0
+  uint offset = pio_add_program(pio0, &quadrature_encoder_program);
+  quadrature_encoder_program_init(pio0, 0, offset, ENC1_A, 0);
+  quadrature_encoder_program_init(pio0, 1, offset, ENC2_A, 0);
+
+  sleep_ms(3 * 1000);
+
+  printf("Hello ADIS16505\n");
+
+  printf(
+      "System clock: %d hz\n"
+      "PWM target frequency: %d hz, PWM clock divisor: "
+      "%f, PWM TOP: %d\n",
+      clock_get_hz(clk_sys), PWM_FREQ_HZ, divisor, PWM_TOP);
+
+  while (true) {
+    adis16505_reset();
+    //   See if SPI is working - interrogate the device for its product ID
+    //   number, should be 0x4079
+    uint16_t id = read_register(PROD_ID);
+    if (id == EXPECTED_PROD_ID) {
+      printf("Product id: 0x%04x == expected 0x%04x\n", id, EXPECTED_PROD_ID);
+      break;
+    } else {
+      printf("Got 0x%04x for prod id, expected 0x%04x\ntrying again\n", id,
+             EXPECTED_PROD_ID);
+    }
+  }
+
+  uint16_t firmware_revision = read_register(FIRM_REV);
+  uint16_t firmware_day_month = read_register(FIRM_DM);
+  uint16_t firmware_year = read_register(FIRM_Y);
+  uint16_t serial_number = read_register(SERIAL_NUM);
+
+  printf(
+      "Firmware revision: 0x%04x, \nFirmware day month: 0x%04x, \nFirmware "
+      "year: "
+      "0x%04x, \nSerial number: 0x%04x, \n",
+      firmware_revision, firmware_day_month, firmware_year, serial_number);
+
+  // run self test
+  int num_failures = 0;
+  while (true) {
+    write_register(GLOB_CMD, 1u << 2);
+    sleep_ms(24);
+    uint16_t diag_stat = read_register(DIAG_STAT);
+
+    // check the sensor failure bit
+    bool sensor_failure = diag_stat & (1u << 5);
+    printf("Diag stat: 0b%016b, \n", diag_stat);
+
+    if (sensor_failure) {
+      num_failures++;
+      printf("%d failures, trying again\n", num_failures);
+    } else {
+      break;
+    }
+  }
+
+  write_register(FILT_CTRL, 0 /* no filtering */);
+  write_register(
+      MSC_CTRL,
+      (1u << 9) /* enable 32-bit mode for burst reads */ |
+          (0u << 8) /* send gyro and accelerometer data in burst mode */ |
+          (1u << 7) /* enable gyro linear g compensation */ |
+          (1u << 6) /* enable point of percussion alignment */ |
+          (0u << 2) /* internal clock mode */ |
+          (0u << 1) /* sync polarity, doesn't matter */ |
+          (1u << 0) /* data ready is active high */);
+  // Rate of the output will be 2000 / (DEC_RATE + 1) Hz.
+  write_register(DEC_RATE, 0 /* no decimation */);
+
+  sleep_us(200);
+
+  gpio_set_irq_enabled_with_callback(DR_IMU, GPIO_IRQ_EDGE_RISE, true,
+                                     &gpio_irq_handler);
+
+  while (!yaw_zeroed) {
+    dma_channel_wait_for_finish_blocking(imu_dma_rx);
+    sleep_us(100);
+    zero_yaw(yaw_rate);
+  }
+
+  printf("Press q to enter bootloader\n");
+
+  while (1) {
+    dma_channel_wait_for_finish_blocking(imu_dma_rx);
+    // we want the interrupts to happen before or during the sleep so that we
+    // can get a good picture of what's going on without things moving around
+    sleep_us(100);
+
+    // debug
+    // one printf is faster than many printfs
+    printf(
+        "Z pos is %f encoder: %d %d\n"
+        "Num failed checksums: %d, Total messages recieved: %d,\n"
+        "Num messages to pi: %d, Timing overrun count: %d,\n"
+        "Send time: %d us\n",
+        yaw, encoder1_count, encoder2_count, checksum_mismatch_count,
+        message_recieved_count, message_sent_count, timing_overrun_count,
+        send_time);
+
+    // allow the user to enter the bootloader without removing power or having
+    // to install a reset button
+    char user_input = getchar_timeout_us(0);
+    if (user_input == 'q') {
+      printf("Going down! entering bootloader\n");
+      reset_usb_boot(0, 0);
+    }
+
+    sleep_ms(50);
+  }
+
+  printf("All good\n");
+  dma_channel_unclaim(imu_dma_rx);
+  dma_channel_unclaim(imu_dma_tx);
+  dma_channel_unclaim(pi_dma_rx);
+  dma_channel_unclaim(pi_dma_tx);
+  return 0;
+}
diff --git a/frc971/imu/CMakeLists.txt b/frc971/imu/CMakeLists.txt
new file mode 100644
index 0000000..e97ba8d
--- /dev/null
+++ b/frc971/imu/CMakeLists.txt
@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 3.12)
+
+# Pull in SDK (must be before project)
+include(pico_sdk_import.cmake)
+
+project(ADIS16505_communication C CXX ASM)
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_CXX_STANDARD 17)
+
+if (PICO_SDK_VERSION_STRING VERSION_LESS "1.3.0")
+    message(FATAL_ERROR "Raspberry Pi Pico SDK version 1.3.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}")
+endif()
+
+# Initialize the SDK
+pico_sdk_init()
+
+add_compile_options(-Wall
+        -Wno-format          # int != int32_t as far as the compiler is concerned because gcc has int32_t as long int
+        -Wno-unused-function # we have some for the docs that aren't called
+        -Wno-maybe-uninitialized
+        )
+
+add_executable(pico_imu_board)
+
+pico_generate_pio_header(pico_imu_board ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
+
+target_sources(pico_imu_board PRIVATE ADIS16505.cc)
+
+# pull in common dependencies
+target_link_libraries(pico_imu_board
+  pico_stdlib
+  hardware_spi
+  hardware_dma
+  hardware_irq
+  hardware_pwm
+  hardware_pio
+  hardware_timer
+  pico_bootrom
+  pico_double
+  )
+
+# enable usb output, disable uart output
+pico_enable_stdio_usb(pico_imu_board 1)
+pico_enable_stdio_uart(pico_imu_board 1)
+
+# create map/bin/hex file etc.
+pico_add_extra_outputs(pico_imu_board)
diff --git a/frc971/imu/pico_sdk_import.cmake b/frc971/imu/pico_sdk_import.cmake
new file mode 100644
index 0000000..28efe9e
--- /dev/null
+++ b/frc971/imu/pico_sdk_import.cmake
@@ -0,0 +1,62 @@
+# This is a copy of <PICO_SDK_PATH>/external/pico_sdk_import.cmake
+
+# This can be dropped into an external project to help locate this SDK
+# It should be include()ed prior to project()
+
+if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
+    set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
+    message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
+    set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
+    message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
+    set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
+    message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
+endif ()
+
+set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
+set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
+set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
+
+if (NOT PICO_SDK_PATH)
+    if (PICO_SDK_FETCH_FROM_GIT)
+        include(FetchContent)
+        set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
+        if (PICO_SDK_FETCH_FROM_GIT_PATH)
+            get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
+        endif ()
+        FetchContent_Declare(
+                pico_sdk
+                GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                GIT_TAG master
+        )
+        if (NOT pico_sdk)
+            message("Downloading Raspberry Pi Pico SDK")
+            FetchContent_Populate(pico_sdk)
+            set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
+        endif ()
+        set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
+    else ()
+        message(FATAL_ERROR
+                "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
+                )
+    endif ()
+endif ()
+
+get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
+if (NOT EXISTS ${PICO_SDK_PATH})
+    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
+endif ()
+
+set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
+if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
+    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
+endif ()
+
+set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
+
+include(${PICO_SDK_INIT_CMAKE_FILE})
diff --git a/frc971/imu/quadrature_encoder.pio b/frc971/imu/quadrature_encoder.pio
new file mode 100644
index 0000000..b2a0b82
--- /dev/null
+++ b/frc971/imu/quadrature_encoder.pio
@@ -0,0 +1,165 @@
+;
+; Copyright (c) 2021 pmarques-dev @ github
+;
+; SPDX-License-Identifier: BSD-3-Clause
+;
+
+.program quadrature_encoder
+
+; this code must be loaded into address 0, but at 29 instructions, it probably
+; wouldn't be able to share space with other programs anyway
+.origin 0
+
+
+; the code works by running a loop that continuously shifts the 2 phase pins into
+; ISR and looks at the lower 4 bits to do a computed jump to an instruction that
+; does the proper "do nothing" | "increment" | "decrement" action for that pin
+; state change (or no change)
+
+; ISR holds the last state of the 2 pins during most of the code. The Y register
+; keeps the current encoder count and is incremented / decremented according to
+; the steps sampled
+
+; writing any non zero value to the TX FIFO makes the state machine push the
+; current count to RX FIFO between 6 to 18 clocks afterwards. The worst case
+; sampling loop takes 14 cycles, so this program is able to read step rates up
+; to sysclk / 14  (e.g., sysclk 125MHz, max step rate = 8.9 Msteps/sec)
+
+
+; 00 state
+	JMP update	; read 00
+	JMP decrement	; read 01
+	JMP increment	; read 10
+	JMP update	; read 11
+
+; 01 state
+	JMP increment	; read 00
+	JMP update	; read 01
+	JMP update	; read 10
+	JMP decrement	; read 11
+
+; 10 state
+	JMP decrement	; read 00
+	JMP update	; read 01
+	JMP update	; read 10
+	JMP increment	; read 11
+
+; to reduce code size, the last 2 states are implemented in place and become the
+; target for the other jumps
+
+; 11 state
+	JMP update	; read 00
+	JMP increment	; read 01
+decrement:
+	; note: the target of this instruction must be the next address, so that
+	; the effect of the instruction does not depend on the value of Y. The
+	; same is true for the "JMP X--" below. Basically "JMP Y--, <next addr>"
+	; is just a pure "decrement Y" instruction, with no other side effects
+	JMP Y--, update	; read 10
+
+	; this is where the main loop starts
+.wrap_target
+update:
+	; we start by checking the TX FIFO to see if the main code is asking for
+	; the current count after the PULL noblock, OSR will have either 0 if
+	; there was nothing or the value that was there
+	SET X, 0
+	PULL noblock
+
+	; since there are not many free registers, and PULL is done into OSR, we
+	; have to do some juggling to avoid losing the state information and
+	; still place the values where we need them
+	MOV X, OSR
+	MOV OSR, ISR
+
+	; the main code did not ask for the count, so just go to "sample_pins"
+	JMP !X, sample_pins
+
+	; if it did ask for the count, then we push it
+	MOV ISR, Y	; we trash ISR, but we already have a copy in OSR
+	PUSH
+
+sample_pins:
+	; we shift into ISR the last state of the 2 input pins (now in OSR) and
+	; the new state of the 2 pins, thus producing the 4 bit target for the
+	; computed jump into the correct action for this state
+	MOV ISR, NULL
+	IN OSR, 2
+	IN PINS, 2
+	MOV PC, ISR
+
+	; the PIO does not have a increment instruction, so to do that we do a
+	; negate, decrement, negate sequence
+increment:
+	MOV X, !Y
+	JMP X--, increment_cont
+increment_cont:
+	MOV Y, !X
+.wrap	; the .wrap here avoids one jump instruction and saves a cycle too
+
+
+
+% c-sdk {
+
+#include "hardware/clocks.h"
+#include "hardware/gpio.h"
+
+// max_step_rate is used to lower the clock of the state machine to save power
+// if the application doesn't require a very high sampling rate. Passing zero
+// will set the clock to the maximum, which gives a max step rate of around
+// 8.9 Msteps/sec at 125MHz
+
+static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate)
+{
+	pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false);
+	pio_gpio_init(pio, pin);
+	gpio_pull_up(pin);
+
+	pio_sm_config c = quadrature_encoder_program_get_default_config(offset);
+	sm_config_set_in_pins(&c, pin); // for WAIT, IN
+	sm_config_set_jmp_pin(&c, pin); // for JMP
+	// shift to left, autopull disabled
+	sm_config_set_in_shift(&c, false, false, 32);
+	// don't join FIFO's
+	sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);
+
+	// passing "0" as the sample frequency,
+	if (max_step_rate == 0) {
+		sm_config_set_clkdiv(&c, 1.0);
+	} else {
+		// one state machine loop takes at most 14 cycles
+		float div = (float)clock_get_hz(clk_sys) / (14 * max_step_rate);
+		sm_config_set_clkdiv(&c, div);
+	}
+
+	pio_sm_init(pio, sm, offset, &c);
+	pio_sm_set_enabled(pio, sm, true);
+}
+
+
+// When requesting the current count we may have to wait a few cycles (average
+// ~11 sysclk cycles) for the state machine to reply. If we are reading multiple
+// encoders, we may request them all in one go and then fetch them all, thus
+// avoiding doing the wait multiple times. If we are reading just one encoder,
+// we can use the "get_count" function to request and wait
+
+static inline void quadrature_encoder_request_count(PIO pio, uint sm)
+{
+	pio->txf[sm] = 1;
+}
+
+static inline int32_t quadrature_encoder_fetch_count(PIO pio, uint sm)
+{
+	while (pio_sm_is_rx_fifo_empty(pio, sm))
+		tight_loop_contents();
+	return pio->rxf[sm];
+}
+
+static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm)
+{
+	quadrature_encoder_request_count(pio, sm);
+	return quadrature_encoder_fetch_count(pio, sm);
+}
+
+%}
+
diff --git a/y2020/vision/rootfs/99-usb-mount.rules b/frc971/raspi/rootfs/99-usb-mount.rules
similarity index 100%
rename from y2020/vision/rootfs/99-usb-mount.rules
rename to frc971/raspi/rootfs/99-usb-mount.rules
diff --git a/frc971/raspi/rootfs/README.md b/frc971/raspi/rootfs/README.md
new file mode 100644
index 0000000..a9c7251
--- /dev/null
+++ b/frc971/raspi/rootfs/README.md
@@ -0,0 +1,50 @@
+This modifies a stock debian root filesystem to be able to operate as a vision
+pi.  It is not trying to be reproducible, but should be good enough for FRC
+purposes.
+
+The default hostname and IP is pi-971-1, 10.9.71.101.
+  Username pi, password raspberry.
+
+Download 2021-10-30-raspios-bullseye-armhf-lite.img (or any newer
+bullseye version, as a .zip file) from
+`https://www.raspberrypi.org/downloads/raspberry-pi-os/`, extract
+(unzip) the .img file, and edit `modify_rootfs.sh` to point to it.
+
+Run modify_rootfs.sh to build the filesystem (you might need to hit
+return in a spot or two and will need sudo privileges to mount the
+partition):
+  * `modify_root.sh`
+
+VERY IMPORTANT NOTE: Before doing the next step, use `lsblk` to find
+the device and make absolutely sure this isn't your hard drive or
+something else.  It will target /dev/sda by default, which in some
+computers is your default hard drive.
+
+After confirming the target device, edit the `make_sd.sh` script to point to the correct IMAGE filename, and run the `make_sd.sh` command,
+which takes the name of the pi as an argument:
+  * `make_sd.sh pi-971-1`
+
+OR, if you want to manually run this, you can deploy the image by
+copying the contents of the image to the SD card.  You can do this
+manually, via
+  `dd if=2020-02-13-raspbian-bullseye-lite-frc-mods.img of=/dev/sdX bs=1M`
+
+From there, transfer the SD card to the pi, log in, `sudo` to `root`,
+and run `/root/bin/change_hostname.sh` to change the hostname to the
+actual target.
+
+
+A couple additional notes on setting this up:
+   * You'll likely need to install (`sudo apt install`) the emulation packages `proot` and `qemu-user-static` (or possibly `qemu-arm-static`)
+   * If the modify_root.sh script fails, you may need to manually unmount the image (`sudo umount ${PARTITION}` and `rmdir ${PARTITION}`) before running it again
+   * Don't be clever and try to link to the image file in a different folder.  These scripts need access directly to the file and will fail otherwise
+
+
+Things to do once the SD card is complete, and you've booted a PI with it:
+
+  * Download the code:
+    Once this is completed, you can boot the pi with the newly flashed SD
+    card, and download the code to the pi using:
+      `bazel run -c opt --cpu=armv7 //y2022:pi_download_stripped -- PI_IP_ADDR
+
+    where PI_IP_ADDR is the IP address of the target pi, e.g., 10.9.71.101
diff --git a/y2020/vision/rootfs/change_hostname.sh b/frc971/raspi/rootfs/change_hostname.sh
similarity index 100%
rename from y2020/vision/rootfs/change_hostname.sh
rename to frc971/raspi/rootfs/change_hostname.sh
diff --git a/y2020/vision/rootfs/dhcpcd.conf b/frc971/raspi/rootfs/dhcpcd.conf
similarity index 100%
rename from y2020/vision/rootfs/dhcpcd.conf
rename to frc971/raspi/rootfs/dhcpcd.conf
diff --git a/y2020/vision/rootfs/frc971.service b/frc971/raspi/rootfs/frc971.service
similarity index 100%
rename from y2020/vision/rootfs/frc971.service
rename to frc971/raspi/rootfs/frc971.service
diff --git a/y2020/vision/rootfs/logind.conf b/frc971/raspi/rootfs/logind.conf
similarity index 100%
rename from y2020/vision/rootfs/logind.conf
rename to frc971/raspi/rootfs/logind.conf
diff --git a/frc971/raspi/rootfs/make_sd.sh b/frc971/raspi/rootfs/make_sd.sh
new file mode 100755
index 0000000..81f7727
--- /dev/null
+++ b/frc971/raspi/rootfs/make_sd.sh
@@ -0,0 +1,46 @@
+#!/bin/bash
+
+set -e
+
+# Disk image to use for creating SD card
+# NOTE: You MUST run modify_rootfs.sh on this image BEFORE running make_sd.sh
+ORIG_IMAGE="2021-10-30-raspios-bullseye-armhf-lite.img"
+IMAGE=`echo ${ORIG_IMAGE} | sed s/.img/-frc-mods.img/`
+DEVICE="/dev/sda"
+
+if mount | grep "${DEVICE}" >/dev/null ;
+then
+  echo "Overwriting a mounted partition, is ${DEVICE} the sd card?"
+  exit 1
+fi
+
+sudo dd if=${IMAGE} of=${DEVICE} bs=1M status=progress
+
+PARTITION="${IMAGE}.partition"
+
+mkdir -p "${PARTITION}"
+sudo mount "${DEVICE}2" "${PARTITION}"
+
+function target() {
+  HOME=/root/ USER=root sudo proot -0 -q qemu-arm-static -w / -r "${PARTITION}" "$@"
+}
+
+if [ "${1}" == "" ]; then
+  echo "No hostname specified, so skipping setting it."
+  echo "You do this manually on the pi by running /root/bin/change_hostname.sh PI_NAME"
+else
+  target /root/bin/change_hostname.sh "${1}"
+fi
+
+echo "Starting a shell for any manual configuration"
+target /bin/bash --rcfile /root/.bashrc
+
+# Put a timestamp on when this card got created and by whom
+TIMESTAMP_FILE="${PARTITION}/home/pi/.DiskFlashedDate.txt"
+date > "${TIMESTAMP_FILE}"
+git rev-parse HEAD >> "${TIMESTAMP_FILE}"
+whoami >> "${TIMESTAMP_FILE}"
+
+# Found I had to do a lazy force unmount ("-l" flag) to make it work reliably
+sudo umount -l "${PARTITION}"
+rmdir "${PARTITION}"
diff --git a/y2020/vision/rootfs/modify_rootfs.sh b/frc971/raspi/rootfs/modify_rootfs.sh
similarity index 77%
rename from y2020/vision/rootfs/modify_rootfs.sh
rename to frc971/raspi/rootfs/modify_rootfs.sh
index 1367314..a340c64 100755
--- a/y2020/vision/rootfs/modify_rootfs.sh
+++ b/frc971/raspi/rootfs/modify_rootfs.sh
@@ -2,8 +2,8 @@
 
 set -xe
 
-# Full path to Raspberry Pi Buster disk image
-IMAGE="2020-08-20-raspios-buster-armhf-lite.img"
+# Full path to Raspberry Pi Bullseye disk image
+IMAGE="2021-10-30-raspios-bullseye-armhf-lite.img"
 BOOT_PARTITION="${IMAGE}.boot_partition"
 PARTITION="${IMAGE}.partition"
 
@@ -34,8 +34,11 @@
 if ! grep "gpu_mem=128" "${BOOT_PARTITION}/config.txt"; then
   echo "gpu_mem=128" | sudo tee -a "${BOOT_PARTITION}/config.txt"
 fi
+# For now, disable the new libcamera driver in favor of legacy ones
+sudo sed -i s/^camera_auto_detect=1/#camera_auto_detect=1/ "${BOOT_PARTITION}/config.txt"
 
-sudo umount "${BOOT_PARTITION}"
+# Seeing a race condition with umount, so doing lazy umount
+sudo umount -l "${BOOT_PARTITION}"
 rmdir "${BOOT_PARTITION}"
 
 if mount | grep "${PARTITION}" >/dev/null ;
@@ -86,10 +89,21 @@
 target /bin/mkdir -p /home/pi/.ssh/
 cat ~/.ssh/id_rsa.pub | target tee /home/pi/.ssh/authorized_keys
 
+# Downloads and installs our target libraries
 target /bin/bash /tmp/target_configure.sh
 
+# Add a file to show when this image was last modified and by whom
+TIMESTAMP_FILE="${PARTITION}/home/pi/.ImageModifiedDate.txt"
+date > "${TIMESTAMP_FILE}"
+git rev-parse HEAD >> "${TIMESTAMP_FILE}"
+whoami >> "${TIMESTAMP_FILE}"
+
 # Run a prompt as root inside the target to poke around and check things.
 target /bin/bash --rcfile /root/.bashrc
 
-sudo umount "${PARTITION}"
+sudo umount -l "${PARTITION}"
 rmdir "${PARTITION}"
+
+# Move the image to a different name, to indicated we've modified it
+MOD_IMAGE_NAME=`echo ${IMAGE} | sed s/.img/-frc-mods.img/`
+mv ${IMAGE} ${MOD_IMAGE_NAME}
diff --git a/y2020/vision/rootfs/rt.conf b/frc971/raspi/rootfs/rt.conf
similarity index 100%
rename from y2020/vision/rootfs/rt.conf
rename to frc971/raspi/rootfs/rt.conf
diff --git a/y2020/vision/rootfs/sctp.conf b/frc971/raspi/rootfs/sctp.conf
similarity index 100%
rename from y2020/vision/rootfs/sctp.conf
rename to frc971/raspi/rootfs/sctp.conf
diff --git a/y2020/vision/rootfs/target_configure.sh b/frc971/raspi/rootfs/target_configure.sh
similarity index 78%
rename from y2020/vision/rootfs/target_configure.sh
rename to frc971/raspi/rootfs/target_configure.sh
index 9f2e289..3fd4d40 100755
--- a/y2020/vision/rootfs/target_configure.sh
+++ b/frc971/raspi/rootfs/target_configure.sh
@@ -19,28 +19,29 @@
   git \
   python3-pip \
   cpufrequtils \
-  libopencv-calib3d3.2 \
-  libopencv-contrib3.2 \
-  libopencv-core3.2 \
-  libopencv-features2d3.2 \
-  libopencv-flann3.2 \
-  libopencv-highgui3.2 \
-  libopencv-imgcodecs3.2 \
-  libopencv-imgproc3.2 \
-  libopencv-ml3.2 \
-  libopencv-objdetect3.2 \
-  libopencv-photo3.2 \
-  libopencv-shape3.2 \
-  libopencv-stitching3.2 \
-  libopencv-superres3.2 \
-  libopencv-video3.2 \
-  libopencv-videoio3.2 \
-  libopencv-videostab3.2 \
-  libopencv-viz3.2 \
+  libopencv-calib3d4.5 \
+  libopencv-contrib4.5 \
+  libopencv-core4.5 \
+  libopencv-features2d4.5 \
+  libopencv-flann4.5 \
+  libopencv-highgui4.5 \
+  libopencv-imgcodecs4.5 \
+  libopencv-imgproc4.5 \
+  libopencv-ml4.5 \
+  libopencv-objdetect4.5 \
+  libopencv-photo4.5 \
+  libopencv-shape4.5 \
+  libopencv-stitching4.5 \
+  libopencv-superres4.5 \
+  libopencv-video4.5 \
+  libopencv-videoio4.5 \
+  libopencv-videostab4.5 \
+  libopencv-viz4.5 \
   python3-opencv \
   libnice10 \
   pmount \
-  libnice-dev
+  libnice-dev \
+  feh
 
 echo 'GOVERNOR="performance"' > /etc/default/cpufrequtils
 
diff --git a/y2020/vision/rootfs/usb-mount@.service b/frc971/raspi/rootfs/usb-mount@.service
similarity index 100%
rename from y2020/vision/rootfs/usb-mount@.service
rename to frc971/raspi/rootfs/usb-mount@.service
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
new file mode 100644
index 0000000..1e3ed58
--- /dev/null
+++ b/frc971/vision/BUILD
@@ -0,0 +1,35 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
+
+flatbuffer_cc_library(
+    name = "vision_fbs",
+    srcs = ["vision.fbs"],
+    gen_reflections = 1,
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+)
+
+flatbuffer_ts_library(
+    name = "vision_ts_fbs",
+    srcs = ["vision.fbs"],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+)
+
+cc_library(
+    name = "v4l2_reader",
+    srcs = [
+        "v4l2_reader.cc",
+    ],
+    hdrs = [
+        "v4l2_reader.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":vision_fbs",
+        "//aos/events:event_loop",
+        "//aos/scoped:scoped_fd",
+        "@com_github_google_glog//:glog",
+        "@com_google_absl//absl/base",
+    ],
+)
diff --git a/y2020/vision/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
similarity index 97%
rename from y2020/vision/v4l2_reader.cc
rename to frc971/vision/v4l2_reader.cc
index 3f24f1e..793d8cd 100644
--- a/y2020/vision/v4l2_reader.cc
+++ b/frc971/vision/v4l2_reader.cc
@@ -1,4 +1,4 @@
-#include "y2020/vision/v4l2_reader.h"
+#include "frc971/vision/v4l2_reader.h"
 
 #include <fcntl.h>
 #include <linux/videodev2.h>
@@ -60,8 +60,8 @@
 }
 
 bool V4L2Reader::ReadLatestImage() {
-  // First, enqueue any old buffer we already have. This is the one which may
-  // have been sent.
+  // First, enqueue any old buffer we already have. This is the one which
+  // may have been sent.
   if (saved_buffer_) {
     EnqueueBuffer(saved_buffer_.index);
     saved_buffer_.Clear();
@@ -189,8 +189,8 @@
   if (result == 0) {
     return;
   }
-  // Some devices (like Alex's webcam) return this if streaming isn't currently
-  // on, unlike what the documentations says should happen.
+  // Some devices (like Alex's webcam) return this if streaming isn't
+  // currently on, unlike what the documentations says should happen.
   if (errno == EBUSY) {
     return;
   }
diff --git a/y2020/vision/v4l2_reader.h b/frc971/vision/v4l2_reader.h
similarity index 95%
rename from y2020/vision/v4l2_reader.h
rename to frc971/vision/v4l2_reader.h
index b9b3ce4..6f90cec 100644
--- a/y2020/vision/v4l2_reader.h
+++ b/frc971/vision/v4l2_reader.h
@@ -1,5 +1,5 @@
-#ifndef Y2020_VISION_V4L2_READER_H_
-#define Y2020_VISION_V4L2_READER_H_
+#ifndef FRC971_VISION_V4L2_READER_H_
+#define FRC971_VISION_V4L2_READER_H_
 
 #include <array>
 #include <string>
@@ -9,7 +9,7 @@
 
 #include "aos/events/event_loop.h"
 #include "aos/scoped/scoped_fd.h"
-#include "y2020/vision/vision_generated.h"
+#include "frc971/vision/vision_generated.h"
 
 namespace frc971 {
 namespace vision {
@@ -119,4 +119,4 @@
 }  // namespace vision
 }  // namespace frc971
 
-#endif  // Y2020_VISION_V4L2_READER_H_
+#endif  // FRC971_VISION_V4L2_READER_H_
diff --git a/y2020/vision/vision.fbs b/frc971/vision/vision.fbs
similarity index 100%
rename from y2020/vision/vision.fbs
rename to frc971/vision/vision.fbs
diff --git a/y2020/BUILD b/y2020/BUILD
index 0a32370..82a7848 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -155,9 +155,9 @@
         "//aos/network:message_bridge_client_fbs",
         "//aos/network:message_bridge_server_fbs",
         "//aos/network:timestamp_fbs",
+        "//frc971/vision:vision_fbs",
         "//y2020/vision/sift:sift_fbs",
         "//y2020/vision/sift:sift_training_fbs",
-        "//y2020/vision:vision_fbs",
     ],
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
@@ -179,12 +179,12 @@
         flatbuffers = [
             "//aos/network:message_bridge_client_fbs",
             "//aos/network:message_bridge_server_fbs",
+            "//aos/network:remote_message_fbs",
             "//aos/network:timestamp_fbs",
+            "//frc971/vision:vision_fbs",
+            "//y2020/vision:galactic_search_path_fbs",
             "//y2020/vision/sift:sift_fbs",
             "//y2020/vision/sift:sift_training_fbs",
-            "//y2020/vision:vision_fbs",
-            "//aos/network:remote_message_fbs",
-            "//y2020/vision:galactic_search_path_fbs",
         ],
         target_compatible_with = ["@platforms//os:linux"],
         visibility = ["//visibility:public"],
@@ -209,11 +209,11 @@
     flatbuffers = [
         "//aos/network:message_bridge_client_fbs",
         "//aos/network:message_bridge_server_fbs",
+        "//aos/network:remote_message_fbs",
         "//aos/network:timestamp_fbs",
+        "//frc971/vision:vision_fbs",
         "//y2020/vision/sift:sift_fbs",
         "//y2020/vision/sift:sift_training_fbs",
-        "//y2020/vision:vision_fbs",
-        "//aos/network:remote_message_fbs",
     ],
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index d94de5a..5156b59 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -1,12 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
-
-flatbuffer_cc_library(
-    name = "vision_fbs",
-    srcs = ["vision.fbs"],
-    gen_reflections = 1,
-    target_compatible_with = ["@platforms//os:linux"],
-    visibility = ["//y2020:__subpackages__"],
-)
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 
 flatbuffer_cc_library(
     name = "galactic_search_path_fbs",
@@ -16,25 +8,6 @@
     visibility = ["//y2020:__subpackages__"],
 )
 
-cc_library(
-    name = "v4l2_reader",
-    srcs = [
-        "v4l2_reader.cc",
-    ],
-    hdrs = [
-        "v4l2_reader.h",
-    ],
-    target_compatible_with = ["@platforms//os:linux"],
-    visibility = ["//y2020:__subpackages__"],
-    deps = [
-        ":vision_fbs",
-        "//aos/events:event_loop",
-        "//aos/scoped:scoped_fd",
-        "@com_github_google_glog//:glog",
-        "@com_google_absl//absl/base",
-    ],
-)
-
 cc_binary(
     name = "camera_reader",
     srcs = [
@@ -61,13 +34,13 @@
         "//y2020:config",
     ],
     target_compatible_with = ["@platforms//os:linux"],
-    visibility = ["//y2020:__subpackages__"],
+    visibility = ["//y2020:__subpackages__"] + ["//y2022:__subpackages__"],
     deps = [
-        ":v4l2_reader",
-        ":vision_fbs",
         "//aos:flatbuffer_merge",
         "//aos/events:event_loop",
         "//aos/network:team_number",
+        "//frc971/vision:v4l2_reader",
+        "//frc971/vision:vision_fbs",
         "//third_party:opencv",
         "//y2020/vision/sift:sift971",
         "//y2020/vision/sift:sift_fbs",
@@ -76,13 +49,6 @@
     ],
 )
 
-flatbuffer_ts_library(
-    name = "vision_ts_fbs",
-    srcs = ["vision.fbs"],
-    target_compatible_with = ["@platforms//os:linux"],
-    visibility = ["//y2020:__subpackages__"],
-)
-
 cc_binary(
     name = "viewer",
     srcs = [
@@ -94,9 +60,10 @@
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//y2020:__subpackages__"],
     deps = [
-        ":vision_fbs",
         "//aos:init",
         "//aos/events:shm_event_loop",
+        "//frc971/vision:v4l2_reader",
+        "//frc971/vision:vision_fbs",
         "//third_party:opencv",
         "//y2020/vision/sift:sift_fbs",
     ],
@@ -113,12 +80,12 @@
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//y2020:__subpackages__"],
     deps = [
-        ":vision_fbs",
         "//aos:flatbuffers",
         "//aos/events:event_loop",
         "//aos/network:message_bridge_server_fbs",
         "//aos/network:team_number",
         "//frc971/control_loops:quaternion_utils",
+        "//frc971/vision:vision_fbs",
         "//third_party:opencv",
         "//y2020/vision/sift:sift_fbs",
         "//y2020/vision/sift:sift_training_fbs",
@@ -142,10 +109,10 @@
     visibility = ["//y2020:__subpackages__"],
     deps = [
         ":charuco_lib",
-        ":vision_fbs",
         "//aos:init",
         "//aos/events:shm_event_loop",
         "//frc971/control_loops/drivetrain:improved_down_estimator",
+        "//frc971/vision:vision_fbs",
         "//frc971/wpilib:imu_batch_fbs",
         "//frc971/wpilib:imu_fbs",
         "//third_party:opencv",
@@ -168,10 +135,10 @@
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//y2020:__subpackages__"],
     deps = [
-        ":vision_fbs",
         "//aos:init",
         "//aos/events:simulated_event_loop",
         "//aos/events/logging:log_reader",
+        "//frc971/vision:vision_fbs",
         "//third_party:opencv",
     ],
 )
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index 1e28e82..9966a5b 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -9,12 +9,12 @@
 #include "aos/events/event_loop.h"
 #include "aos/flatbuffer_merge.h"
 #include "aos/network/team_number.h"
+#include "frc971/vision/v4l2_reader.h"
+#include "frc971/vision/vision_generated.h"
 #include "y2020/vision/sift/sift971.h"
 #include "y2020/vision/sift/sift_generated.h"
 #include "y2020/vision/sift/sift_training_generated.h"
 #include "y2020/vision/tools/python_code/sift_training_data.h"
-#include "y2020/vision/v4l2_reader.h"
-#include "y2020/vision/vision_generated.h"
 
 DEFINE_bool(skip_sift, false,
             "If true don't run any feature extraction.  Just forward images.");
diff --git a/y2020/vision/camera_reader.h b/y2020/vision/camera_reader.h
index c05ebea..37fb5a9 100644
--- a/y2020/vision/camera_reader.h
+++ b/y2020/vision/camera_reader.h
@@ -10,12 +10,12 @@
 #include "aos/events/event_loop.h"
 #include "aos/flatbuffer_merge.h"
 #include "aos/network/team_number.h"
+#include "frc971/vision/v4l2_reader.h"
+#include "frc971/vision/vision_generated.h"
 #include "y2020/vision/sift/sift971.h"
 #include "y2020/vision/sift/sift_generated.h"
 #include "y2020/vision/sift/sift_training_generated.h"
 #include "y2020/vision/tools/python_code/sift_training_data.h"
-#include "y2020/vision/v4l2_reader.h"
-#include "y2020/vision/vision_generated.h"
 
 namespace frc971 {
 namespace vision {
diff --git a/y2020/vision/charuco_lib.cc b/y2020/vision/charuco_lib.cc
index 21bdcc3..0df820b 100644
--- a/y2020/vision/charuco_lib.cc
+++ b/y2020/vision/charuco_lib.cc
@@ -11,11 +11,11 @@
 #include "aos/flatbuffers.h"
 #include "aos/network/team_number.h"
 #include "frc971/control_loops/quaternion_utils.h"
+#include "frc971/vision/vision_generated.h"
 #include "glog/logging.h"
 #include "y2020/vision/sift/sift_generated.h"
 #include "y2020/vision/sift/sift_training_generated.h"
 #include "y2020/vision/tools/python_code/sift_training_data.h"
-#include "y2020/vision/vision_generated.h"
 
 DEFINE_uint32(min_targets, 10,
               "The mininum number of targets required to match.");
@@ -133,8 +133,7 @@
 
     const monotonic_clock::time_point eof = eof_source_node - offset;
 
-    const monotonic_clock::duration age =
-        event_loop_->monotonic_now() - eof;
+    const monotonic_clock::duration age = event_loop_->monotonic_now() - eof;
     const double age_double =
         std::chrono::duration_cast<std::chrono::duration<double>>(age).count();
     if (age > std::chrono::milliseconds(100)) {
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index 1d4de28..c7ef752 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -12,13 +12,13 @@
 #include "frc971/analysis/in_process_plotter.h"
 #include "frc971/control_loops/drivetrain/improved_down_estimator.h"
 #include "frc971/control_loops/quaternion_utils.h"
+#include "frc971/vision/vision_generated.h"
 #include "frc971/wpilib/imu_batch_generated.h"
 #include "y2020/vision/calibration_accumulator.h"
 #include "y2020/vision/charuco_lib.h"
 #include "y2020/vision/sift/sift_generated.h"
 #include "y2020/vision/sift/sift_training_generated.h"
 #include "y2020/vision/tools/python_code/sift_training_data.h"
-#include "y2020/vision/vision_generated.h"
 
 DEFINE_string(config, "config.json", "Path to the config file to use.");
 DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
@@ -120,7 +120,7 @@
 
   const Eigen::Quaternion<Scalar> &orientation() const { return orientation_; }
 
-  std::vector<Eigen::Matrix<Scalar, 3, 1> > errors_;
+  std::vector<Eigen::Matrix<Scalar, 3, 1>> errors_;
 
   // Returns the angular errors for each camera sample.
   size_t num_errors() const { return errors_.size(); }
@@ -136,18 +136,18 @@
     result.template block<4, 1>(0, 0) = q.coeffs();
     result.template block<6, 1>(4, 0) = x_hat;
     result.template block<36, 1>(10, 0) =
-        Eigen::Map<Eigen::Matrix<Scalar, 36, 1> >(p.data(), p.size());
+        Eigen::Map<Eigen::Matrix<Scalar, 36, 1>>(p.data(), p.size());
 
     return result;
   }
 
   std::tuple<Eigen::Quaternion<Scalar>, Eigen::Matrix<Scalar, 6, 1>,
-             Eigen::Matrix<Scalar, 6, 6> >
+             Eigen::Matrix<Scalar, 6, 6>>
   UnPack(Eigen::Matrix<Scalar, 46, 1> input) {
     Eigen::Quaternion<Scalar> q(input.template block<4, 1>(0, 0));
     Eigen::Matrix<Scalar, 6, 1> x_hat(input.template block<6, 1>(4, 0));
     Eigen::Matrix<Scalar, 6, 6> p =
-        Eigen::Map<Eigen::Matrix<Scalar, 6, 6> >(input.data() + 10, 6, 6);
+        Eigen::Map<Eigen::Matrix<Scalar, 6, 6>>(input.data() + 10, 6, 6);
     return std::make_tuple(q, x_hat, p);
   }
 
@@ -361,8 +361,8 @@
   std::vector<double> imu_ratez;
 
   std::vector<double> times_;
-  std::vector<Eigen::Matrix<double, 6, 1> > x_hats_;
-  std::vector<Eigen::Quaternion<double> > orientations_;
+  std::vector<Eigen::Matrix<double, 6, 1>> x_hats_;
+  std::vector<Eigen::Quaternion<double>> orientations_;
 
   Eigen::Matrix<double, 3, 1> last_accel_ = Eigen::Matrix<double, 3, 1>::Zero();
 };
diff --git a/y2020/vision/rootfs/README.md b/y2020/vision/rootfs/README.md
deleted file mode 100644
index 0f66864..0000000
--- a/y2020/vision/rootfs/README.md
+++ /dev/null
@@ -1,32 +0,0 @@
-This modifies a stock debian root filesystem to be able to operate as a vision
-pi.  It is not trying to be reproducible, but should be good enough for FRC
-purposes.
-
-The default hostname and IP is pi-8971-1, 10.89.71.101.
-  Username pi, password raspberry.
-
-Download 2020-08-20-raspios-buster-armhf-lite.img (or any newer buster
-version, as a .zip file) from
-`https://www.raspberrypi.org/downloads/raspberry-pi-os/` (or
-`https://downloads.raspberrypi.org/raspios_lite_armhf/images/` for
-older images), extract (unzip) the .img file, and edit
-`modify_rootfs.sh` to point to it.
-
-Run modify_rootfs.sh to build the filesystem (you might need to hit
-return in a spot or two and will need sudo privileges to mount the
-partition).
-
-After confirming the target device, deploy by copying the contents of the image
-to the SD card.
-
-  `dd if=2020-02-13-raspbian-buster-lite.img of=/dev/sdX bs=1M`
-
-Use `lsblk` to find the device and make absolutely sure this isn't your hard
-drive or something else.
-
-From there, log in, `sudo` to `root`, and run `/root/bin/change_hostname.sh` to
-change the hostname to the actual target.
-
-A couple additional notes on setting this up:
-   * You'll likely need to install (`sudo apt install`) the emulation packages `proot` and `qemu-user-static` (or possibly `qemu-arm-static`)
-   * If the modify_root.sh script fails, you may need to manually unmount the image (`sudo umount ${IMAGE}`) before running it again
diff --git a/y2020/vision/rootfs/make_sd.sh b/y2020/vision/rootfs/make_sd.sh
deleted file mode 100755
index eba47bc..0000000
--- a/y2020/vision/rootfs/make_sd.sh
+++ /dev/null
@@ -1,30 +0,0 @@
-#!/bin/bash
-
-set -e
-
-IMAGE="2020-08-20-raspios-buster-armhf-lite.img"
-DEVICE="/dev/sda"
-
-if mount | grep "${DEVICE}" >/dev/null ;
-then
-  echo "Overwriting a mounted partition, is ${DEVICE} the sd card?"
-  exit 1
-fi
-
-sudo dd if=${IMAGE} of=${DEVICE} bs=1M status=progress
-
-PARTITION="${IMAGE}.partition"
-
-mkdir -p "${PARTITION}"
-sudo mount "${DEVICE}2" "${PARTITION}"
-
-function target() {
-  HOME=/root/ USER=root sudo proot -0 -q qemu-arm-static -w / -r "${PARTITION}" "$@"
-}
-
-target /root/bin/change_hostname.sh "${1}"
-
-echo "Starting a shell for any manual configuration"
-target /bin/bash --rcfile /root/.bashrc
-
-sudo umount "${PARTITION}"
diff --git a/y2020/vision/tools/python_code/BUILD b/y2020/vision/tools/python_code/BUILD
index b495658..d259ba7 100644
--- a/y2020/vision/tools/python_code/BUILD
+++ b/y2020/vision/tools/python_code/BUILD
@@ -192,8 +192,8 @@
     deps = [
         ":sift_training_data_test",
         "//aos/testing:googletest",
+        "//frc971/vision:vision_fbs",
         "//third_party:opencv",
-        "//y2020/vision:vision_fbs",
         "//y2020/vision/sift:sift_fbs",
         "//y2020/vision/sift:sift_training_fbs",
     ],
diff --git a/y2020/vision/tools/python_code/camera_definition.py b/y2020/vision/tools/python_code/camera_definition.py
index 465f7bd..3e3623d 100644
--- a/y2020/vision/tools/python_code/camera_definition.py
+++ b/y2020/vision/tools/python_code/camera_definition.py
@@ -147,8 +147,8 @@
                 dist_coeffs = np.asarray(calib_dict["dist_coeffs"]).reshape(
                     (1, 5))
 
-            glog.info("Found calib for " + node_name + ", team #" +
-                      str(team_number))
+            glog.debug("Found calib for " + node_name + ", team #" +
+                       str(team_number))
 
             camera_params = CameraParameters()
             camera_params.camera_ext, camera_params.turret_ext = compute_extrinsic_by_pi(
diff --git a/y2020/vision/tools/python_code/camera_param_test.cc b/y2020/vision/tools/python_code/camera_param_test.cc
index 483fe75..a2d3a75 100644
--- a/y2020/vision/tools/python_code/camera_param_test.cc
+++ b/y2020/vision/tools/python_code/camera_param_test.cc
@@ -12,9 +12,9 @@
 #include "y2020/vision/tools/python_code/sift_training_data.h"
 #endif
 
+#include "frc971/vision/vision_generated.h"
 #include "y2020/vision/sift/sift_generated.h"
 #include "y2020/vision/sift/sift_training_generated.h"
-#include "y2020/vision/vision_generated.h"
 
 namespace frc971 {
 namespace vision {
diff --git a/y2020/vision/tools/python_code/target_definition.py b/y2020/vision/tools/python_code/target_definition.py
index e518c8c..8d1c601 100644
--- a/y2020/vision/tools/python_code/target_definition.py
+++ b/y2020/vision/tools/python_code/target_definition.py
@@ -5,6 +5,7 @@
 import json
 import math
 import numpy as np
+import os
 
 import camera_definition
 import define_training_data as dtd
@@ -153,7 +154,10 @@
     # otherwise get them directly from the training targets.
     for target in (ideal_target_list
                    if AUTO_PROJECTION else training_target_list):
-        glog.debug("\nPreparing target for image %s" % target.image_filename)
+        # TODO<Jim>: Save this info to flatbuffer and publish on start
+        # And then, make this debug message again
+        glog.debug("\nPreparing target for image %s" %
+                   os.path.basename(target.image_filename))
         target.extract_features(feature_extractor)
         target.filter_keypoints_by_polygons()
         target.compute_reprojection_maps()
diff --git a/y2020/vision/tools/python_code/train_and_match.py b/y2020/vision/tools/python_code/train_and_match.py
index 6250da1..ca67095 100644
--- a/y2020/vision/tools/python_code/train_and_match.py
+++ b/y2020/vision/tools/python_code/train_and_match.py
@@ -52,13 +52,13 @@
 # Load feature extractor based on extractor name
 # Returns feature extractor object
 def load_feature_extractor():
-    if FEATURE_EXTRACTOR_NAME is 'SIFT':
+    if FEATURE_EXTRACTOR_NAME == 'SIFT':
         # Initiate SIFT detector
         feature_extractor = cv2.SIFT_create()
-    elif FEATURE_EXTRACTOR_NAME is 'SURF':
+    elif FEATURE_EXTRACTOR_NAME == 'SURF':
         # Initiate SURF detector
         feature_extractor = cv2.xfeatures2d.SURF_create()
-    elif FEATURE_EXTRACTOR_NAME is 'ORB':
+    elif FEATURE_EXTRACTOR_NAME == 'ORB':
         # Initiate ORB detector
         feature_extractor = cv2.ORB_create()
 
@@ -78,7 +78,7 @@
             kp, des = feature_extractor.detectAndCompute(image_list[i], None)
             descriptor_lists.append(des)
             keypoint_lists.append(kp)
-    elif FEATURE_EXTRACTOR_NAME is 'ORB':
+    elif FEATURE_EXTRACTOR_NAME == 'ORB':
         # TODO: Check whether ORB extractor can do detectAndCompute.
         # If so, we don't need to have this branch for ORB
         for i in range(len(image_list)):
@@ -104,7 +104,7 @@
         matcher = cv2.FlannBasedMatcher(index_params, search_params)
         matcher.add(descriptor_lists)
         matcher.train()
-    elif FEATURE_EXTRACTOR_NAME is 'ORB':
+    elif FEATURE_EXTRACTOR_NAME == 'ORB':
         # Use FLANN LSH for ORB
         FLANN_INDEX_LSH = 6
         index_params = dict(
@@ -144,7 +144,7 @@
 
             good_matches_list.append(good_matches)
 
-    elif FEATURE_EXTRACTOR_NAME is 'ORB':
+    elif FEATURE_EXTRACTOR_NAME == 'ORB':
         matches = matcher.knnMatch(train_keypoint_lists[0], desc_query, k=2)
         good_matches = []
         for m in matches:
diff --git a/y2020/vision/viewer.cc b/y2020/vision/viewer.cc
index 2aff3fb..37ff4a3 100644
--- a/y2020/vision/viewer.cc
+++ b/y2020/vision/viewer.cc
@@ -8,8 +8,9 @@
 #include "aos/events/shm_event_loop.h"
 #include "aos/init.h"
 #include "aos/time/time.h"
+#include "frc971/vision/v4l2_reader.h"
+#include "frc971/vision/vision_generated.h"
 #include "y2020/vision/sift/sift_generated.h"
-#include "y2020/vision/vision_generated.h"
 
 DEFINE_string(config, "config.json", "Path to the config file to use.");
 DEFINE_bool(show_features, true, "Show the SIFT features that matched.");
diff --git a/y2020/vision/viewer_replay.cc b/y2020/vision/viewer_replay.cc
index 93e531d..a818859 100644
--- a/y2020/vision/viewer_replay.cc
+++ b/y2020/vision/viewer_replay.cc
@@ -6,7 +6,7 @@
 #include "aos/events/logging/log_reader.h"
 #include "aos/events/simulated_event_loop.h"
 #include "aos/init.h"
-#include "y2020/vision/vision_generated.h"
+#include "frc971/vision/vision_generated.h"
 
 DEFINE_string(node, "pi1", "Node name to replay.");
 DEFINE_string(image_save_prefix, "/tmp/img",
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index 27c0392..8467994 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -15,7 +15,7 @@
         "//aos/network:connect_ts_fbs",
         "//aos/network:web_proxy_ts_fbs",
         "//aos/network/www:proxy",
-        "//y2020/vision:vision_ts_fbs",
+        "//frc971/vision:vision_ts_fbs",
         "//y2020/vision/sift:sift_ts_fbs",
         "@com_github_google_flatbuffers//ts:flatbuffers_ts",
     ],
diff --git a/y2020/www/image_handler.ts b/y2020/www/image_handler.ts
index b254f2c..661100b 100644
--- a/y2020/www/image_handler.ts
+++ b/y2020/www/image_handler.ts
@@ -4,7 +4,7 @@
 import {ByteBuffer} from 'org_frc971/external/com_github_google_flatbuffers/ts/byte-buffer';
 import {Long} from 'org_frc971/external/com_github_google_flatbuffers/ts/long';
 import * as sift from 'org_frc971/y2020/vision/sift/sift_generated'
-import * as vision from 'org_frc971/y2020/vision/vision_generated';
+import * as vision from 'org_frc971/frc971/vision/vision_generated';
 import * as web_proxy from 'org_frc971/aos/network/web_proxy_generated';
 
 import Channel = configuration.aos.Channel;
diff --git a/y2022/BUILD b/y2022/BUILD
index dba5796..2173877 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -1,19 +1,152 @@
 load("//frc971:downloader.bzl", "robot_downloader")
 load("//aos:config.bzl", "aos_config")
+load("//tools/build_rules:template.bzl", "jinja2_template")
 
 robot_downloader(
+    binaries = [
+        "//aos/network:web_proxy_main",
+    ],
     data = [
         ":config",
     ],
     start_binaries = [
+        "//aos/events/logging:logger_main",
+        "//aos/network:web_proxy_main",
         ":joystick_reader",
         ":wpilib_interface",
+        "//aos/network:message_bridge_client",
+        "//aos/network:message_bridge_server",
+        "//y2022/actors:binaries",
         "//y2022/control_loops/drivetrain:drivetrain",
         "//y2022/control_loops/superstructure:superstructure",
-        "//y2022/actors:binaries",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+)
+
+robot_downloader(
+    name = "pi_download",
+    binaries = [
+        "//y2022/vision:viewer",
+    ],
+    data = [
+        ":config",
+    ],
+    start_binaries = [
+        "//aos/events/logging:logger_main",
+        "//aos/network:message_bridge_client",
+        "//aos/network:message_bridge_server",
+        "//aos/network:web_proxy_main",
+        "//y2022/vision:camera_reader",
+    ],
+    target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
+    target_type = "pi",
+)
+
+aos_config(
+    name = "config",
+    src = "y2022.json",
+    flatbuffers = [
+        "//aos/network:message_bridge_client_fbs",
+        "//aos/network:message_bridge_server_fbs",
+        "//aos/network:timestamp_fbs",
+        "//frc971/input:robot_state_fbs",
+        "//frc971/vision:vision_fbs",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":config_logger",
+        ":config_pi1",
+        ":config_pi2",
+        ":config_pi3",
+        ":config_pi4",
+        ":config_pi5",
+        ":config_roborio",
     ],
 )
 
+[
+    aos_config(
+        name = "config_" + pi,
+        src = "y2022_" + pi + ".json",
+        flatbuffers = [
+            "//aos/network:message_bridge_client_fbs",
+            "//aos/network:message_bridge_server_fbs",
+            "//aos/network:timestamp_fbs",
+            "//aos/network:remote_message_fbs",
+            "//frc971/vision:vision_fbs",
+            "//y2022/vision:target_estimate_fbs",
+        ],
+        target_compatible_with = ["@platforms//os:linux"],
+        visibility = ["//visibility:public"],
+        deps = [
+            "//aos/events:config",
+            "//frc971/control_loops/drivetrain:config",
+            "//frc971/input:config",
+        ],
+    )
+    for pi in [
+        "pi1",
+        "pi2",
+        "pi3",
+        "pi4",
+        "pi5",
+    ]
+]
+
+aos_config(
+    name = "config_logger",
+    src = "y2022_logger.json",
+    flatbuffers = [
+        "//aos/network:message_bridge_client_fbs",
+        "//aos/network:message_bridge_server_fbs",
+        "//aos/network:timestamp_fbs",
+        "//aos/network:remote_message_fbs",
+        "//frc971/vision:vision_fbs",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//aos/events:config",
+        "//frc971/control_loops/drivetrain:config",
+        "//frc971/input:config",
+    ],
+)
+
+aos_config(
+    name = "config_roborio",
+    src = "y2022_roborio.json",
+    flatbuffers = [
+        "//aos/network:remote_message_fbs",
+        "//aos/network:message_bridge_client_fbs",
+        "//aos/network:message_bridge_server_fbs",
+        "//aos/network:timestamp_fbs",
+        "//y2019/control_loops/drivetrain:target_selector_fbs",
+        "//y2022/control_loops/superstructure:superstructure_goal_fbs",
+        "//y2022/control_loops/superstructure:superstructure_output_fbs",
+        "//y2022/control_loops/superstructure:superstructure_position_fbs",
+        "//y2022/control_loops/superstructure:superstructure_status_fbs",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//aos/events:config",
+        "//frc971/autonomous:config",
+        "//frc971/control_loops/drivetrain:config",
+        "//frc971/input:config",
+        "//frc971/wpilib:config",
+    ],
+)
+
+[
+    jinja2_template(
+        name = "y2022_pi" + str(num) + ".json",
+        src = "y2022_pi_template.json",
+        parameters = {"NUM": str(num)},
+        target_compatible_with = ["@platforms//os:linux"],
+    )
+    for num in range(1, 6)
+]
+
 cc_library(
     name = "constants",
     srcs = [
@@ -96,21 +229,3 @@
         "//y2022/control_loops/superstructure:superstructure_status_fbs",
     ],
 )
-
-aos_config(
-    name = "config",
-    src = "y2022.json",
-    flatbuffers = [
-        "//y2022/control_loops/superstructure:superstructure_goal_fbs",
-        "//y2022/control_loops/superstructure:superstructure_output_fbs",
-        "//y2022/control_loops/superstructure:superstructure_position_fbs",
-        "//y2022/control_loops/superstructure:superstructure_status_fbs",
-    ],
-    visibility = ["//visibility:public"],
-    deps = [
-        "//frc971/autonomous:config",
-        "//frc971/control_loops/drivetrain:config",
-        "//frc971/input:config",
-        "//frc971/wpilib:config",
-    ],
-)
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index e985779..b2c89d1 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -26,7 +26,7 @@
 
   if (output != nullptr && unsafe_goal != nullptr) {
     OutputT output_struct;
-
+    output_struct.climber_voltage = unsafe_goal->climber_speed();
     output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
   }
 
@@ -36,8 +36,8 @@
   status_builder.add_estopped(false);
 
   if (unsafe_goal != nullptr) {
+    status_builder.add_climber_speed(unsafe_goal->climber_speed());
   }
-
   (void)status->Send(status_builder.Finish());
 }
 
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 439c0fb..3e0679e 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -3,7 +3,9 @@
 namespace y2022.control_loops.superstructure;
 
 table Goal {
-
+    // Voltage of the climber falcon
+    // - is down + is up
+    climber_speed:double (id: 0);
 }
 
 root_type Goal;
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index e95fbe3..78b2c01 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -1,7 +1,9 @@
 namespace y2022.control_loops.superstructure;
 
 table Output {
-
+    // Voltage of the climber falcon
+    // - is down + is up
+    climber_voltage:double (id: 0);
 }
 
 root_type Output;
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index deff2f6..5f8cdc3 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -9,6 +9,10 @@
 
   // If true, we have aborted. This is the or of all subsystem estops.
   estopped:bool (id: 1);
+
+  // Goal voltage of the climber falcon
+  // - is down + is up
+  climber_speed:double (id: 2);
 }
 
 root_type Status;
\ No newline at end of file
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index e69de29..e893e54 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -0,0 +1,103 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+cc_binary(
+    name = "camera_reader",
+    srcs = [
+        "camera_reader_main.cc",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//y2022:__subpackages__"],
+    deps = [
+        ":camera_reader_lib",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+    ],
+)
+
+cc_library(
+    name = "camera_reader_lib",
+    srcs = [
+        "camera_reader.cc",
+    ],
+    hdrs = [
+        "camera_reader.h",
+    ],
+    data = [
+        "//y2022:config",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//y2022:__subpackages__"],
+    deps = [
+        ":blob_detector_lib",
+        ":target_estimator_lib",
+        "//aos:flatbuffer_merge",
+        "//aos/events:event_loop",
+        "//aos/network:team_number",
+        "//frc971/vision:v4l2_reader",
+        "//frc971/vision:vision_fbs",
+        "//third_party:opencv",
+        "//y2020/vision/sift:sift_fbs",
+        "//y2020/vision/sift:sift_training_fbs",
+        "//y2020/vision/tools/python_code:sift_training_data",
+    ],
+)
+
+cc_library(
+    name = "blob_detector_lib",
+    srcs = [
+        "blob_detector.cc",
+    ],
+    hdrs = [
+        "blob_detector.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//y2022:__subpackages__"],
+    deps = [
+        "//aos/network:team_number",
+        "//third_party:opencv",
+    ],
+)
+
+cc_library(
+    name = "target_estimator_lib",
+    srcs = [
+        "target_estimator.cc",
+    ],
+    hdrs = [
+        "target_estimator.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//y2022:__subpackages__"],
+    deps = [
+        ":target_estimate_fbs",
+        "//third_party:opencv",
+    ],
+)
+
+flatbuffer_cc_library(
+    name = "target_estimate_fbs",
+    srcs = ["target_estimate.fbs"],
+    gen_reflections = 1,
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//y2022:__subpackages__"],
+)
+
+cc_binary(
+    name = "viewer",
+    srcs = [
+        "viewer.cc",
+    ],
+    data = [
+        "//y2022:config",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//y2022:__subpackages__"],
+    deps = [
+        ":blob_detector_lib",
+        ":target_estimator_lib",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//frc971/vision:vision_fbs",
+        "//third_party:opencv",
+    ],
+)
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
new file mode 100644
index 0000000..b76ffc5
--- /dev/null
+++ b/y2022/vision/blob_detector.cc
@@ -0,0 +1,148 @@
+#include "y2022/vision/blob_detector.h"
+
+#include <cmath>
+#include <string>
+
+#include "aos/network/team_number.h"
+#include "opencv2/features2d.hpp"
+#include "opencv2/imgproc.hpp"
+
+DEFINE_uint64(green_delta, 50,
+              "Required difference between green pixels vs. red and blue");
+DEFINE_bool(use_outdoors, false,
+            "If true, change thresholds to handle outdoor illumination");
+
+namespace y2022 {
+namespace vision {
+
+cv::Mat BlobDetector::ThresholdImage(cv::Mat rgb_image) {
+  cv::Mat binarized_image(cv::Size(rgb_image.cols, rgb_image.rows), CV_8UC1);
+  for (int row = 0; row < rgb_image.rows; row++) {
+    for (int col = 0; col < rgb_image.cols; col++) {
+      cv::Vec3b pixel = rgb_image.at<cv::Vec3b>(row, col);
+      uint8_t blue = pixel.val[0];
+      uint8_t green = pixel.val[1];
+      uint8_t red = pixel.val[2];
+      // Simple filter that looks for green pixels sufficiently brigher than
+      // red and blue
+      if ((green > blue + FLAGS_green_delta) &&
+          (green > red + FLAGS_green_delta)) {
+        binarized_image.at<uint8_t>(row, col) = 255;
+      } else {
+        binarized_image.at<uint8_t>(row, col) = 0;
+      }
+    }
+  }
+
+  return binarized_image;
+}
+
+std::vector<std::vector<cv::Point>> BlobDetector::FindBlobs(
+    cv::Mat binarized_image) {
+  // find the contours (blob outlines)
+  std::vector<std::vector<cv::Point>> contours;
+  std::vector<cv::Vec4i> hierarchy;
+  cv::findContours(binarized_image, contours, hierarchy, cv::RETR_CCOMP,
+                   cv::CHAIN_APPROX_SIMPLE);
+
+  return contours;
+}
+
+std::vector<BlobDetector::BlobStats> BlobDetector::ComputeStats(
+    std::vector<std::vector<cv::Point>> blobs) {
+  std::vector<BlobDetector::BlobStats> blob_stats;
+  for (auto blob : blobs) {
+    // Make the blob convex before finding bounding box
+    std::vector<cv::Point> convex_blob;
+    cv::convexHull(blob, convex_blob);
+    auto blob_size = cv::boundingRect(convex_blob).size();
+    cv::Moments moments = cv::moments(convex_blob);
+
+    const auto centroid =
+        cv::Point(moments.m10 / moments.m00, moments.m01 / moments.m00);
+    const double aspect_ratio =
+        static_cast<double>(blob_size.width) / blob_size.height;
+    const double area = moments.m00;
+    const size_t points = blob.size();
+
+    blob_stats.emplace_back(BlobStats{centroid, aspect_ratio, area, points});
+  }
+  return blob_stats;
+}
+
+// Filter blobs to get rid of noise, too large items, etc.
+std::vector<std::vector<cv::Point>> BlobDetector::FilterBlobs(
+    std::vector<std::vector<cv::Point>> blobs,
+    std::vector<BlobDetector::BlobStats> blob_stats) {
+  std::vector<std::vector<cv::Point>> filtered_blobs;
+
+  auto blob_it = blobs.begin();
+  auto stats_it = blob_stats.begin();
+  while (blob_it < blobs.end() && stats_it < blob_stats.end()) {
+    // To estimate the maximum y, we can figure out the y value of the blobs
+    // when the camera is the farthest from the target, at the field corner.
+    // We can solve for the pitch of the blob:
+    // blob_pitch = atan((height_tape - height_camera) / depth) + camera_pitch
+    // The triangle with the height of the tape above the camera and the camera
+    // depth is similar to the one with the focal length in y pixels and the y
+    // coordinate offset from the center of the image.
+    // Therefore y_offset = focal_length_y * tan(blob_pitch), and
+    // y = -(y_offset - offset_y)
+    constexpr int kMaxY = 400;
+    constexpr double kTapeAspectRatio = 5.0 / 2.0;
+    constexpr double kAspectRatioThreshold = 1.5;
+    constexpr double kMinArea = 10;
+    constexpr size_t kMinPoints = 6;
+
+    // Remove all blobs that are at the bottom of the image, have a different
+    // aspect ratio than the tape, or have too little area or points
+    // TODO(milind): modify to take into account that blobs will be on the side.
+    if ((stats_it->centroid.y <= kMaxY) &&
+        (std::abs(kTapeAspectRatio - stats_it->aspect_ratio) <
+         kAspectRatioThreshold) &&
+        (stats_it->area >= kMinArea) && (stats_it->points >= kMinPoints)) {
+      filtered_blobs.push_back(*blob_it);
+    }
+    blob_it++;
+    stats_it++;
+  }
+
+  return filtered_blobs;
+}
+
+void BlobDetector::DrawBlobs(
+    cv::Mat view_image,
+    const std::vector<std::vector<cv::Point>> &unfiltered_blobs,
+    const std::vector<std::vector<cv::Point>> &filtered_blobs,
+    const std::vector<BlobStats> &blob_stats) {
+  CHECK_GT(view_image.cols, 0);
+  if (unfiltered_blobs.size() > 0) {
+    // Draw blobs unfilled, with red color border
+    cv::drawContours(view_image, unfiltered_blobs, -1, cv::Scalar(0, 0, 255),
+                     0);
+  }
+
+  cv::drawContours(view_image, filtered_blobs, -1, cv::Scalar(0, 255, 0),
+                   cv::FILLED);
+
+  // Draw blob centroids
+  for (auto stats : blob_stats) {
+    cv::circle(view_image, stats.centroid, 2, cv::Scalar(255, 0, 0),
+               cv::FILLED);
+  }
+}
+
+void BlobDetector::ExtractBlobs(
+    cv::Mat rgb_image, cv::Mat binarized_image, cv::Mat blob_image,
+    std::vector<std::vector<cv::Point>> &filtered_blobs,
+    std::vector<std::vector<cv::Point>> &unfiltered_blobs,
+    std::vector<BlobStats> &blob_stats) {
+  binarized_image = ThresholdImage(rgb_image);
+  unfiltered_blobs = FindBlobs(binarized_image);
+  blob_stats = ComputeStats(unfiltered_blobs);
+  filtered_blobs = FilterBlobs(unfiltered_blobs, blob_stats);
+  DrawBlobs(blob_image, unfiltered_blobs, filtered_blobs, blob_stats);
+}
+
+}  // namespace vision
+}  // namespace y2022
diff --git a/y2022/vision/blob_detector.h b/y2022/vision/blob_detector.h
new file mode 100644
index 0000000..84e504d
--- /dev/null
+++ b/y2022/vision/blob_detector.h
@@ -0,0 +1,54 @@
+#ifndef Y2022_BLOB_DETECTOR_H_
+#define Y2022_BLOB_DETECTOR_H_
+
+#include <opencv2/features2d.hpp>
+#include <opencv2/imgproc.hpp>
+
+namespace y2022 {
+namespace vision {
+
+class BlobDetector {
+ public:
+  struct BlobStats {
+    cv::Point centroid;
+    double aspect_ratio;
+    double area;
+    size_t points;
+  };
+
+  BlobDetector() {}
+  // Given an image, threshold it to find "green" pixels
+  // Input: Color image
+  // Output: Grayscale (binarized) image with green pixels set to 255
+  static cv::Mat ThresholdImage(cv::Mat rgb_image);
+
+  // Given binary image, extract blobs
+  static std::vector<std::vector<cv::Point>> FindBlobs(cv::Mat threshold_image);
+
+  // Extract stats for each blob
+  static std::vector<BlobStats> ComputeStats(
+      std::vector<std::vector<cv::Point>> blobs);
+
+  // Filter blobs to get rid of noise, too large items, etc.
+  static std::vector<std::vector<cv::Point>> FilterBlobs(
+      std::vector<std::vector<cv::Point>> blobs,
+      std::vector<BlobStats> blob_stats);
+
+  // Draw Blobs on image
+  // Optionally draw all blobs and filtered blobs
+  static void DrawBlobs(
+      cv::Mat view_image,
+      const std::vector<std::vector<cv::Point>> &filtered_blobs,
+      const std::vector<std::vector<cv::Point>> &unfiltered_blobs,
+      const std::vector<BlobStats> &blob_stats);
+
+  static void ExtractBlobs(
+      cv::Mat rgb_image, cv::Mat binarized_image, cv::Mat blob_image,
+      std::vector<std::vector<cv::Point>> &filtered_blobs,
+      std::vector<std::vector<cv::Point>> &unfiltered_blobs,
+      std::vector<BlobStats> &blob_stats);
+};
+}  // namespace vision
+}  // namespace y2022
+
+#endif  // Y2022_BLOB_DETECTOR_H_
diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
new file mode 100644
index 0000000..f466c8b
--- /dev/null
+++ b/y2022/vision/camera_reader.cc
@@ -0,0 +1,85 @@
+#include "y2022/vision/camera_reader.h"
+
+#include <math.h>
+
+#include <opencv2/imgproc.hpp>
+
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/network/team_number.h"
+#include "frc971/vision/v4l2_reader.h"
+#include "frc971/vision/vision_generated.h"
+#include "y2020/vision/sift/sift_generated.h"
+#include "y2020/vision/sift/sift_training_generated.h"
+#include "y2020/vision/tools/python_code/sift_training_data.h"
+#include "y2022/vision/blob_detector.h"
+#include "y2022/vision/target_estimator.h"
+
+namespace y2022 {
+namespace vision {
+
+using namespace frc971::vision;
+
+const sift::CameraCalibration *CameraReader::FindCameraCalibration() const {
+  const std::string_view node_name = event_loop_->node()->name()->string_view();
+  const int team_number = aos::network::GetTeamNumber();
+  for (const sift::CameraCalibration *candidate :
+       *training_data_->camera_calibrations()) {
+    if (candidate->node_name()->string_view() != node_name) {
+      continue;
+    }
+    if (candidate->team_number() != team_number) {
+      continue;
+    }
+    return candidate;
+  }
+  LOG(FATAL) << ": Failed to find camera calibration for " << node_name
+             << " on " << team_number;
+}
+
+void CameraReader::ProcessImage(const CameraImage &image) {
+  // Remember, we're getting YUYV images, so we start by converting to RGB
+
+  // TOOD: Need to code this up for blob detection
+  cv::Mat image_mat(image.rows(), image.cols(), CV_8U);
+  CHECK(image_mat.isContinuous());
+  const int number_pixels = image.rows() * image.cols();
+  for (int i = 0; i < number_pixels; ++i) {
+    reinterpret_cast<uint8_t *>(image_mat.data)[i] =
+        image.data()->data()[i * 2];
+  }
+
+  // Now, send our two messages-- one large, with details for remote
+  // debugging(features), and one smaller
+  // TODO: Send debugging message as well
+  std::vector<std::vector<cv::Point> > filtered_blobs, unfiltered_blobs;
+  std::vector<BlobDetector::BlobStats> blob_stats;
+  cv::Mat binarized_image =
+      cv::Mat::zeros(cv::Size(image_mat.cols, image_mat.rows), CV_8UC1);
+  cv::Mat ret_image =
+      cv::Mat::zeros(cv::Size(image_mat.cols, image_mat.rows), CV_8UC3);
+  BlobDetector::ExtractBlobs(image_mat, binarized_image, ret_image,
+                             filtered_blobs, unfiltered_blobs, blob_stats);
+  // TODO(milind): use actual centroid
+  TargetEstimateT target = TargetEstimator::EstimateTargetLocation(
+      blob_stats[0].centroid, CameraIntrinsics(), CameraExtrinsics());
+
+  auto builder = target_estimate_sender_.MakeBuilder();
+  builder.CheckOk(builder.Send(TargetEstimate::Pack(*builder.fbb(), &target)));
+}
+
+void CameraReader::ReadImage() {
+  if (!reader_->ReadLatestImage()) {
+    read_image_timer_->Setup(event_loop_->monotonic_now() +
+                             std::chrono::milliseconds(10));
+    return;
+  }
+
+  ProcessImage(reader_->LatestImage());
+
+  reader_->SendLatestImage();
+  read_image_timer_->Setup(event_loop_->monotonic_now());
+}
+
+}  // namespace vision
+}  // namespace y2022
diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
new file mode 100644
index 0000000..6880248
--- /dev/null
+++ b/y2022/vision/camera_reader.h
@@ -0,0 +1,92 @@
+#ifndef Y2022_VISION_CAMERA_READER_H_
+#define Y2022_VISION_CAMERA_READER_H_
+
+#include <math.h>
+
+#include <opencv2/calib3d.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/network/team_number.h"
+#include "frc971/vision/v4l2_reader.h"
+#include "frc971/vision/vision_generated.h"
+#include "y2020/vision/sift/sift_generated.h"
+#include "y2020/vision/sift/sift_training_generated.h"
+#include "y2020/vision/tools/python_code/sift_training_data.h"
+#include "y2022/vision/target_estimate_generated.h"
+
+namespace y2022 {
+namespace vision {
+
+using namespace frc971::vision;
+
+// TODO<Jim/Milind>: Need to add in senders to send out the blob data/stats
+
+class CameraReader {
+ public:
+  CameraReader(aos::EventLoop *event_loop,
+               const sift::TrainingData *training_data, V4L2Reader *reader)
+      : event_loop_(event_loop),
+        training_data_(training_data),
+        camera_calibration_(FindCameraCalibration()),
+        reader_(reader),
+        image_sender_(event_loop->MakeSender<CameraImage>("/camera")),
+        target_estimate_sender_(
+            event_loop->MakeSender<TargetEstimate>("/camera")),
+        read_image_timer_(event_loop->AddTimer([this]() { ReadImage(); })) {
+    event_loop->OnRun(
+        [this]() { read_image_timer_->Setup(event_loop_->monotonic_now()); });
+  }
+
+ private:
+  const sift::CameraCalibration *FindCameraCalibration() const;
+
+  // Processes an image (including sending the results).
+  void ProcessImage(const CameraImage &image);
+
+  // Reads an image, and then performs all of our processing on it.
+  void ReadImage();
+
+  cv::Mat CameraIntrinsics() const {
+    const cv::Mat result(3, 3, CV_32F,
+                         const_cast<void *>(static_cast<const void *>(
+                             camera_calibration_->intrinsics()->data())));
+    CHECK_EQ(result.total(), camera_calibration_->intrinsics()->size());
+    return result;
+  }
+
+  cv::Mat CameraExtrinsics() const {
+    const cv::Mat result(
+        4, 4, CV_32F,
+        const_cast<void *>(static_cast<const void *>(
+            camera_calibration_->fixed_extrinsics()->data()->data())));
+    CHECK_EQ(result.total(),
+             camera_calibration_->fixed_extrinsics()->data()->size());
+    return result;
+  }
+
+  cv::Mat CameraDistCoeffs() const {
+    const cv::Mat result(5, 1, CV_32F,
+                         const_cast<void *>(static_cast<const void *>(
+                             camera_calibration_->dist_coeffs()->data())));
+    CHECK_EQ(result.total(), camera_calibration_->dist_coeffs()->size());
+    return result;
+  }
+
+  aos::EventLoop *const event_loop_;
+  const sift::TrainingData *const training_data_;
+  const sift::CameraCalibration *const camera_calibration_;
+  V4L2Reader *const reader_;
+  aos::Sender<CameraImage> image_sender_;
+  aos::Sender<TargetEstimate> target_estimate_sender_;
+
+  // We schedule this immediately to read an image. Having it on a timer
+  // means other things can run on the event loop in between.
+  aos::TimerHandler *const read_image_timer_;
+};
+
+}  // namespace vision
+}  // namespace y2022
+#endif  // Y2022_VISION_CAMERA_READER_H_
diff --git a/y2022/vision/camera_reader_main.cc b/y2022/vision/camera_reader_main.cc
new file mode 100644
index 0000000..6f65a6d
--- /dev/null
+++ b/y2022/vision/camera_reader_main.cc
@@ -0,0 +1,51 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2022/vision/camera_reader.h"
+
+// config used to allow running camera_reader independently.  E.g.,
+// bazel run //y2022/vision:camera_reader -- --config y2022/config.json
+//   --override_hostname pi-7971-1  --ignore_timestamps true
+DEFINE_string(config, "config.json", "Path to the config file to use.");
+DEFINE_uint32(exposure, 5, "Exposure time, in 100us increments");
+
+namespace y2022 {
+namespace vision {
+namespace {
+
+using namespace frc971::vision;
+
+void CameraReaderMain() {
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+
+  const aos::FlatbufferSpan<sift::TrainingData> training_data(
+      SiftTrainingData());
+  CHECK(training_data.Verify());
+
+  aos::ShmEventLoop event_loop(&config.message());
+
+  // First, log the data for future reference.
+  {
+    aos::Sender<sift::TrainingData> training_data_sender =
+        event_loop.MakeSender<sift::TrainingData>("/camera");
+    CHECK_EQ(training_data_sender.Send(training_data),
+             aos::RawSender::Error::kOk);
+  }
+
+  V4L2Reader v4l2_reader(&event_loop, "/dev/video0");
+  v4l2_reader.SetExposure(FLAGS_exposure);
+
+  CameraReader camera_reader(&event_loop, &training_data.message(),
+                             &v4l2_reader);
+
+  event_loop.Run();
+}
+
+}  // namespace
+}  // namespace vision
+}  // namespace y2022
+
+int main(int argc, char **argv) {
+  aos::InitGoogle(&argc, &argv);
+  y2022::vision::CameraReaderMain();
+}
diff --git a/y2022/vision/target_estimate.fbs b/y2022/vision/target_estimate.fbs
new file mode 100644
index 0000000..e22654a
--- /dev/null
+++ b/y2022/vision/target_estimate.fbs
@@ -0,0 +1,14 @@
+namespace y2022.vision;
+
+// Contains the information the EKF wants from blobs from a single image.
+table TargetEstimate {
+  // Horizontal distance from the camera to the center of the upper hub
+  distance:double (id: 0);
+  // Angle from the camera to the target (horizontal angle in rad).
+  // Positive means right of center, negative means left.
+  angle_to_target:double (id: 1);
+
+  // TODO(milind): add confidence and blob stats
+}
+
+root_type TargetEstimate;
diff --git a/y2022/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
new file mode 100644
index 0000000..c51eb08
--- /dev/null
+++ b/y2022/vision/target_estimator.cc
@@ -0,0 +1,31 @@
+#include "y2022/vision/target_estimator.h"
+
+namespace y2022::vision {
+
+TargetEstimateT TargetEstimator::EstimateTargetLocation(
+    cv::Point2i blob_point, const cv::Mat &intrinsics,
+    const cv::Mat &extrinsics) {
+  const cv::Point2d focal_length(intrinsics.at<double>(0, 0),
+                                 intrinsics.at<double>(1, 1));
+  const cv::Point2d offset(intrinsics.at<double>(0, 2),
+                           intrinsics.at<double>(1, 2));
+
+  // Blob pitch in camera reference frame
+  const double blob_pitch =
+      std::atan(static_cast<double>(-(blob_point.y - offset.y)) /
+                static_cast<double>(focal_length.y));
+  const double camera_height = extrinsics.at<double>(2, 3);
+  // Depth from camera to blob
+  const double depth = (kTapeHeight - camera_height) / std::tan(blob_pitch);
+
+  TargetEstimateT target;
+  target.angle_to_target =
+      std::atan2(static_cast<double>(blob_point.x - offset.x),
+                 static_cast<double>(focal_length.x));
+  target.distance =
+      (depth / std::cos(target.angle_to_target)) + kUpperHubRadius;
+
+  return target;
+}
+
+}  // namespace y2022::vision
diff --git a/y2022/vision/target_estimator.h b/y2022/vision/target_estimator.h
new file mode 100644
index 0000000..4988c3c
--- /dev/null
+++ b/y2022/vision/target_estimator.h
@@ -0,0 +1,26 @@
+#ifndef Y2022_VISION_POSE_ESTIMATOR_H_
+#define Y2022_VISION_POSE_ESTIMATOR_H_
+
+#include "opencv2/imgproc.hpp"
+#include "y2022/vision/target_estimate_generated.h"
+
+namespace y2022::vision {
+
+class TargetEstimator {
+ public:
+  // Computes the location of the target.
+  // blob_point is the mean (x, y) of blob pixels.
+  static TargetEstimateT EstimateTargetLocation(cv::Point2i blob_point,
+                                                const cv::Mat &intrinsics,
+                                                const cv::Mat &extrinsics);
+
+ private:
+  // Height of the center of the tape (m)
+  static constexpr double kTapeHeight = 2.58 + (0.05 / 2);
+  // Horizontal distance from tape to center of hub (m)
+  static constexpr double kUpperHubRadius = 1.22 / 2;
+};
+
+}  // namespace y2022::vision
+
+#endif  // Y2022_VISION_POSE_ESTIMATOR_H_
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
new file mode 100644
index 0000000..8ecb36d
--- /dev/null
+++ b/y2022/vision/viewer.cc
@@ -0,0 +1,144 @@
+#include <map>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <random>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/time/time.h"
+#include "frc971/vision/vision_generated.h"
+#include "y2022/vision/blob_detector.h"
+
+DEFINE_string(capture, "",
+              "If set, capture a single image and save it to this filename.");
+DEFINE_string(channel, "/camera", "Channel name for the image.");
+DEFINE_string(config, "config.json", "Path to the config file to use.");
+DEFINE_string(png_dir, "LED_Ring_exp", "Path to a set of images to display.");
+DEFINE_bool(show_features, true, "Show the blobs.");
+
+namespace y2022 {
+namespace vision {
+namespace {
+
+aos::Fetcher<frc971::vision::CameraImage> image_fetcher;
+
+bool DisplayLoop() {
+  int64_t image_timestamp = 0;
+  const frc971::vision::CameraImage *image;
+  // Read next image
+  if (!image_fetcher.FetchNext()) {
+    VLOG(2) << "Couldn't fetch next image";
+    return true;
+  }
+
+  image = image_fetcher.get();
+  CHECK(image != nullptr) << "Couldn't read image";
+  image_timestamp = image->monotonic_timestamp_ns();
+  VLOG(2) << "Got image at timestamp: " << image_timestamp;
+
+  // Create color image:
+  cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
+                          (void *)image->data()->data());
+  cv::Mat rgb_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
+  cv::cvtColor(image_color_mat, rgb_image, cv::COLOR_YUV2BGR_YUYV);
+
+  if (!FLAGS_capture.empty()) {
+    cv::imwrite(FLAGS_capture, rgb_image);
+    return false;
+  }
+
+  cv::Mat binarized_image, ret_image;
+  std::vector<std::vector<cv::Point>> unfiltered_blobs, filtered_blobs;
+  std::vector<BlobDetector::BlobStats> blob_stats;
+  BlobDetector::ExtractBlobs(rgb_image, binarized_image, ret_image,
+                             filtered_blobs, unfiltered_blobs, blob_stats);
+
+  LOG(INFO) << image->monotonic_timestamp_ns()
+            << ": # blobs: " << filtered_blobs.size();
+
+  // Downsize for viewing
+  cv::resize(rgb_image, rgb_image,
+             cv::Size(rgb_image.cols / 2, rgb_image.rows / 2),
+             cv::INTER_LINEAR);
+
+  cv::imshow("image", rgb_image);
+  cv::imshow("blobs", ret_image);
+
+  int keystroke = cv::waitKey(1);
+  if ((keystroke & 0xFF) == static_cast<int>('c')) {
+    // Convert again, to get clean image
+    cv::cvtColor(image_color_mat, rgb_image, cv::COLOR_YUV2BGR_YUYV);
+    std::stringstream name;
+    name << "capture-" << aos::realtime_clock::now() << ".png";
+    cv::imwrite(name.str(), rgb_image);
+    LOG(INFO) << "Saved image file: " << name.str();
+  } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
+    return false;
+  }
+  return true;
+}
+
+void ViewerMain() {
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+
+  aos::ShmEventLoop event_loop(&config.message());
+
+  image_fetcher =
+      event_loop.MakeFetcher<frc971::vision::CameraImage>(FLAGS_channel);
+
+  // Run the display loop
+  event_loop.AddPhasedLoop(
+      [&event_loop](int) {
+        if (!DisplayLoop()) {
+          LOG(INFO) << "Calling event_loop Exit";
+          event_loop.Exit();
+        };
+      },
+      ::std::chrono::milliseconds(100));
+
+  event_loop.Run();
+
+  image_fetcher = aos::Fetcher<frc971::vision::CameraImage>();
+}
+
+void ViewerLocal() {
+  std::vector<cv::String> file_list;
+  cv::glob(FLAGS_png_dir + "/*.png", file_list, false);
+  for (auto file : file_list) {
+    LOG(INFO) << "Reading file " << file;
+    cv::Mat rgb_image = cv::imread(file.c_str());
+    std::vector<std::vector<cv::Point>> filtered_blobs, unfiltered_blobs;
+    std::vector<BlobDetector::BlobStats> blob_stats;
+    cv::Mat binarized_image =
+        cv::Mat::zeros(cv::Size(rgb_image.cols, rgb_image.rows), CV_8UC1);
+    cv::Mat ret_image =
+        cv::Mat::zeros(cv::Size(rgb_image.cols, rgb_image.rows), CV_8UC3);
+    BlobDetector::ExtractBlobs(rgb_image, binarized_image, ret_image,
+                               filtered_blobs, unfiltered_blobs, blob_stats);
+
+    LOG(INFO) << ": # blobs: " << filtered_blobs.size() << " (# removed: "
+              << unfiltered_blobs.size() - filtered_blobs.size() << ")";
+    cv::imshow("image", rgb_image);
+    cv::imshow("blobs", ret_image);
+
+    int keystroke = cv::waitKey(0);
+    if ((keystroke & 0xFF) == static_cast<int>('q')) {
+      return;
+    }
+  }
+}
+}  // namespace
+}  // namespace vision
+}  // namespace y2022
+
+// Quick and lightweight viewer for images
+int main(int argc, char **argv) {
+  aos::InitGoogle(&argc, &argv);
+  if (FLAGS_png_dir != "")
+    y2022::vision::ViewerLocal();
+  else
+    y2022::vision::ViewerMain();
+}
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 5f43b70..f4cbac8 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -193,17 +193,33 @@
       : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
             event_loop, "/superstructure") {}
 
+  void set_climber_falcon(
+      ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+    climber_falcon_ = ::std::move(t);
+    climber_falcon_->ConfigSupplyCurrentLimit(
+        {true, Values::kClimberSupplyCurrentLimit(),
+         Values::kClimberSupplyCurrentLimit(), 0});
+  }
+
  private:
-  void WriteToFalcon(const double voltage,
-                     ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
+  void WriteToFalconCan(const double voltage,
+                        ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
     falcon->Set(
         ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
         std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
   }
 
-  void Write(const superstructure::Output & /* output */) override {}
+  void Write(const superstructure::Output &output) override {
+    WriteToFalconCan(output.climber_voltage(), climber_falcon_.get());
+  }
 
-  void Stop() override { AOS_LOG(WARNING, "Superstructure output too old.\n"); }
+  void Stop() override {
+    AOS_LOG(WARNING, "Superstructure output too old.\n");
+    climber_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+  }
+
+  ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+      climber_falcon_;
 };
 
 class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -246,6 +262,9 @@
 
     SuperstructureWriter superstructure_writer(&output_event_loop);
 
+    superstructure_writer.set_climber_falcon(
+        make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+
     AddLoop(&output_event_loop);
 
     RunLoops();
diff --git a/y2022/y2022.json b/y2022/y2022.json
index 4abbd5e..010c675 100644
--- a/y2022/y2022.json
+++ b/y2022/y2022.json
@@ -1,39 +1,23 @@
 {
-  "channels":
-  [
+  "channel_storage_duration": 2000000000,
+  "maps": [
     {
-      "name": "/superstructure",
-      "type": "y2022.control_loops.superstructure.Goal",
-      "frequency": 200
-    },
-    {
-      "name": "/superstructure",
-      "type": "y2022.control_loops.superstructure.Status",
-      "frequency": 200
-    },
-    {
-      "name": "/superstructure",
-      "type": "y2022.control_loops.superstructure.Output",
-      "frequency": 200
-    },
-    {
-      "name": "/superstructure",
-      "type": "y2022.control_loops.superstructure.Position",
-      "frequency": 200
-    }
-  ],
-  "applications": [
-    {
-      "name": "drivetrain"
-    },
-    {
-      "name": "superstructure"
+      "match": {
+        "name": "/aos",
+        "type": "aos.RobotState"
+      },
+      "rename": {
+        "name": "/roborio/aos"
+      }
     }
   ],
   "imports": [
-    "../frc971/input/robot_state_config.json",
-    "../frc971/control_loops/drivetrain/drivetrain_config.json",
-    "../frc971/autonomous/autonomous_config.json",
-    "../frc971/wpilib/wpilib_config.json"
+    "y2022_roborio.json",
+    "y2022_pi1.json",
+    "y2022_pi2.json",
+    "y2022_pi3.json",
+    "y2022_pi4.json",
+    "y2022_pi5.json",
+    "y2022_logger.json"
   ]
 }
diff --git a/y2022/y2022_pi_template.json b/y2022/y2022_pi_template.json
index 81b96d2..4994a36 100644
--- a/y2022/y2022_pi_template.json
+++ b/y2022/y2022_pi_template.json
@@ -99,6 +99,13 @@
       "num_senders": 18
     },
     {
+      "name": "/pi{{ NUM }}/camera",
+      "type": "y2022.vision.TargetEstimate",
+      "source_node": "pi{{ NUM }}",
+      "frequency": 25,
+      "num_senders": 2
+    },
+    {
       "name": "/logger/aos",
       "type": "aos.starter.StarterRpc",
       "source_node": "logger",