commit 0e86e249deb3421b5fb5dc939f9a4f27405df0c5
author Ravago Jones <ravagojones@gmail.com> Sat Feb 12 18:38:14 2022 -0800
committer Ravago Jones <ravagojones@gmail.com> Sun Feb 27 14:10:37 2022 -0800
tree 6cb124198af48017565cd9ca2eb8977ab9fc8fc6
parent 3dbf1c90692a9aaa38b7f42e73161590d7f249e7

Read heading and velocity pwm in wpilib_interface

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

y2022/wpilib_interface.cc

diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index b95a4ec..eb70b6f 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -36,6 +36,7 @@
 #include "frc971/autonomous/auto_mode_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
 #include "frc971/input/robot_state_generated.h"
+#include "frc971/queues/gyro_generated.h"
 #include "frc971/wpilib/ADIS16448.h"
 #include "frc971/wpilib/buffered_pcm.h"
 #include "frc971/wpilib/buffered_solenoid.h"
@@ -131,7 +132,9 @@
         drivetrain_position_sender_(
             event_loop
                 ->MakeSender<::frc971::control_loops::drivetrain::Position>(
-                    "/drivetrain")) {
+                    "/drivetrain")),
+        gyro_sender_(event_loop->MakeSender<::frc971::sensors::GyroReading>(
+            "/drivetrain")) {
     // Set to filter out anything shorter than 1/4 of the minimum pulse width
     // we should ever see.
     UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -158,6 +161,14 @@
     catapult_encoder_.set_potentiometer(::std::move(potentiometer));
   }
 
+  void set_heading_input(::std::unique_ptr<frc::DigitalInput> sensor) {
+    imu_heading_reader_.set_input(::std::move(sensor));
+  }
+
+  void set_yaw_rate_input(::std::unique_ptr<frc::DigitalInput> sensor) {
+    imu_yaw_rate_reader_.set_input(::std::move(sensor));
+  }
+
   void RunIteration() override {
     {
       auto builder = superstructure_position_sender_.MakeBuilder();
@@ -250,6 +261,38 @@
     }
 
     {
+      auto builder = gyro_sender_.MakeBuilder();
+      ::frc971::sensors::GyroReading::Builder gyro_reading_builder =
+          builder.MakeBuilder<::frc971::sensors::GyroReading>();
+      constexpr double kMaxVelocity = 2000;  // degrees / second
+      constexpr double kVelocityRadiansPerSecond =
+          kMaxVelocity / 360 * (2.0 * M_PI);
+
+      // Only part of the full range is used to prevent being 100% on or off.
+      constexpr double kScaledRangeLow = 0.1;
+      constexpr double kScaledRangeHigh = 0.9;
+
+      constexpr double kDutyCycleScale =
+          1 / (kScaledRangeHigh - kScaledRangeLow);
+
+      // scale from 0.1 - 0.9 to 0 - 1
+      double rescaled_heading_duty_cycle =
+          (imu_heading_reader_.Read() - kScaledRangeLow) * kDutyCycleScale;
+      double rescaled_velocity_duty_cycle =
+          (imu_yaw_rate_reader_.Read() - 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);
+      }
+      builder.CheckOk(builder.Send(gyro_reading_builder.Finish()));
+    }
+
+    {
       auto builder = auto_mode_sender_.MakeBuilder();
 
       uint32_t mode = 0;
@@ -345,6 +388,7 @@
   aos::Sender<superstructure::Position> superstructure_position_sender_;
   aos::Sender<frc971::control_loops::drivetrain::Position>
       drivetrain_position_sender_;
+  ::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
 
   std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
 
@@ -354,9 +398,9 @@
   std::unique_ptr<frc::AnalogInput> climber_potentiometer_,
       flipper_arm_right_potentiometer_, flipper_arm_left_potentiometer_;
   frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_encoder_front_,
-      intake_encoder_back_, turret_encoder_;
+      intake_encoder_back_, turret_encoder_, catapult_encoder_;
 
-  frc971::wpilib::AbsoluteEncoderAndPotentiometer catapult_encoder_;
+  frc971::wpilib::DutyCycleReader imu_heading_reader_, imu_yaw_rate_reader_;
 };
 
 class SuperstructureWriter
@@ -602,6 +646,9 @@
     sensor_reader.set_catapult_potentiometer(
         std::make_unique<frc::AnalogInput>(2));
 
+    sensor_reader.set_heading_input(make_unique<frc::DigitalInput>(8));
+    sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(9));
+
     AddLoop(&sensor_reader_event_loop);
 
     // Thread 4.