commit e2f2248b2e032cd57ce2971e0f25fe2c98a23741
author Austin Schuh <austin.linux@gmail.com> Sat Apr 13 23:05:43 2019 -0700
committer Austin Schuh <austin.linux@gmail.com> Sat Apr 13 23:10:05 2019 -0700
tree 56c64f0ca65e74168af6bd5a729fb1dabc4e5cab
parent ce2885dbf3fd667df13185a6087e78549602d289

Require the robot to see the platform before retracting the stilts

If you hit the level 2 button, it will force retract them.

Change-Id: I8d16cd3c06769043c7a5fd50de50aa1d6ac2e3d8

y2019/wpilib_interface.cc

diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 95ed28e..17dabde 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -198,6 +198,16 @@
     stilts_encoder_.set_potentiometer(::std::move(potentiometer));
   }
 
+  void set_platform_left_detect(
+      ::std::unique_ptr<frc::DigitalInput> platform_left_detect) {
+    platform_left_detect_ = ::std::move(platform_left_detect);
+  }
+
+  void set_platform_right_detect(
+      ::std::unique_ptr<frc::DigitalInput> platform_right_detect) {
+    platform_right_detect_ = ::std::move(platform_right_detect);
+  }
+
   // Vacuum pressure sensor
   void set_vacuum_sensor(int port) {
     vacuum_sensor_ = make_unique<frc::AnalogInput>(port);
@@ -257,6 +267,11 @@
           (vacuum_sensor_->GetVoltage() - kMinVoltage) /
           (kMaxVoltage - kMinVoltage);
 
+      superstructure_message->platform_left_detect =
+          !platform_left_detect_->Get();
+      superstructure_message->platform_right_detect =
+          !platform_right_detect_->Get();
+
       superstructure_message.Send();
     }
 
@@ -277,6 +292,9 @@
   ::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
       wrist_encoder_, stilts_encoder_;
 
+  ::std::unique_ptr<frc::DigitalInput> platform_left_detect_;
+  ::std::unique_ptr<frc::DigitalInput> platform_right_detect_;
+
   ::std::unique_ptr<frc::AnalogInput> vacuum_sensor_;
 
   ::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
@@ -668,6 +686,9 @@
     reader.set_pwm_trigger(true);
     reader.set_vacuum_sensor(7);
 
+    reader.set_platform_right_detect(make_unique<frc::DigitalInput>(6));
+    reader.set_platform_left_detect(make_unique<frc::DigitalInput>(7));
+
     reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(22));
     reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(23));