From 79259b0fb6e15aefba84ecf6a3d28dc6f6a8183d Mon Sep 17 00:00:00 2001 From: Dave Korhumel Date: Mon, 27 Mar 2023 23:01:23 -0700 Subject: [PATCH 1/2] create AcceleratedChassisSpeeds --- src/main/java/frc/robot/Constants.java | 5 +- .../frc/robot/subsystems/DriveSubsystem.java | 49 ++++++------------- .../robot/util/AcceleratedChassisSpeeds.java | 49 +++++++++++++++++++ 3 files changed, 69 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc/robot/util/AcceleratedChassisSpeeds.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5387880..c3a7670 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -109,7 +109,10 @@ public static final class DriveConstants { new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); public static final double kMaxSpeedMetersPerSecond = 3.6576; - public static final double kMaxAngularSpeedRadiansPerSecond = 15.24/3; + public static final double kMaxAngularSpeedRadiansPerSecond = 15.24 / 3; + + public static final double kMaxAccelerationMetersPerSecondSquared = kMaxSpeedMetersPerSecond * 4; + public static final double kMaxAngularAccelerationRadiansPerSecondSquared = kMaxAngularSpeedRadiansPerSecond * 4; public static final double kTurningStopTime = 0.2; // TODO: tune heading correction stop time public static final double kSpeedIncreasePerPeriod = 0.15; diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index c69035b..c44493d 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.DriveConstants; +import frc.robot.util.AcceleratedChassisSpeeds; import frc.robot.Robot; public class DriveSubsystem extends SubsystemBase { @@ -78,8 +79,9 @@ public class DriveSubsystem extends SubsystemBase { private final Field2d m_field = new Field2d(); - private double m_currentXSpeed = 0; - private double m_currentYSpeed = 0; + private AcceleratedChassisSpeeds m_acceleratedChassisSpeeds = new AcceleratedChassisSpeeds( + DriveConstants.kMaxAccelerationMetersPerSecondSquared, + DriveConstants.kMaxAngularAccelerationRadiansPerSecondSquared); /** Creates a new {@link DriveSubsystem}. */ public DriveSubsystem() { @@ -170,30 +172,29 @@ public void resetOdometry(Pose2d pose) { * @param fieldRelative Whether the provided x and y speeds are relative to the * field. */ - public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - if (rot != 0) { + public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) { + if (rotation != 0) { m_headingCorrectionTimer.reset(); } - double rotation = rot; + Rotation2d gyroRotation = m_gyro.getRotation2d(); - double currentAngle = MathUtil.angleModulus(m_gyro.getRotation2d().getRadians()); - - m_currentXSpeed = calculateAcceleration(m_currentXSpeed, xSpeed); - m_currentYSpeed = calculateAcceleration(m_currentYSpeed, ySpeed); - - if ((m_currentXSpeed == 0 && m_currentYSpeed == 0) + ChassisSpeeds currentSpeeds = m_acceleratedChassisSpeeds.getCurrentSpeeds(); + double currentAngle = MathUtil.angleModulus(gyroRotation.getRadians()); + if ((currentSpeeds.vxMetersPerSecond == 0 && currentSpeeds.vyMetersPerSecond == 0) || m_headingCorrectionTimer.get() < DriveConstants.kTurningStopTime) { m_headingCorrectionPID.setSetpoint(currentAngle); } else { rotation = m_headingCorrectionPID.calculate(currentAngle); } + ChassisSpeeds desiredSpeeds = fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotation, + Robot.isReal() ? gyroRotation : new Rotation2d(m_gyroAngle)) + : new ChassisSpeeds(xSpeed, ySpeed, rotation); + var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(m_currentXSpeed, m_currentYSpeed, rotation, - Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle)) - : new ChassisSpeeds(m_currentXSpeed, m_currentYSpeed, rotation)); + m_acceleratedChassisSpeeds.calculateNewSpeeds(desiredSpeeds)); setModuleStates(swerveModuleStates); } @@ -230,22 +231,4 @@ public void zeroHeading() { public double getGyroPitch() { return m_gyro.getPitch(); } - - /** - * Slowly accelerates the bot to the desired speed. - * - * @param currentSpeed The current speed. - * @param desiredSpeed The desired speed. - * - * @return The new speed to use. - */ - private double calculateAcceleration(double currentSpeed, double desiredSpeed) { - if (Math.abs(currentSpeed - desiredSpeed) < DriveConstants.kSpeedIncreasePerPeriod) { - return desiredSpeed; - } else if (currentSpeed > desiredSpeed) { - return currentSpeed - DriveConstants.kSpeedIncreasePerPeriod; - } else { - return currentSpeed + DriveConstants.kSpeedIncreasePerPeriod; - } - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/AcceleratedChassisSpeeds.java b/src/main/java/frc/robot/util/AcceleratedChassisSpeeds.java new file mode 100644 index 0000000..6188f35 --- /dev/null +++ b/src/main/java/frc/robot/util/AcceleratedChassisSpeeds.java @@ -0,0 +1,49 @@ +package frc.robot.util; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** + * Calculates acceleration for ChassisSpeeds using given acceleration constants. + * This object maintains state and should be reused. + */ +public class AcceleratedChassisSpeeds { + private SlewRateLimiter m_xTranslationRateLimiter; + private SlewRateLimiter m_yTranslationRateLimiter; + private SlewRateLimiter m_rotationRateLimiter; + private ChassisSpeeds m_currentChassisSpeeds; + + /** + * Create an accelerated ChassisSpeeds + * @param translationMaxAcceleration The max acceleration for translation, in m/s^2 + * @param rotationMaxAcceleration The max acceleration for rotation, in radians/s^2 + */ + public AcceleratedChassisSpeeds(double translationMaxAcceleration, double rotationMaxAcceleration) { + m_xTranslationRateLimiter = new SlewRateLimiter(translationMaxAcceleration); + m_yTranslationRateLimiter = new SlewRateLimiter(translationMaxAcceleration); + m_rotationRateLimiter = new SlewRateLimiter(rotationMaxAcceleration); + m_currentChassisSpeeds = new ChassisSpeeds(); + } + + /** + * Retrieves the current ChassisSpeeds + * @return The current speeds + */ + public ChassisSpeeds getCurrentSpeeds() { + return m_currentChassisSpeeds; + } + + /** + * Calculates and saves the new ChassisSpeeds, given the acceleration + * constants and the provided desired speeds. + * @param desiredSpeeds The desired speeds + * @return The new calculated speeds + */ + public ChassisSpeeds calculateNewSpeeds(ChassisSpeeds desiredSpeeds) { + m_currentChassisSpeeds = new ChassisSpeeds( + m_xTranslationRateLimiter.calculate(desiredSpeeds.vxMetersPerSecond), + m_yTranslationRateLimiter.calculate(desiredSpeeds.vyMetersPerSecond), + m_rotationRateLimiter.calculate(desiredSpeeds.omegaRadiansPerSecond)); + return m_currentChassisSpeeds; + } +} From 480bb08c5f1a782c12c11dc4ccfede2affbd0fbd Mon Sep 17 00:00:00 2001 From: Dave Korhumel Date: Tue, 28 Mar 2023 00:21:00 -0700 Subject: [PATCH 2/2] ensure the robot is not moving on init --- src/main/java/frc/robot/Robot.java | 4 ++++ src/main/java/frc/robot/RobotContainer.java | 7 +++++++ src/main/java/frc/robot/subsystems/DriveSubsystem.java | 9 +++++++++ .../java/frc/robot/util/AcceleratedChassisSpeeds.java | 10 ++++++++++ 4 files changed, 30 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 258da4b..fa527b6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -37,6 +37,7 @@ public void robotPeriodic() { @Override public void disabledInit() { + m_robotContainer.ensureRobotStopped(); } @Override @@ -51,6 +52,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { + m_robotContainer.ensureRobotStopped(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { @@ -64,6 +66,7 @@ public void autonomousPeriodic() { @Override public void teleopInit() { + m_robotContainer.ensureRobotStopped(); if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } @@ -75,6 +78,7 @@ public void teleopPeriodic() { @Override public void testInit() { + m_robotContainer.ensureRobotStopped(); CommandScheduler.getInstance().cancelAll(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 887583e..8555a55 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -190,4 +190,11 @@ public Command getAutonomousCommand() { AutonConstants.maxVelocity - (path.equals("Charger-comms") ? 0.75 : 0), AutonConstants.maxAcceleration))); } + + /** + * Ensure the robot is stopped + */ + public void ensureRobotStopped() { + m_robotDrive.stop(); + } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index c44493d..92589b2 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -231,4 +231,13 @@ public void zeroHeading() { public double getGyroPitch() { return m_gyro.getPitch(); } + + /** + * Stops the robot from moving + */ + public void stop() { + m_acceleratedChassisSpeeds.reset(); + setModuleStates( + DriveConstants.kDriveKinematics.toSwerveModuleStates(m_acceleratedChassisSpeeds.getCurrentSpeeds())); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/AcceleratedChassisSpeeds.java b/src/main/java/frc/robot/util/AcceleratedChassisSpeeds.java index 6188f35..0c56dfe 100644 --- a/src/main/java/frc/robot/util/AcceleratedChassisSpeeds.java +++ b/src/main/java/frc/robot/util/AcceleratedChassisSpeeds.java @@ -46,4 +46,14 @@ public ChassisSpeeds calculateNewSpeeds(ChassisSpeeds desiredSpeeds) { m_rotationRateLimiter.calculate(desiredSpeeds.omegaRadiansPerSecond)); return m_currentChassisSpeeds; } + + /** + * Resets the speeds to 0 + */ + public void reset() { + m_xTranslationRateLimiter.reset(0); + m_yTranslationRateLimiter.reset(0); + m_rotationRateLimiter.reset(0); + m_currentChassisSpeeds = new ChassisSpeeds(); + } }