commit 7ecf067fca3a04a3e89a208d4ae8511b4f9a7f4b
author Brian Silverman <bsilver16384@gmail.com> Sat Mar 02 15:30:03 2019 -0800
committer Brian Silverman <bsilver16384@gmail.com> Sat Mar 02 15:51:25 2019 -0800
tree 1b428026bc34604a309f81eb985fd9366569f10d
parent e9924fd548add786d851f5fdb124143416515668

Add support for active-low buttons to switch camera modes

Change-Id: I254408f35dd7b48c48210111e9ee99c40198198a

y2019/wpilib_interface.cc

diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index be6ee33..02269eb 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -279,11 +279,26 @@
     spi_->SetMSBFirst();
   }
 
+  void set_activate_usb(std::unique_ptr<frc::DigitalInput> activate_usb) {
+    activate_usb_ = std::move(activate_usb);
+  }
+
+  void set_activate_passthrough(
+      std::unique_ptr<frc::DigitalInput> activate_passthrough) {
+    activate_passthrough_ = std::move(activate_passthrough);
+  }
+
   void DoSpiTransaction() {
     using namespace frc971::jevois;
     RoborioToTeensy to_teensy{};
     to_teensy.realtime_now = aos::realtime_clock::now();
-    to_teensy.camera_command = CameraCommand::kNormal;
+    if (activate_usb_ && !activate_usb_->Get()) {
+      to_teensy.camera_command = CameraCommand::kUsb;
+    } else if (activate_passthrough_ && !activate_passthrough_->Get()) {
+      to_teensy.camera_command = CameraCommand::kCameraPassthrough;
+    } else {
+      to_teensy.camera_command = CameraCommand::kNormal;
+    }
 
     std::array<char, spi_transfer_size() + 1> to_send{};
     {
@@ -344,6 +359,9 @@
   frc::SPI *spi_ = nullptr;
   ::std::unique_ptr<frc::SPI> dummy_spi_;
 
+  std::unique_ptr<frc::DigitalInput> activate_usb_;
+  std::unique_ptr<frc::DigitalInput> activate_passthrough_;
+
   frc971::wpilib::SpiRxClearer rx_clearer_;
 };
 
@@ -554,6 +572,9 @@
     frc::SPI camera_spi(frc::SPI::Port::kOnboardCS3);
     camera_reader.set_spi(&camera_spi);
     camera_reader.SetDummySPI(frc::SPI::Port::kOnboardCS2);
+    // Austin says 8, 9, 24, and 25 are good options to choose from for these.
+    camera_reader.set_activate_usb(make_unique<frc::DigitalInput>(24));
+    camera_reader.set_activate_passthrough(make_unique<frc::DigitalInput>(25));
 
     auto imu_trigger = make_unique<frc::DigitalInput>(0);
     ::frc971::wpilib::ADIS16448 imu(frc::SPI::Port::kOnboardCS1,