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/BUILD

diff --git a/y2020/BUILD b/y2020/BUILD
index 17d3020..b044f53 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -60,7 +60,7 @@
         "//frc971/autonomous:auto_mode_fbs",
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
-        "//frc971/wpilib:ADIS16448",
+        "//frc971/wpilib:ADIS16470",
         "//frc971/wpilib:buffered_pcm",
         "//frc971/wpilib:drivetrain_writer",
         "//frc971/wpilib:encoder_and_potentiometer",
@@ -107,6 +107,7 @@
         "//y2020/control_loops/superstructure:superstructure_goal_fbs",
         "//y2020/control_loops/superstructure:superstructure_output_fbs",
         "//y2020/control_loops/superstructure:superstructure_position_fbs",
+        "//y2019/control_loops/drivetrain:target_selector_fbs",
         "//y2020/control_loops/superstructure:superstructure_status_fbs",
     ],
     visibility = ["//visibility:public"],

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.

y2020/y2020.json

diff --git a/y2020/y2020.json b/y2020/y2020.json
index 411e3ff..7680ef5 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -17,6 +17,10 @@
       "frequency": 200
     },
     {
+      "name": "/drivetrain",
+      "type": "y2019.control_loops.drivetrain.TargetSelectorHint"
+    },
+    {
       "name": "/superstructure",
       "type": "y2020.control_loops.superstructure.Position",
       "frequency": 200