commit a244a91d48c75928d521c1e7c10dcdb344d5a74a
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Jan 18 13:50:50 2020 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Sun Jan 19 15:07:22 2020 -0800
tree dc9bf450345f2230d2c854c9a9e661538688f466
parent 47d0703243057be0527c435ac81bc5805e01e0bd

Use ADIS16470 in 2020 robot

Change-Id: I0b0ff931b98efacaf7cb7d3ba92940c78c71af9e

y2020/wpilib_interface.cc

diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 4968c9f..a363775 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -34,7 +34,7 @@
 #include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
 #include "frc971/autonomous/auto_mode_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
-#include "frc971/wpilib/ADIS16448.h"
+#include "frc971/wpilib/ADIS16470.h"
 #include "frc971/wpilib/buffered_pcm.h"
 #include "frc971/wpilib/buffered_solenoid.h"
 #include "frc971/wpilib/dma.h"
@@ -130,7 +130,11 @@
     autonomous_modes_.at(i) = ::std::move(sensor);
   }
 
+  void set_imu(frc971::wpilib::ADIS16470 *imu) { imu_ = imu; }
+
   void RunIteration() override {
+    CHECK_NOTNULL(imu_)->DoReads();
+
     {
       auto builder = drivetrain_position_sender_.MakeBuilder();
       frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
@@ -181,6 +185,8 @@
       drivetrain_position_sender_;
 
   ::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+
+  frc971::wpilib::ADIS16470 *imu_ = nullptr;
 };
 
 class SuperstructureWriter
@@ -224,6 +230,13 @@
     sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
     sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
 
+    auto imu_trigger = make_unique<frc::DigitalInput>(0);
+    auto imu_reset = make_unique<frc::DigitalOutput>(1);
+    auto spi = make_unique<frc::SPI>(frc::SPI::Port::kOnboardCS2);
+    frc971::wpilib::ADIS16470 imu(&sensor_reader_event_loop, spi.get(),
+                                  imu_trigger.get(), imu_reset.get());
+    sensor_reader.set_imu(&imu);
+
     AddLoop(&sensor_reader_event_loop);
 
     // Thread 4.