diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java index 833c905c65..a00a3243d2 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java @@ -8,7 +8,6 @@ package edu.wpi.first.wpilibj2.command; import java.util.function.BiConsumer; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; import edu.wpi.first.wpilibj.Timer; @@ -39,20 +38,18 @@ import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; @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; private final RamseteController m_follower; private final SimpleMotorFeedforward m_feedforward; private final DifferentialDriveKinematics m_kinematics; - private final DoubleSupplier m_leftSpeed; - private final DoubleSupplier m_rightSpeed; + private final Supplier m_speeds; private final PIDController m_leftController; private final PIDController m_rightController; private final BiConsumer m_output; + private DifferentialDriveWheelSpeeds m_prevSpeeds; + private double m_prevTime; /** * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory. @@ -63,21 +60,19 @@ public class RamseteCommand extends CommandBase { * this * is left to the user, since it is not appropriate for paths with nonstationary endstates. * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one of - * the odometry classes to provide this. - * @param controller The RAMSETE controller used to follow the trajectory. - * @param feedforward The feedforward to use for the drive. - * @param kinematics The kinematics for the robot drivetrain. - * @param leftWheelSpeedMetersPerSecond A function that supplies the speed of the left side of - * the robot drive. - * @param rightWheelSpeedMetersPerSecond A function that supplies the speed of the right side of - * the robot drive. - * @param leftController The PIDController for the left side of the robot drive. - * @param rightController The PIDController for the right side of the robot drive. - * @param outputVolts A function that consumes the computed left and right - * outputs (in volts) for the robot drive. - * @param requirements The subsystems to require. + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param controller The RAMSETE controller used to follow the trajectory. + * @param feedforward The feedforward to use for the drive. + * @param kinematics The kinematics for the robot drivetrain. + * @param wheelSpeeds A function that supplies the speeds of the left and + * right sides of the robot drive. + * @param leftController The PIDController for the left side of the robot drive. + * @param rightController The PIDController for the right side of the robot drive. + * @param outputVolts A function that consumes the computed left and right + * outputs (in volts) for the robot drive. + * @param requirements The subsystems to require. */ @SuppressWarnings("PMD.ExcessiveParameterList") public RamseteCommand(Trajectory trajectory, @@ -85,8 +80,7 @@ public class RamseteCommand extends CommandBase { RamseteController controller, SimpleMotorFeedforward feedforward, DifferentialDriveKinematics kinematics, - DoubleSupplier leftWheelSpeedMetersPerSecond, - DoubleSupplier rightWheelSpeedMetersPerSecond, + Supplier wheelSpeeds, PIDController leftController, PIDController rightController, BiConsumer outputVolts, @@ -96,12 +90,7 @@ public class RamseteCommand extends CommandBase { m_follower = requireNonNullParam(controller, "controller", "RamseteCommand"); m_feedforward = feedforward; m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand"); - m_leftSpeed = requireNonNullParam(leftWheelSpeedMetersPerSecond, - "leftWheelSpeedMetersPerSecond", - "RamseteCommand"); - m_rightSpeed = requireNonNullParam(rightWheelSpeedMetersPerSecond, - "rightWheelSpeedMetersPerSecond", - "RamseteCommand"); + m_speeds = requireNonNullParam(wheelSpeeds, "wheelSpeeds", "RamseteCommand"); m_leftController = requireNonNullParam(leftController, "leftController", "RamseteCommand"); m_rightController = requireNonNullParam(rightController, "rightController", "RamseteCommand"); m_output = requireNonNullParam(outputVolts, "outputVolts", "RamseteCommand"); @@ -138,8 +127,7 @@ public class RamseteCommand extends CommandBase { m_output = requireNonNullParam(outputMetersPerSecond, "output", "RamseteCommand"); m_feedforward = null; - m_leftSpeed = null; - m_rightSpeed = null; + m_speeds = null; m_leftController = null; m_rightController = null; @@ -154,9 +142,9 @@ public class RamseteCommand extends CommandBase { var initialState = m_trajectory.sample(0); m_prevSpeeds = m_kinematics.toWheelSpeeds( new ChassisSpeeds(initialState.velocityMetersPerSecond, - 0, - initialState.curvatureRadPerMeter - * initialState.velocityMetersPerSecond)); + 0, + initialState.curvatureRadPerMeter + * initialState.velocityMetersPerSecond)); m_timer.reset(); m_timer.start(); if (m_usePID) { @@ -182,19 +170,19 @@ public class RamseteCommand extends CommandBase { if (m_usePID) { double leftFeedforward = m_feedforward.calculate(leftSpeedSetpoint, - (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt); + (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt); double rightFeedforward = m_feedforward.calculate(rightSpeedSetpoint, - (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt); + (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt); leftOutput = leftFeedforward - + m_leftController.calculate(m_leftSpeed.getAsDouble(), - leftSpeedSetpoint); + + m_leftController.calculate(m_speeds.get().leftMetersPerSecond, + leftSpeedSetpoint); rightOutput = rightFeedforward - + m_rightController.calculate(m_rightSpeed.getAsDouble(), - rightSpeedSetpoint); + + m_rightController.calculate(m_speeds.get().rightMetersPerSecond, + rightSpeedSetpoint); } else { leftOutput = leftSpeedSetpoint; rightOutput = rightSpeedSetpoint; diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp index c0ed8b7504..54a4cc33b1 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp @@ -15,8 +15,7 @@ RamseteCommand::RamseteCommand( frc::RamseteController controller, frc::SimpleMotorFeedforward feedforward, frc::DifferentialDriveKinematics kinematics, - std::function leftSpeed, - std::function rightSpeed, + std::function wheelSpeeds, frc2::PIDController leftController, frc2::PIDController rightController, std::function output, std::initializer_list requirements) @@ -25,8 +24,7 @@ RamseteCommand::RamseteCommand( m_controller(controller), m_feedforward(feedforward), m_kinematics(kinematics), - m_leftSpeed(leftSpeed), - m_rightSpeed(rightSpeed), + m_speeds(wheelSpeeds), m_leftController(std::make_unique(leftController)), m_rightController(std::make_unique(rightController)), m_outputVolts(output), @@ -82,11 +80,11 @@ void RamseteCommand::Execute() { auto leftOutput = volt_t(m_leftController->Calculate( - m_leftSpeed().to(), targetWheelSpeeds.left.to())) + + m_speeds().left.to(), targetWheelSpeeds.left.to())) + leftFeedforward; auto rightOutput = volt_t(m_rightController->Calculate( - m_rightSpeed().to(), + m_speeds().right.to(), targetWheelSpeeds.right.to())) + rightFeedforward; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h index 5d8477ff0d..e38c14ad1c 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h @@ -60,10 +60,8 @@ class RamseteCommand : public CommandHelper { * trajectory. * @param feedforward A component for calculating the feedforward for the drive. * @param kinematics The kinematics for the robot drivetrain. - * @param leftSpeed A function that supplies the speed of the left side - * of the robot drive. - * @param rightSpeed A function that supplies the speed of the right side - * of the robot drive. + * @param wheelSpeeds A function that supplies the speeds of the left + * and right sides of the robot drive. * @param leftController The PIDController for the left side of the robot * drive. * @param rightController The PIDController for the right side of the robot @@ -76,8 +74,7 @@ class RamseteCommand : public CommandHelper { frc::RamseteController controller, frc::SimpleMotorFeedforward feedforward, frc::DifferentialDriveKinematics kinematics, - std::function leftSpeed, - std::function rightSpeed, + std::function wheelSpeeds, frc2::PIDController leftController, frc2::PIDController rightController, std::function output, @@ -121,8 +118,7 @@ class RamseteCommand : public CommandHelper { frc::RamseteController m_controller; frc::SimpleMotorFeedforward m_feedforward; frc::DifferentialDriveKinematics m_kinematics; - std::function m_leftSpeed; - std::function m_rightSpeed; + std::function m_speeds; std::unique_ptr m_leftController; std::unique_ptr m_rightController; std::function m_outputVolts; diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp index 9342f324cf..bd03d5b04b 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp @@ -68,12 +68,7 @@ frc2::Command* RobotContainer::GetAutonomousCommand() { frc::SimpleMotorFeedforward( DriveConstants::ks, DriveConstants::kv, DriveConstants::ka), DriveConstants::kDriveKinematics, - [this] { - return units::meters_per_second_t(m_drive.GetLeftEncoder().GetRate()); - }, - [this] { - return units::meters_per_second_t(m_drive.GetRightEncoder().GetRate()); - }, + [this] { return m_drive.GetWheelSpeeds(); }, frc2::PIDController(DriveConstants::kPDriveVel, 0, 0), frc2::PIDController(DriveConstants::kPDriveVel, 0, 0), [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); }, diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp index faa9ec2b4a..70e93b7ec0 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp @@ -29,9 +29,7 @@ DriveSubsystem::DriveSubsystem() void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())), - frc::DifferentialDriveWheelSpeeds{ - units::meters_per_second_t(m_leftEncoder.GetRate()), - units::meters_per_second_t(m_rightEncoder.GetRate())}); + GetWheelSpeeds()); } void DriveSubsystem::ArcadeDrive(double fwd, double rot) { @@ -70,6 +68,11 @@ double DriveSubsystem::GetTurnRate() { frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } +frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() { + return {units::meters_per_second_t(m_leftEncoder.GetRate()), + units::meters_per_second_t(m_rightEncoder.GetRate())}; +} + void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { m_odometry.ResetPosition(pose, frc::Rotation2d(units::degree_t(GetHeading()))); diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h index 8eb5b8617b..d8d1abd8af 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h @@ -101,6 +101,13 @@ class DriveSubsystem : public frc2::SubsystemBase { */ frc::Pose2d GetPose(); + /** + * Returns the current wheel speeds of the robot. + * + * @return The current wheel speeds. + */ + frc::DifferentialDriveWheelSpeeds GetWheelSpeeds(); + /** * Resets the odometry to the specified pose. * diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java index 565ea539cc..1467f84360 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java @@ -118,8 +118,7 @@ public class RobotContainer { new RamseteController(kRamseteB, kRamseteZeta), new SimpleMotorFeedforward(ksVolts, kvVoltSecondsPerMeter, kaVoltSecondsSquaredPerMeter), kDriveKinematics, - m_robotDrive.getLeftEncoder()::getRate, - m_robotDrive.getRightEncoder()::getRate, + m_robotDrive::getWheelSpeeds, new PIDController(kPDriveVel, 0, 0), new PIDController(kPDriveVel, 0, 0), // RamseteCommand passes volts to the callback diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java index a9f272bd90..f121a840d1 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java @@ -72,11 +72,7 @@ public class DriveSubsystem extends SubsystemBase { @Override public void periodic() { // Update the odometry in the periodic block - m_odometry.update(Rotation2d.fromDegrees(getHeading()), - new DifferentialDriveWheelSpeeds( - m_leftEncoder.getRate(), - m_rightEncoder.getRate() - )); + m_odometry.update(Rotation2d.fromDegrees(getHeading()), getWheelSpeeds()); } /** @@ -88,6 +84,15 @@ public class DriveSubsystem extends SubsystemBase { return m_odometry.getPoseMeters(); } + /** + * Returns the current wheel speeds of the robot. + * + * @return The current wheel speeds. + */ + public DifferentialDriveWheelSpeeds getWheelSpeeds() { + return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()); + } + /** * Resets the odometry to the specified pose. *