diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index 4e5e6fe..4eec961 100644 --- 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("/SwerveStates") .Publish(); + m_desiredPublisher = + nt::NetworkTableInstance::GetDefault() + .GetStructArrayTopic("/SwerveDesiredStates") + .Publish(); } void DriveSubsystem::SimulationPeriodic() { @@ -190,8 +194,13 @@ void DriveSubsystem::logDrivebase() { m_rearLeft.GetState(), m_rearRight.GetState()}; std::span 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 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 index 32cb94c..1173d20 100644 --- 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 m_publisher; + nt::StructArrayPublisher m_desiredPublisher; // Pose viewing frc::Field2d m_field;