commit 6c7e61f09114fad014ecc033914f673366d51eda
author Maxwell Henderson <mxwhenderson@gmail.com> Wed Feb 22 16:43:43 2023 -0800
committer Maxwell Henderson <mxwhenderson@gmail.com> Thu Feb 23 20:55:19 2023 -0800
tree 18d80aee85cf5294fe5e9382c9d5f98a8a79e06e
parent 589cf273abed419c88e405916990f745963ee39c

Add end effector beambreak to wpi

Change-Id: I6348a8d5cadffa7345a74c80455821261151c66b
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>

y2023/wpilib_interface.cc

diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 3ccb036..9ffb783 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -197,6 +197,10 @@
 
       position_builder.add_arm(arm_offset);
       position_builder.add_wrist(wrist_offset);
+      position_builder.add_end_effector_cone_beam_break(
+          end_effector_cone_beam_break_->Get());
+      position_builder.add_end_effector_cube_beam_break(
+          end_effector_cube_beam_break_->Get());
       builder.CheckOk(builder.Send(position_builder.Finish()));
     }
 
@@ -338,6 +342,15 @@
     wrist_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
   }
 
+  void set_end_effector_cone_beam_break(
+      ::std::unique_ptr<frc::DigitalInput> sensor) {
+    end_effector_cone_beam_break_ = ::std::move(sensor);
+  }
+  void set_end_effector_cube_beam_break(
+      ::std::unique_ptr<frc::DigitalInput> sensor) {
+    end_effector_cube_beam_break_ = ::std::move(sensor);
+  }
+
  private:
   std::shared_ptr<const Values> values_;
 
@@ -349,7 +362,8 @@
 
   std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
 
-  std::unique_ptr<frc::DigitalInput> imu_heading_input_, imu_yaw_rate_input_;
+  std::unique_ptr<frc::DigitalInput> imu_heading_input_, imu_yaw_rate_input_,
+      end_effector_cone_beam_break_, end_effector_cube_beam_break_;
 
   frc971::wpilib::DMAPulseWidthReader imu_heading_reader_, imu_yaw_rate_reader_;
 
@@ -817,6 +831,12 @@
     sensor_reader.set_wrist_encoder(make_encoder(4));
     sensor_reader.set_wrist_absolute_pwm(make_unique<frc::DigitalInput>(4));
 
+    // TODO(Max): Make the DigitalInput values the accurate robot values.
+    sensor_reader.set_end_effector_cone_beam_break(
+        make_unique<frc::DigitalInput>(6));
+    sensor_reader.set_end_effector_cube_beam_break(
+        make_unique<frc::DigitalInput>(7));
+
     AddLoop(&sensor_reader_event_loop);
 
     // Thread 4.