diff --git a/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp index 0de739e7a0..7be6655bf7 100644 --- a/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp +++ b/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp @@ -37,7 +37,8 @@ RamseteCommand::RamseteCommand( m_rightSpeed(rightSpeed), m_leftController(std::make_unique(leftController)), m_rightController(std::make_unique(rightController)), - m_outputVolts(output) { + m_outputVolts(output), + m_usePID(true) { AddRequirements(requirements); } @@ -55,7 +56,8 @@ RamseteCommand::RamseteCommand( m_kv(0), m_ka(0), m_kinematics(kinematics), - m_outputVel(output) { + m_outputVel(output), + m_usePID(false) { AddRequirements(requirements); } @@ -67,8 +69,10 @@ void RamseteCommand::Initialize() { initialState.velocity * initialState.curvature}); m_timer.Reset(); m_timer.Start(); - m_leftController->Reset(); - m_rightController->Reset(); + if (m_usePID) { + m_leftController->Reset(); + m_rightController->Reset(); + } } void RamseteCommand::Execute() { @@ -78,7 +82,7 @@ void RamseteCommand::Execute() { auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds( m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime))); - if (m_leftController.get() != nullptr) { + if (m_usePID) { auto leftFeedforward = m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left + m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt; diff --git a/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h index 592f194711..82f7835543 100644 --- a/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h +++ b/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h @@ -143,5 +143,6 @@ class RamseteCommand : public CommandHelper { Timer m_timer; units::second_t m_prevTime; frc::DifferentialDriveWheelSpeeds m_prevSpeeds; + bool m_usePID; }; } // namespace frc2 diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java index 754660c058..e59e81aa0a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java @@ -35,10 +35,12 @@ import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; * the PID and feedforward functionality, returning only the raw wheel speeds from the RAMSETE * controller. */ +@SuppressWarnings("PMD.TooManyFields") public class RamseteCommand extends CommandBase { private final Timer m_timer = new Timer(); private DifferentialDriveWheelSpeeds m_prevSpeeds; private double m_prevTime; + private final boolean m_usePID; private final Trajectory m_trajectory; private final Supplier m_pose; @@ -113,6 +115,8 @@ public class RamseteCommand extends CommandBase { m_rightController = requireNonNullParam(rightController, "rightController", "RamseteCommand"); m_output = requireNonNullParam(outputVolts, "outputVolts", "RamseteCommand"); + m_usePID = true; + addRequirements(requirements); } @@ -150,6 +154,8 @@ public class RamseteCommand extends CommandBase { m_leftController = null; m_rightController = null; + m_usePID = false; + addRequirements(requirements); } @@ -164,8 +170,10 @@ public class RamseteCommand extends CommandBase { * initialState.velocityMetersPerSecond)); m_timer.reset(); m_timer.start(); - m_leftController.reset(); - m_rightController.reset(); + if (m_usePID) { + m_leftController.reset(); + m_rightController.reset(); + } } @Override @@ -182,7 +190,7 @@ public class RamseteCommand extends CommandBase { double leftOutput; double rightOutput; - if (m_leftController != null) { + if (m_usePID) { double leftFeedforward = m_ks * Math.signum(leftSpeedSetpoint) + m_kv * leftSpeedSetpoint