commit 749f24427dcafc29ac89e8ef0ae228273ee866b3
author Filip Kujawa <filip.j.kujawa@gmail.com> Sun Feb 04 01:12:35 2024 -0800
committer Filip Kujawa <filip.j.kujawa@gmail.com> Sat Feb 10 11:44:01 2024 -0800
tree d736c2c560190a0595a3a690670f6722cad6abbe
parent 6d71763946ce9527381bb4a917d0e88235fdb343

Add climber wpilib interface

Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: I81a101cb000375b9753b170467bd21bb17c16482

y2024/wpilib_interface.cc

diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index 8130795..4a6c00b 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -81,6 +81,10 @@
   return voltage * Values::kIntakePivotPotRadiansPerVolt();
 }
 
+double climber_pot_translate(double voltage) {
+  return voltage * Values::kClimberPotRadiansPerVolt();
+}
+
 double drivetrain_velocity_translate(double in) {
   return (((1.0 / in) / Values::kDrivetrainCyclesPerRevolution()) *
           (2.0 * M_PI)) *
@@ -91,6 +95,7 @@
 constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
     Values::kMaxDrivetrainEncoderPulsesPerSecond(),
     Values::kMaxIntakePivotEncoderPulsesPerSecond(),
+    Values::kMaxClimberEncoderPulsesPerSecond(),
 });
 static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
               "fast encoders are too fast");
@@ -141,6 +146,13 @@
                        ->intake_constants()
                        ->potentiometer_offset());
 
+      CopyPosition(climber_encoder_, builder->add_climber(),
+                   Values::kClimberEncoderCountsPerRevolution(),
+                   Values::kClimberEncoderRatio(), climber_pot_translate, true,
+                   robot_constants_->robot()
+                       ->climber_constants()
+                       ->potentiometer_offset());
+
       builder->set_transfer_beambreak(transfer_beam_break_->Get());
       builder.CheckOk(builder.Send());
     }
@@ -212,6 +224,15 @@
     transfer_beam_break_ = ::std::move(sensor);
   }
 
+  void set_climber(::std::unique_ptr<frc::Encoder> encoder,
+                   ::std::unique_ptr<frc::DigitalInput> absolute_pwm,
+                   ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+    fast_encoder_filter_.Add(encoder.get());
+    climber_encoder_.set_encoder(::std::move(encoder));
+    climber_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+    climber_encoder_.set_potentiometer(::std::move(potentiometer));
+  }
+
  private:
   const Constants *robot_constants_;
 
@@ -226,6 +247,7 @@
   std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_, transfer_beam_break_;
 
   frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_pivot_encoder_;
+  frc971::wpilib::AbsoluteEncoderAndPotentiometer climber_encoder_;
 
   frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
 };
@@ -272,6 +294,10 @@
                                    make_unique<frc::AnalogInput>(4));
     sensor_reader.set_transfer_beambreak(make_unique<frc::DigitalInput>(7));
 
+    sensor_reader.set_climber(make_encoder(5),
+                              make_unique<frc::DigitalInput>(5),
+                              make_unique<frc::AnalogInput>(5));
+
     AddLoop(&sensor_reader_event_loop);
 
     // Thread 4.
@@ -315,6 +341,11 @@
         current_limits->transfer_roller_stator_current_limit(),
         current_limits->transfer_roller_supply_current_limit());
 
+    std::shared_ptr<TalonFX> climber = std::make_shared<TalonFX>(
+        7, false, "Drivetrain Bus", &signals_registry,
+        current_limits->climber_stator_current_limit(),
+        current_limits->climber_supply_current_limit());
+
     ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
         constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
     ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
@@ -332,7 +363,8 @@
       talonfxs.push_back(talonfx);
     }
 
-    for (auto talonfx : {intake_pivot, intake_roller, transfer_roller}) {
+    for (auto talonfx :
+         {intake_pivot, intake_roller, transfer_roller, climber}) {
       talonfxs.push_back(talonfx);
     }
 
@@ -351,7 +383,8 @@
     frc971::wpilib::CANSensorReader can_sensor_reader(
         &can_sensor_reader_event_loop, std::move(signals_registry), talonfxs,
         [drivetrain_talonfxs, &intake_pivot, &intake_roller, &transfer_roller,
-         &drivetrain_can_position_sender, &superstructure_can_position_sender](
+         &climber, &drivetrain_can_position_sender,
+         &superstructure_can_position_sender](
             ctre::phoenix::StatusCode status) {
           aos::Sender<frc971::control_loops::drivetrain::CANPositionStatic>::
               StaticBuilder drivetrain_can_builder =
@@ -387,6 +420,9 @@
           transfer_roller->SerializePosition(
               superstructure_can_builder->add_transfer_roller(),
               control_loops::drivetrain::kHighOutputRatio);
+          climber->SerializePosition(
+              superstructure_can_builder->add_climber(),
+              control_loops::drivetrain::kHighOutputRatio);
 
           superstructure_can_builder->set_timestamp(
               intake_roller->GetTimestamp());
@@ -415,6 +451,8 @@
                   ->second->WriteVoltage(output.intake_roller_voltage());
               talonfx_map.find("transfer_roller")
                   ->second->WriteVoltage(output.transfer_roller_voltage());
+              talonfx_map.find("climber")->second->WriteVoltage(
+                  output.climber_voltage());
             });
 
     can_drivetrain_writer.set_talonfxs({right_front, right_back},
@@ -423,6 +461,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_superstructure_writer.add_talonfx("climber", climber);
 
     can_output_event_loop.MakeWatcher(
         "/roborio", [&can_drivetrain_writer, &can_superstructure_writer](