commit 2544ad84fc0ea69e94e1a7947b202adbbbf40b2c
author Ravago Jones <ravagojones@gmail.com> Sat Mar 04 22:24:49 2023 -0800
committer Ravago Jones <ravagojones@gmail.com> Sat Mar 04 22:24:49 2023 -0800
tree 1dc1fd92d7f15a9d10f2831607d448763f5ee8c1
parent 20cee015f17946638686213782d28252eb5b9e1f

Remove the heading pwm from wpilib_interface

Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I950148810529ec8b4d930ed22bb502dc344a6010

y2023/wpilib_interface.cc

diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 12a1779..921531e 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -425,7 +425,6 @@
   }
 
   void Start() override {
-    AddToDMA(&imu_heading_reader_);
     AddToDMA(&imu_yaw_rate_reader_);
     AddToDMA(&cone_position_sensor_);
   }
@@ -435,11 +434,6 @@
     autonomous_modes_.at(i) = ::std::move(sensor);
   }
 
-  void set_heading_input(::std::unique_ptr<frc::DigitalInput> sensor) {
-    imu_heading_input_ = ::std::move(sensor);
-    imu_heading_reader_.set_input(imu_heading_input_.get());
-  }
-
   void set_yaw_rate_input(::std::unique_ptr<frc::DigitalInput> sensor) {
     imu_yaw_rate_input_ = ::std::move(sensor);
     imu_yaw_rate_reader_.set_input(imu_yaw_rate_input_.get());
@@ -536,23 +530,15 @@
       constexpr double kScaledRangeHigh = 0.9;
 
       constexpr double kPWMFrequencyHz = 200;
-      double heading_duty_cycle =
-          imu_heading_reader_.last_width() * kPWMFrequencyHz;
       double velocity_duty_cycle =
           imu_yaw_rate_reader_.last_width() * kPWMFrequencyHz;
 
       constexpr double kDutyCycleScale =
           1 / (kScaledRangeHigh - kScaledRangeLow);
       // scale from 0.1 - 0.9 to 0 - 1
-      double rescaled_heading_duty_cycle =
-          (heading_duty_cycle - kScaledRangeLow) * kDutyCycleScale;
       double rescaled_velocity_duty_cycle =
           (velocity_duty_cycle - kScaledRangeLow) * kDutyCycleScale;
 
-      if (!std::isnan(rescaled_heading_duty_cycle)) {
-        gyro_reading_builder.add_angle(rescaled_heading_duty_cycle *
-                                       (2.0 * M_PI));
-      }
       if (!std::isnan(rescaled_velocity_duty_cycle)) {
         gyro_reading_builder.add_velocity((rescaled_velocity_duty_cycle - 0.5) *
                                           kVelocityRadiansPerSecond);
@@ -662,10 +648,10 @@
 
   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_yaw_rate_input_,
       end_effector_cube_beam_break_;
 
-  frc971::wpilib::DMAPulseWidthReader imu_heading_reader_, imu_yaw_rate_reader_;
+  frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
 
   frc971::wpilib::AbsoluteEncoderAndPotentiometer proximal_encoder_,
       distal_encoder_, roll_joint_encoder_;
@@ -976,7 +962,6 @@
     sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
     sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
     sensor_reader.set_superstructure_reading(superstructure_reading);
-    sensor_reader.set_heading_input(make_unique<frc::DigitalInput>(1));
     sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(0));
 
     sensor_reader.set_proximal_encoder(make_encoder(3));