From 513e7aafbf20aa64fc36caf6af0e099034239e3a Mon Sep 17 00:00:00 2001 From: WispySparks Date: Fri, 22 Nov 2024 15:05:26 -0600 Subject: [PATCH] rearrange constants, unused constants, docs --- src/main/java/frc/robot/Constants.java | 16 +- .../frc/robot/DynamicSlewRateLimiter.java | 147 +++++++++--------- src/main/java/frc/robot/Field.java | 1 + src/main/java/frc/robot/Main.java | 2 + src/main/java/frc/robot/Robot.java | 8 +- .../frc/robot/commands/AbsoluteDriveAdv.java | 1 + .../java/frc/robot/subsystems/Swerve.java | 9 +- 7 files changed, 89 insertions(+), 95 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c368928..9d23c4a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,11 +14,8 @@ import edu.wpi.first.wpilibj.Filesystem; public class Constants { - public static class SwerveK { - public static final int driveMotorID = 0; - public static final int turnMotorID = 1; - public static final int encoderID = 2; + public static class SwerveK { public static final Measure wheelDiameter = Inches.of(3); public static final Measure driveBaseRadius = Meters.of(0.4579874); @@ -35,13 +32,14 @@ public static class SwerveK { public static class ControllerK { public static final int xboxPort = 0; - public static final DynamicSlewRateLimiter translationalYLimiter = new DynamicSlewRateLimiter(0.5, 2); // Larger number = faster rate of change - public static final DynamicSlewRateLimiter translationalXLimiter = new DynamicSlewRateLimiter(0.5, 2); - public static final DynamicSlewRateLimiter rotationalLimiter = new DynamicSlewRateLimiter(0.5, 2); + public static final double leftJoystickDeadband = 0.05; + public static final double rightJoystickDeadband = 0.05; } public static class DriveK { - public static final double leftJoystickDeadband = 0.05; - public static final double rightJoystickDeadband = 0.05; + public static final DynamicSlewRateLimiter translationalYLimiter = new DynamicSlewRateLimiter(0.5, 2); // Larger number = faster rate of change + public static final DynamicSlewRateLimiter translationalXLimiter = new DynamicSlewRateLimiter(0.5, 2); + public static final DynamicSlewRateLimiter rotationalLimiter = new DynamicSlewRateLimiter(0.5, 2); } + } diff --git a/src/main/java/frc/robot/DynamicSlewRateLimiter.java b/src/main/java/frc/robot/DynamicSlewRateLimiter.java index c1df966..846abb1 100644 --- a/src/main/java/frc/robot/DynamicSlewRateLimiter.java +++ b/src/main/java/frc/robot/DynamicSlewRateLimiter.java @@ -2,85 +2,80 @@ import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** - * A class that limits the rate of change of an input value. Useful for implementing voltage, - * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being - * controlled is a velocity or a voltage; when controlling a position, consider using a {@link - * edu.wpi.first.math.trajectory.TrapezoidProfile} instead. - */ +* A class that limits the rate of change of an input value. Can have different rate limits for +* getting farther from zero (increasing) and getting closer to zero (decreasing). Useful for +* controlling something like robot acceleration and deceleration. +*/ public class DynamicSlewRateLimiter { - private final double increasingRateLimit; - private final double decreasingRateLimit; - private double prevVal; - private double prevTime; - - /** - * Creates a new DynamicSlewRateLimiter with the given increasing and decreasing rate limits. - * - * The rate limits are only magnitudes. - * @param increasingRateLimit The rate-of-change limit when the input is increasing, in units per - * second. This is expected to be positive. How quickly the input can accelerate. - * @param decreasingRateLimit The rate-of-change limit when the input is decreasing, in units per - * second. This is expected to be positive. How quickly the input can decelerate. - */ - public DynamicSlewRateLimiter(double increasingRateLimit, double decreasingRateLimit) { - if (increasingRateLimit < 0 || decreasingRateLimit < 0) { - throw new IllegalArgumentException("Rate limits can't be negative!"); + private final double increasingRateLimit; + private final double decreasingRateLimit; + private double prevVal; + private double prevTime; + + /** + * Creates a new DynamicSlewRateLimiter with the given increasing and decreasing rate limits. + * Increasing is how fast the input can get farther from zero, Decreasing is how fast the input can get closer to zero. + * The rate limits are only magnitudes. + * @param increasingRateLimit The rate-of-change limit when the input is increasing, in units per + * second. This is expected to be positive. How quickly the input can get farther from zero. + * @param decreasingRateLimit The rate-of-change limit when the input is decreasing, in units per + * second. This is expected to be positive. How quickly the input can get closer to zero. + */ + public DynamicSlewRateLimiter(double increasingRateLimit, double decreasingRateLimit) { + if (increasingRateLimit < 0 || decreasingRateLimit < 0) { + throw new IllegalArgumentException("Rate limits can't be negative!"); + } + this.increasingRateLimit = increasingRateLimit; + this.decreasingRateLimit = decreasingRateLimit; + prevVal = 0; + prevTime = MathSharedStore.getTimestamp(); } - this.increasingRateLimit = increasingRateLimit; - this.decreasingRateLimit = decreasingRateLimit; - prevVal = 0; - prevTime = MathSharedStore.getTimestamp(); - } - - /** - * Filters the input to limit its slew rate. - * - * @param input The input value whose slew rate is to be limited. - * @return The filtered value, which will not change faster than the slew rate. - */ - public double calculate(double input) { - double currentTime = MathSharedStore.getTimestamp(); - double elapsedTime = currentTime - prevTime; - double sign = Math.signum(prevVal); - double positiveRateLimit = increasingRateLimit; - double negativeRateLimit = decreasingRateLimit; - // Flip the positive and negative limits so that decreasing still means towards zero and increasing still means away. - if (sign < 0) { - positiveRateLimit = decreasingRateLimit; - negativeRateLimit = increasingRateLimit; - } - prevVal += + + /** + * Filters the input to limit its slew rate. + * + * @param input The input value whose slew rate is to be limited. + * @return The filtered value, which will not change faster than the slew rate. + */ + public double calculate(double input) { + double currentTime = MathSharedStore.getTimestamp(); + double elapsedTime = currentTime - prevTime; + double sign = Math.signum(prevVal); + double positiveRateLimit = increasingRateLimit; + double negativeRateLimit = decreasingRateLimit; + // Flip the positive and negative limits so that decreasing still means towards zero and increasing still means away. + if (sign < 0) { + positiveRateLimit = decreasingRateLimit; + negativeRateLimit = increasingRateLimit; + } + prevVal += MathUtil.clamp( - input - prevVal, - -negativeRateLimit * elapsedTime, - positiveRateLimit * elapsedTime); - prevTime = currentTime; - SmartDashboard.putNumber("positiveRateLimit", positiveRateLimit); - SmartDashboard.putNumber("negativeRateLimit", negativeRateLimit); - SmartDashboard.putNumber("sign", sign); - return prevVal; - } - - /** - * Returns the value last calculated by the DynamicSlewRateLimiter. - * - * @return The last value. - */ - public double lastValue() { - return prevVal; - } - - /** - * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so. - * - * @param value The value to reset to. - */ - public void reset(double value) { - prevVal = value; - prevTime = MathSharedStore.getTimestamp(); - } - + input - prevVal, + -negativeRateLimit * elapsedTime, + positiveRateLimit * elapsedTime); + prevTime = currentTime; + return prevVal; + } + + /** + * Returns the value last calculated by the DynamicSlewRateLimiter. + * + * @return The last value. + */ + public double lastValue() { + return prevVal; + } + + /** + * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so. + * + * @param value The value to reset to. + */ + public void reset(double value) { + prevVal = value; + prevTime = MathSharedStore.getTimestamp(); + } + } diff --git a/src/main/java/frc/robot/Field.java b/src/main/java/frc/robot/Field.java index f5e0062..22d11dc 100644 --- a/src/main/java/frc/robot/Field.java +++ b/src/main/java/frc/robot/Field.java @@ -1,4 +1,5 @@ package frc.robot; + public class Field { } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index fe215d7..b4e285e 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -7,9 +7,11 @@ import edu.wpi.first.wpilibj.RobotBase; public final class Main { + private Main() {} public static void main(String... args) { RobotBase.startRobot(Robot::new); } + } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 419b730..14a033e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -39,9 +39,9 @@ public Robot() { var sliderTwo = Shuffleboard.getTab("testing").add("Forward/Backward", 0).withWidget(BuiltInWidgets.kNumberSlider).getEntry(); // Fill in parameter info Command driveFieldOrientedAngularVelocity = swerve.driveCommand( - () -> ControllerK.translationalYLimiter.calculate(MathUtil.applyDeadband(-xboxController.getLeftY(), DriveK.leftJoystickDeadband)), - () -> ControllerK.translationalXLimiter.calculate(MathUtil.applyDeadband(-xboxController.getLeftX(), DriveK.leftJoystickDeadband)), - () -> ControllerK.rotationalLimiter.calculate(MathUtil.applyDeadband(-xboxController.getRightX(), DriveK.rightJoystickDeadband)), + () -> DriveK.translationalYLimiter.calculate(MathUtil.applyDeadband(-xboxController.getLeftY(), ControllerK.leftJoystickDeadband)), + () -> DriveK.translationalXLimiter.calculate(MathUtil.applyDeadband(-xboxController.getLeftX(), ControllerK.leftJoystickDeadband)), + () -> DriveK.rotationalLimiter.calculate(MathUtil.applyDeadband(-xboxController.getRightX(), ControllerK.rightJoystickDeadband)), false ); @@ -54,7 +54,7 @@ private void configureBindings() { @Override public void robotPeriodic() { + } } - diff --git a/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java index e888e21..95c5fe7 100644 --- a/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java @@ -74,4 +74,5 @@ public void end() { public boolean isFinished() { return false; //! TODO: } + } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index c7937d2..66f95c4 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -31,16 +31,12 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; /** - * ? TODO: Need to tune current limits, pidfproperties, controllerproperties, maybe find wheel grip coefficient of friction + * ? TODO: Need to tune current limits, controllerproperties, maybe find wheel grip coefficient of friction */ public class Swerve extends SubsystemBase { private final SwerveDrive swerveDrive; - /** - * - * @param directory Directory of swerve drive config files. - */ public Swerve() { double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(SwerveK.steerGearRatio, 4096); double driveConversionFactor = SwerveMath.calculateMetersPerRotation(SwerveK.driveGearRatio, SwerveK.wheelDiameter.in(Inches)); @@ -56,7 +52,7 @@ public Swerve() { throw new RuntimeException("Swerve directory not found."); } swerveDrive = parser.createSwerveDrive(SwerveK.maxRobotSpeed.in(MetersPerSecond), angleConversionFactor, driveConversionFactor); - for (var mod : swerveDrive.getModules()) { //^ BANDAID SOLUTION FOR INVERT ISSUE + for (var mod : swerveDrive.getModules()) { //? BANDAID SOLUTION FOR INVERT ISSUE var motor = (WPI_TalonSRX) mod.getAngleMotor().getMotor(); motor.configSelectedFeedbackSensor(FeedbackDevice.PulseWidthEncodedPosition); // if (motor.getDeviceID() == 4) { @@ -105,6 +101,7 @@ public void setupPathPlanner() { * @param TranslationX Translation in the X direction (Forwards, Backwards) * @param TranslationY Translation in the Y direction (Left, Right) * @param angularVelocity Angular Velocity to set + * @param fieldRelative Whether or not swerve is controlled using field relative speeds * @return */ public Command driveCommand(DoubleSupplier TranslationX, DoubleSupplier TranslationY, DoubleSupplier angularVelocity, boolean fieldRelative) {