log desired states (#153)

SubZero-Robotics · Jun 15, 2024 · 3d1039f · 3d1039f
1 parent fb0bd1f
commit 3d1039f
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 0 deletions.
diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp
@@ -65,6 +65,10 @@ DriveSubsystem::DriveSubsystem(PhotonVisionEstimators* vision)
       nt::NetworkTableInstance::GetDefault()
           .GetStructArrayTopic<frc::SwerveModuleState>("/SwerveStates")
           .Publish();
+  m_desiredPublisher =
+      nt::NetworkTableInstance::GetDefault()
+          .GetStructArrayTopic<frc::SwerveModuleState>("/SwerveDesiredStates")
+          .Publish();
 }
 
 void DriveSubsystem::SimulationPeriodic() {
@@ -190,8 +194,13 @@ void DriveSubsystem::logDrivebase() {
                             m_rearLeft.GetState(), m_rearRight.GetState()};
   std::span<frc::SwerveModuleState, 4> states(states_vec.begin(),
                                               states_vec.end());
+  std::vector desired_vec = {m_frontLeft.GetDesiredState(), m_frontRight.GetDesiredState(),
+                            m_rearLeft.GetDesiredState(), m_rearRight.GetDesiredState()};
+  std::span<frc::SwerveModuleState, 4> desiredStates(desired_vec.begin(),
+                                              desired_vec.end());
 
   m_publisher.Set(states);
+  m_desiredPublisher.Set(desiredStates);
 }
 
 void DriveSubsystem::AddVisionMeasurement(const frc::Pose2d& visionMeasurement,

diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h
@@ -204,6 +204,7 @@ class DriveSubsystem : public frc2::SubsystemBase {
       VisionConstants::kDrivetrainStd,
       VisionConstants::kVisionStd};
   nt::StructArrayPublisher<frc::SwerveModuleState> m_publisher;
+  nt::StructArrayPublisher<frc::SwerveModuleState> m_desiredPublisher;
 
   // Pose viewing
   frc::Field2d m_field;