commit ed7ffc43e6313a69fa3234d21cf9a638e1d5465a
author Niko Sohmers <nikolai@sohmers.com> Sat Feb 03 16:05:19 2024 -0800
committer Niko Sohmers <nikolai@sohmers.com> Sat Feb 03 20:01:44 2024 -0800
tree 4a7973d965e279d826b50f1c45d115f0d123521f
parent e735fa82c52b1d2333468e1c4b0b8e47dee72f60

Add Transfer Roller Wpilib

Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: I1ebd58e51c3d0aaf7113cda06975b39701b5d136

y2024/wpilib_interface.cc

diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index 2217513..b708266 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -140,6 +140,7 @@
                        ->intake_constants()
                        ->potentiometer_offset());
 
+      builder->set_transfer_beambreak(transfer_beam_break_->Get());
       builder.CheckOk(builder.Send());
     }
 
@@ -212,6 +213,10 @@
     intake_pivot_encoder_.set_potentiometer(::std::move(potentiometer));
   }
 
+  void set_transfer_beambreak(::std::unique_ptr<frc::DigitalInput> sensor) {
+    transfer_beam_break_ = ::std::move(sensor);
+  }
+
  private:
   const Constants *robot_constants_;
 
@@ -223,7 +228,7 @@
 
   std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
 
-  std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_;
+  std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_, transfer_beam_break_;
 
   frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_pivot_encoder_;
 
@@ -272,6 +277,7 @@
         make_unique<frc::DigitalInput>(4));
     sensor_reader.set_intake_pivot_potentiometer(
         make_unique<frc::AnalogInput>(4));
+    sensor_reader.set_transfer_beambreak(make_unique<frc::DigitalInput>(7));
 
     AddLoop(&sensor_reader_event_loop);
 
@@ -316,7 +322,14 @@
                                   robot_constants->common()
                                       ->current_limits()
                                       ->intake_roller_supply_current_limit());
-
+    std::shared_ptr<TalonFX> transfer_roller =
+        std::make_shared<TalonFX>(6, false, "Drivetrain Bus", &signals_registry,
+                                  robot_constants->common()
+                                      ->current_limits()
+                                      ->transfer_roller_stator_current_limit(),
+                                  robot_constants->common()
+                                      ->current_limits()
+                                      ->transfer_roller_supply_current_limit());
     ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
         constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
     ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
@@ -334,7 +347,7 @@
       talonfxs.push_back(talonfx);
     }
 
-    for (auto talonfx : {intake_pivot, intake_roller}) {
+    for (auto talonfx : {intake_pivot, intake_roller, transfer_roller}) {
       talonfxs.push_back(talonfx);
     }
 
@@ -352,7 +365,7 @@
 
     frc971::wpilib::CANSensorReader can_sensor_reader(
         &can_sensor_reader_event_loop, std::move(signals_registry), talonfxs,
-        [drivetrain_talonfxs, &intake_pivot, &intake_roller,
+        [drivetrain_talonfxs, &intake_pivot, &intake_roller, &transfer_roller,
          &drivetrain_can_position_sender, &superstructure_can_position_sender](
             ctre::phoenix::StatusCode status) {
           aos::Sender<frc971::control_loops::drivetrain::CANPositionStatic>::
@@ -384,6 +397,9 @@
           intake_pivot->SerializePosition(
               superstructure_can_builder->add_intake_pivot(),
               control_loops::drivetrain::kHighOutputRatio);
+          transfer_roller->SerializePosition(
+              superstructure_can_builder->add_transfer_roller(),
+              control_loops::drivetrain::kHighOutputRatio);
 
           superstructure_can_builder->set_timestamp(
               intake_roller->GetTimestamp());
@@ -410,6 +426,8 @@
                   ->second->WriteVoltage(output.intake_pivot_voltage());
               talonfx_map.find("intake_roller")
                   ->second->WriteVoltage(output.intake_roller_voltage());
+              talonfx_map.find("transfer_roller")
+                  ->second->WriteVoltage(output.transfer_roller_voltage());
             });
 
     can_drivetrain_writer.set_talonfxs({right_front, right_back},
@@ -417,6 +435,7 @@
 
     can_superstructure_writer.add_talonfx("intake_pivot", intake_pivot);
     can_superstructure_writer.add_talonfx("intake_roller", intake_roller);
+    can_superstructure_writer.add_talonfx("transfer_roller", transfer_roller);
 
     can_output_event_loop.MakeWatcher(
         "/roborio", [&can_drivetrain_writer, &can_superstructure_writer](