Add support for the IMU
Change-Id: Iaff4b088d9bec8edf439e00dda5b788cf87dff0c
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
new file mode 100644
index 0000000..d834f33
--- /dev/null
+++ b/frc971/wpilib/ADIS16448.cc
@@ -0,0 +1,353 @@
+#include "frc971/wpilib/ADIS16448.h"
+
+#include "InterruptableSensorBase.h"
+#undef ERROR
+
+#include <inttypes.h>
+#include <math.h>
+
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+
+#include "frc971/wpilib/imu.q.h"
+
+namespace frc971 {
+namespace wpilib {
+
+template <uint8_t size>
+bool ADIS16448::DoTransaction(uint8_t to_send[size], uint8_t to_receive[size]) {
+ switch (spi_->Transaction(to_send, to_receive, size)) {
+ case -1:
+ LOG(INFO, "SPI::Transaction of %zd bytes failed\n", size);
+ return false;
+ case size:
+ return true;
+ default:
+ LOG(FATAL, "SPI::Transaction returned something weird\n");
+ }
+}
+
+namespace {
+
+// Addresses pulled out of the documentation.
+constexpr uint8_t kMscCtrlAddress = 0x34;
+constexpr uint8_t kSmplPrdAddress = 0x36;
+constexpr uint8_t kDiagStatAddress = 0x3C;
+constexpr uint8_t kGlobalReadAddress = 0x3E;
+constexpr uint8_t kLotId1Address = 0x52;
+constexpr uint8_t kLotId2Address = 0x54;
+constexpr uint8_t kProdIdAddress = 0x56;
+constexpr uint8_t kSerialNumberAddress = 0x58;
+
+// LSB/degree/second for the gyros.
+constexpr double kGyroLsbDegreeSecond = 25;
+// LSB/G for the accelerometers.
+constexpr double kAccelerometerLsbG = 1200;
+// LSB/gauss for the magnetometers.
+constexpr double kMagnetometerLsbGauss = 7.0 / 1000.0 /* mgauss to gauss */;
+// LSB/bar for the barometer.
+constexpr double kBarometerLsbPascal = 1.0 / (20.0 / 10 /* ubar to pascals */);
+// LSB/degree C for the temperature sensor.
+constexpr double kTemperatureLsbDegree = 1.0 / 0.07386;
+// Degrees C corresponding to 0 for the temperature sensor.
+constexpr double kTemperatureZero = 31;
+
+// From somebody online who says this works with the sensor. I don't feel like
+// re-deriving this, and I can't find what all the CRC parameters are supposed
+// to be.
+const uint16_t kCrcTable[256] = {
+ 0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF, 0x1F3F,
+ 0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890, 0x1E3D, 0x09F3,
+ 0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992, 0x0102, 0x16CC, 0x0EDD,
+ 0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD, 0x1C39, 0x0BF7, 0x13E6, 0x0428,
+ 0x0387, 0x1449, 0x0C58, 0x1B96, 0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8,
+ 0x0B76, 0x1367, 0x04A9, 0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74,
+ 0x1265, 0x05AB, 0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A,
+ 0x1A94, 0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E,
+ 0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1, 0x060C,
+ 0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3, 0x1933, 0x0EFD,
+ 0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C, 0x0408, 0x13C6, 0x0BD7,
+ 0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7, 0x1B37, 0x0CF9, 0x14E8, 0x0326,
+ 0x0489, 0x1347, 0x0B56, 0x1C98, 0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B,
+ 0x1245, 0x0A54, 0x1D9A, 0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A,
+ 0x156B, 0x02A5, 0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040,
+ 0x178E, 0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1,
+ 0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3, 0x1123,
+ 0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C, 0x0C18, 0x1BD6,
+ 0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7, 0x1327, 0x04E9, 0x1CF8,
+ 0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488, 0x1225, 0x05EB, 0x1DFA, 0x0A34,
+ 0x0D9B, 0x1A55, 0x0244, 0x158A, 0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4,
+ 0x056A, 0x1D7B, 0x0AB5, 0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060,
+ 0x1871, 0x0FBF, 0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E,
+ 0x1080, 0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182,
+ 0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD, 0x1429,
+ 0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386, 0x0B16, 0x1CD8,
+ 0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9, 0x0A14, 0x1DDA, 0x05CB,
+ 0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB, 0x152B, 0x02E5, 0x1AF4, 0x0D3A,
+ 0x0A95, 0x1D5B, 0x054A, 0x1284};
+
+uint16_t CalculateCrc(const uint8_t *data, size_t data_length) {
+ uint16_t crc = 0xFFFF;
+ uint16_t byte;
+
+ while (data_length--) {
+ // Compute lower byte CRC first.
+ byte = data[1];
+ crc = (crc >> 8) ^ kCrcTable[(crc & 0x00FF) ^ byte];
+ // Compute upper byte of CRC.
+ byte = data[0];
+ crc = (crc >> 8) ^ kCrcTable[(crc & 0x00FF) ^ byte];
+ data += 2;
+ }
+ crc = ~crc; // Compute complement of CRC
+ return static_cast<uint16_t>(
+ (crc << 8) | (crc >> 8)); // Perform byte swap prior to returning CRC;
+}
+
+} // namespace
+
+ADIS16448::ADIS16448(SPI::Port port, DigitalInput *dio1)
+ : spi_(new SPI(port)), dio1_(dio1) {
+ // 1MHz is the maximum supported for burst reads.
+ spi_->SetClockRate(1e6);
+ spi_->SetChipSelectActiveLow();
+ spi_->SetClockActiveLow();
+ spi_->SetSampleDataOnFalling();
+ spi_->SetMSBFirst();
+
+ dio1_->RequestInterrupts();
+ dio1_->SetUpSourceEdge(true, false);
+}
+
+void ADIS16448::operator()() {
+ ::aos::SetCurrentThreadName("IMU");
+
+ // Try to initialize repeatedly as long as we're supposed to be running.
+ while (run_ && !Initialize()) {
+ ::aos::time::SleepFor(::aos::time::Time::InMS(50));
+ }
+ LOG(INFO, "IMU initialized successfully\n");
+
+ bool got_an_interrupt = false;
+ while (run_) {
+ {
+ const InterruptableSensorBase::WaitResult result =
+ dio1_->WaitForInterrupt(0.1, !got_an_interrupt);
+ if (result == InterruptableSensorBase::kTimeout) {
+ LOG(WARNING, "IMU read timed out\n");
+ continue;
+ }
+ }
+ got_an_interrupt = true;
+ const ::aos::time::Time read_time = ::aos::time::Time::Now();
+
+ uint8_t to_send[8 * 2 * 14], to_receive[8 * 2 * 14];
+ to_send[0] = kGlobalReadAddress;
+ memset(&to_send[1], 0, sizeof(to_send) - 1);
+ if (!DoTransaction<8 * 2 * 14>(to_send, to_receive)) continue;
+
+ // If it's false now or another edge happened, then we're in trouble. This
+ // won't catch all instances of being a little bit slow (because of the
+ // interrupt delay among other things), but it will catch the code
+ // constantly falling behind, which seems like the most likely failure
+ // scenario.
+ if (!dio1_->Get() ||
+ dio1_->WaitForInterrupt(0, false) !=
+ InterruptableSensorBase::kTimeout) {
+ LOG(ERROR, "IMU read took too long\n");
+ continue;
+ }
+
+ {
+ const uint16_t calculated_crc = CalculateCrc(&to_receive[4], 11);
+ uint16_t received_crc;
+ memcpy(&received_crc, &to_receive[13], 2);
+ if (received_crc != calculated_crc) {
+ LOG(WARNING, "received CRC %" PRIx16 " but calculated %" PRIx16 "\n",
+ received_crc, calculated_crc);
+ continue;
+ }
+ }
+
+ {
+ uint16_t diag_stat;
+ memcpy(&diag_stat, &to_receive[1], 2);
+ if (!CheckDiagStatValue(diag_stat)) continue;
+ }
+
+ auto message = imu_values.MakeMessage();
+ message->fpga_timestamp = dio1_->ReadRisingTimestamp();
+ message->monotonic_timestamp_ns = read_time.ToNSec();
+
+ message->gyro_x =
+ ConvertValue(&to_receive[2], kGyroLsbDegreeSecond) * (M_PI / 2.0);
+ message->gyro_y =
+ ConvertValue(&to_receive[3], kGyroLsbDegreeSecond) * (M_PI / 2.0);
+ message->gyro_z =
+ ConvertValue(&to_receive[4], kGyroLsbDegreeSecond) * (M_PI / 2.0);
+
+ message->accelerometer_x = ConvertValue(&to_receive[5], kAccelerometerLsbG);
+ message->accelerometer_y = ConvertValue(&to_receive[6], kAccelerometerLsbG);
+ message->accelerometer_z = ConvertValue(&to_receive[7], kAccelerometerLsbG);
+
+ message->magnetometer_x =
+ ConvertValue(&to_receive[8], kMagnetometerLsbGauss);
+ message->magnetometer_y =
+ ConvertValue(&to_receive[9], kMagnetometerLsbGauss);
+ message->magnetometer_z =
+ ConvertValue(&to_receive[10], kMagnetometerLsbGauss);
+
+ message->barometer =
+ ConvertValue(&to_receive[11], kBarometerLsbPascal, false);
+
+ message->temperature =
+ ConvertValue(&to_receive[12], kTemperatureLsbDegree) + kTemperatureZero;
+
+ LOG_STRUCT(DEBUG, "sending", *message);
+ if (!message.Send()) {
+ LOG(WARNING, "sending queue message failed\n");
+ }
+ }
+}
+
+float ADIS16448::ConvertValue(uint8_t *data, double lsb_per_output, bool sign) {
+ double value;
+ if (sign) {
+ int16_t raw_value;
+ memcpy(&raw_value, data, 2);
+ value = raw_value;
+ } else {
+ uint16_t raw_value;
+ memcpy(&raw_value, data, 2);
+ value = raw_value;
+ }
+ return value * lsb_per_output;
+}
+
+bool ADIS16448::ReadRegister(uint8_t next_address, uint16_t *value) {
+ uint8_t to_send[2], to_receive[2];
+ to_send[0] = next_address;
+ to_send[1] = 0;
+
+ if (!DoTransaction<2>(to_send, to_receive)) return false;
+
+ if (value) memcpy(value, to_receive, 2);
+ return true;
+}
+
+bool ADIS16448::WriteRegister(uint8_t address, uint16_t value) {
+ uint8_t to_send[4], to_receive[4];
+ to_send[0] = address | 0x80;
+ to_send[1] = value & 0xFF;
+ to_send[2] = address | 0x81;
+ to_send[3] = value >> 8;
+ if (!DoTransaction<4>(to_send, to_receive)) return false;
+ return true;
+}
+
+bool ADIS16448::CheckDiagStatValue(uint16_t value) const {
+ bool r = true;
+ if (value & (1 << 0)) {
+ LOG(WARNING, "IMU gave magnetometer functional test failure\n");
+ }
+ if (value & (1 << 1)) {
+ LOG(WARNING, "IMU gave barometer functional test failure\n");
+ }
+ if (value & (1 << 2)) {
+ LOG(WARNING, "IMU gave flash update failure\n");
+ }
+ if (value & (1 << 3)) {
+ LOG(WARNING, "IMU gave SPI communication failure\n");
+ }
+ if (value & (1 << 4)) {
+ LOG(WARNING, "IMU gave sensor overrange\n");
+ }
+ if (value & (1 << 5)) {
+ LOG(WARNING, "IMU gave self-test failure\n");
+ r = false;
+ }
+ if (value & (1 << 6)) {
+ LOG(WARNING, "IMU gave flash test checksum failure\n");
+ }
+ if (value & (1 << 8)) {
+ LOG(WARNING, "IMU says alarm 1 is active\n");
+ }
+ if (value & (1 << 9)) {
+ LOG(WARNING, "IMU says alarm 2 is active\n");
+ }
+ if (value & (1 << 10)) {
+ LOG(WARNING, "IMU gave X-axis gyro self-test failure\n");
+ r = false;
+ }
+ if (value & (1 << 11)) {
+ LOG(WARNING, "IMU gave Y-axis gyro self-test failure\n");
+ r = false;
+ }
+ if (value & (1 << 12)) {
+ LOG(WARNING, "IMU gave Z-axis gyro self-test failure\n");
+ r = false;
+ }
+ if (value & (1 << 13)) {
+ LOG(WARNING, "IMU gave X-axis accelerometer self-test failure\n");
+ r = false;
+ }
+ if (value & (1 << 14)) {
+ LOG(WARNING, "IMU gave Y-axis accelerometer self-test failure\n");
+ r = false;
+ }
+ if (value & (1 << 15)) {
+ LOG(WARNING, "IMU gave Z-axis accelerometer self-test failure\n");
+ r = false;
+ }
+ return r;
+}
+
+bool ADIS16448::Initialize() {
+ if (!ReadRegister(kProdIdAddress, nullptr)) return false;
+ uint16_t product_id;
+ if (!ReadRegister(kLotId1Address, &product_id)) return false;
+ if (product_id != 0x4040) {
+ LOG(ERROR, "product ID is %" PRIx16 " instead of 0x4040\n", product_id);
+ return false;
+ }
+
+ uint16_t lot_id1, lot_id2, serial_number;
+ if (!ReadRegister(kLotId2Address, &lot_id1)) return false;
+ if (!ReadRegister(kSerialNumberAddress, &lot_id2)) return false;
+ if (!ReadRegister(0, &serial_number)) return false;
+ LOG(INFO, "have IMU %" PRIx16 "%" PRIx16 ": %" PRIx16 "\n", lot_id1, lot_id2,
+ serial_number);
+
+ // Divide the sampling by 2^2 = 4 to get 819.2 / 4 = 204.8 Hz.
+ if (!WriteRegister(kSmplPrdAddress, 2 << 8)) return false;
+
+ // Start a self test.
+ if (!WriteRegister(kMscCtrlAddress, 1 << 10)) return false;
+ // Wait for the self test to finish.
+ {
+ uint16_t value;
+ do {
+ ::aos::time::SleepFor(::aos::time::Time::InMS(10));
+ if (!ReadRegister(kMscCtrlAddress, &value)) return false;
+ } while ((value & (1 << 10)) != 0);
+ }
+
+ if (!ReadRegister(kDiagStatAddress, nullptr)) return false;
+ uint16_t diag_stat;
+ if (!ReadRegister(0, &diag_stat)) return false;
+ if (!CheckDiagStatValue(diag_stat)) return false;
+
+ if (!WriteRegister(kMscCtrlAddress,
+ ((0 << 0) | // DIO1
+ (1 << 1) | // DIO goes high when data is valid
+ (1 << 2) | // enable DIO changing when data is vald
+ (1 << 4)) // enable CRC16 for burst mode
+ )) {
+ return false;
+ }
+ return true;
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
new file mode 100644
index 0000000..778ab49
--- /dev/null
+++ b/frc971/wpilib/ADIS16448.h
@@ -0,0 +1,78 @@
+#ifndef FRC971_WPILIB_ADIS16448_H_
+#define FRC971_WPILIB_ADIS16448_H_
+
+#include <stdint.h>
+
+#include <memory>
+#include <atomic>
+
+#include "SPI.h"
+#include "DigitalInput.h"
+#undef ERROR
+
+#include "aos/common/logging/logging.h"
+
+namespace frc971 {
+namespace wpilib {
+
+// Handles interfacing with an Analog Devices ADIS16448 Inertial Sensor over
+// SPI and sending values out on a queue.
+//
+// The sensor is configured to generate samples at 204.8 Hz, and the values are
+// sent out as each sample is received.
+//
+// This is designed to be passed into ::std::thread's constructor so it will run
+// as a separate thread.
+class ADIS16448 {
+ public:
+ // port is where to find the sensor over SPI.
+ // dio1 must be connected to DIO1 on the sensor.
+ ADIS16448(SPI::Port port, DigitalInput *dio1);
+
+ // For ::std::thread to call.
+ //
+ // Initializes the sensor and then loops until Quit() is called taking
+ // readings.
+ void operator()();
+
+ void Quit() { run_ = false; }
+
+ private:
+ // Converts a 16-bit value at data to a scaled output value where a value of 1
+ // corresponds to lsb_per_output.
+ float ConvertValue(uint8_t *data, double lsb_per_output, bool sign = true);
+
+ // Performs an SPI transaction.
+ // Returns true if it succeeds.
+ template <uint8_t size>
+ bool DoTransaction(uint8_t to_send[size], uint8_t to_receive[size]);
+
+ // Reads one of the gyro's registers and returns the value in value.
+ // next_address is the address of the *next* register to read.
+ // Not sure what gets stored in value for the first read, but it should be
+ // ignored. Passing nullptr for value is allowed to completely ignore it.
+ // Returns true if it succeeds.
+ bool ReadRegister(uint8_t next_address, uint16_t *value);
+
+ // Writes a value to one of the registers.
+ // Returns true if it succeeds.
+ bool WriteRegister(uint8_t address, uint16_t value);
+
+ // Checks the given value of the DIAG_STAT register and logs any errors.
+ // Returns true if there are no errors we care about.
+ bool CheckDiagStatValue(uint16_t value) const;
+
+ // Starts everything up and runs a self test.
+ // Returns true if it succeeds.
+ bool Initialize();
+
+ const ::std::unique_ptr<SPI> spi_;
+ DigitalInput *const dio1_;
+
+ ::std::atomic<bool> run_{true};
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_ADIS16448_H_
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 0e31d83..288f809 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -206,3 +206,28 @@
'//aos/externals:wpilib',
],
)
+
+queue_library(
+ name = 'imu_queue',
+ srcs = [
+ 'imu.q',
+ ],
+)
+
+cc_library(
+ name = 'ADIS16448',
+ hdrs = [
+ 'ADIS16448.h',
+ ],
+ srcs = [
+ 'ADIS16448.cc',
+ ],
+ deps = [
+ '//aos/externals:wpilib',
+ '//aos/common/logging',
+ '//aos/common/logging:queue_logging',
+ '//aos/common:time',
+ '//aos/linux_code:init',
+ ':imu_queue',
+ ],
+)
diff --git a/frc971/wpilib/gyro_interface.cc b/frc971/wpilib/gyro_interface.cc
index 4b440a9..6b3ab40 100644
--- a/frc971/wpilib/gyro_interface.cc
+++ b/frc971/wpilib/gyro_interface.cc
@@ -75,7 +75,11 @@
static_assert(kBytes == sizeof(to_write),
"need the same number of bytes as sizeof(the data)");
- if (__builtin_parity(to_write & ~1) == 0) to_write |= 1;
+ if (__builtin_parity(to_write & ~1) == 0) {
+ to_write |= 1;
+ } else {
+ to_write &= ~1;
+ }
uint8_t to_send[kBytes], to_receive[kBytes];
const uint32_t to_write_flipped = __builtin_bswap32(to_write);
@@ -124,7 +128,7 @@
}
double GyroInterface::ExtractAngle(uint32_t value) {
- const int16_t reading = -(int16_t)(value >> 10 & 0xFFFF);
+ const int16_t reading = -static_cast<int16_t>(value >> 10 & 0xFFFF);
return static_cast<double>(reading) * 2.0 * M_PI / 360.0 / 80.0;
}
diff --git a/frc971/wpilib/gyro_sender.h b/frc971/wpilib/gyro_sender.h
index 029ace9..a36fc2e 100644
--- a/frc971/wpilib/gyro_sender.h
+++ b/frc971/wpilib/gyro_sender.h
@@ -20,7 +20,7 @@
// For ::std::thread to call.
//
- // Initializes the gyro and then loops forever taking readings.
+ // Initializes the gyro and then loops until Quit() is called taking readings.
void operator()();
void Quit() { run_ = false; }
diff --git a/frc971/wpilib/imu.q b/frc971/wpilib/imu.q
new file mode 100644
index 0000000..c404126
--- /dev/null
+++ b/frc971/wpilib/imu.q
@@ -0,0 +1,41 @@
+package frc971;
+
+// Values returned from an IMU.
+message IMUValues {
+ // Gyro readings in radians/second.
+ // Positive is clockwise looking at the connector.
+ float gyro_x;
+ // Positive is clockwise looking at the right side (from the connector).
+ float gyro_y;
+ // Positive is counterclockwise looking at the top.
+ float gyro_z;
+
+ // Accelerometer readings in Gs.
+ // Positive is up.
+ float accelerometer_x;
+ // Positive is away from the right side (from the connector).
+ float accelerometer_y;
+ // Positive is away from the connector.
+ float accelerometer_z;
+
+ // Magnetometer readings in gauss.
+ // Positive is up.
+ float magnetometer_x;
+ // Positive is away from the right side (from the connector).
+ float magnetometer_y;
+ // Positive is away from the connector.
+ float magnetometer_z;
+
+ // Barometer readings in pascals.
+ float barometer;
+
+ // Temperature readings in degrees Celsius.
+ float temperature;
+
+ // FPGA timestamp when the values were captured.
+ double fpga_timestamp;
+ // CLOCK_MONOTONIC time in nanoseconds when the values were captured.
+ int64_t monotonic_timestamp_ns;
+};
+
+queue IMUValues imu_values;
diff --git a/y2016/wpilib/BUILD b/y2016/wpilib/BUILD
index 1571623..13e5eae 100644
--- a/y2016/wpilib/BUILD
+++ b/y2016/wpilib/BUILD
@@ -31,6 +31,7 @@
'//frc971/wpilib:logging_queue',
'//frc971/wpilib:wpilib_interface',
'//frc971/wpilib:pdp_fetcher',
+ '//frc971/wpilib:ADIS16448',
'//y2016:constants',
'//y2016/control_loops/drivetrain:polydrivetrain_plants',
'//y2016/control_loops/shooter:shooter_queue',
diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 1898951..6d16323 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -49,6 +49,7 @@
#include "frc971/wpilib/logging.q.h"
#include "frc971/wpilib/wpilib_interface.h"
#include "frc971/wpilib/pdp_fetcher.h"
+#include "frc971/wpilib/ADIS16448.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -631,6 +632,12 @@
::frc971::wpilib::GyroSender gyro_sender;
::std::thread gyro_thread(::std::ref(gyro_sender));
+#if 0
+ auto imu_trigger = make_unique<DigitalInput>(100);
+ ::frc971::wpilib::ADIS16448 imu(SPI::Port::kOnboardCS1, imu_trigger.get());
+ ::std::thread imu_thread(::std::ref(imu));
+#endif
+
DrivetrainWriter drivetrain_writer;
drivetrain_writer.set_drivetrain_left_talon(
::std::unique_ptr<Talon>(new Talon(5)));
@@ -691,6 +698,10 @@
reader_thread.join();
gyro_sender.Quit();
gyro_thread.join();
+#if 0
+ imu.Quit();
+ imu_thread.join();
+#endif
drivetrain_writer.Quit();
drivetrain_writer_thread.join();