From a28413eb7bb892db2310548e88abc73c7a87b438 Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Wed, 20 Mar 2024 15:27:13 -0500 Subject: [PATCH 1/6] Added another Override for drive to allow for changing dt of discretize --- src/main/java/swervelib/SwerveDrive.java | 55 ++++++++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 0aa2fbd9..31a3c379 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -502,6 +502,61 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent setRawModuleStates(swerveModuleStates, isOpenLoop); } + /** + * The primary method for controlling the drivebase. Takes a {@link ChassisSpeeds}, and calculates and commands module + * states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Applies + * heading correction if enabled and necessary. + * + * @param velocity The chassis speeds to set the robot to achieve. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + * @param dtSeconds The duration of the timestep the speeds should be applied for. + */ + public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters, double dtSeconds) + { + + // Thank you to Jared Russell FRC254 for Open Loop Compensation Code + // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 + if (chassisVelocityCorrection) + { + velocity = ChassisSpeeds.discretize(velocity, dtSeconds); + } + + // Heading Angular Velocity Deadband, might make a configuration option later. + // Originally made by Team 1466 Webb Robotics. + // Modified by Team 7525 Pioneers and BoiledBurntBagel of 6036 + if (headingCorrection) + { + if (Math.abs(velocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND + && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND + || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND)) + { + velocity.omegaRadiansPerSecond = + swerveController.headingCalculate(getOdometryHeading().getRadians(), lastHeadingRadians); + } else + { + lastHeadingRadians = getOdometryHeading().getRadians(); + } + } + + // Display commanded speed for testing + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.INFO) + { + SmartDashboard.putString("RobotVelocity", velocity.toString()); + } + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + { + SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond; + SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond; + SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond); + } + + // Calculate required module states via kinematics + SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity, centerOfRotationMeters); + + setRawModuleStates(swerveModuleStates, isOpenLoop); + } + /** * Set the maximum speeds for desaturation. From a1d7281c681c09df63f2669f8b52b8a5cfbc9140 Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Wed, 20 Mar 2024 20:42:31 -0500 Subject: [PATCH 2/6] added setChassisDiscretization --- src/main/java/swervelib/SwerveDrive.java | 67 ++++-------------------- 1 file changed, 10 insertions(+), 57 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 31a3c379..1965a53b 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -101,6 +101,10 @@ public class SwerveDrive * Whether to correct heading when driving translationally. Set to true to enable. */ public boolean headingCorrection = false; + /** + * Amount of seconds the duration of the timestep the speeds should be applied for. + */ + public double discretizationdtSeconds = 0.02; /** * Deadband for speeds in heading correction. */ @@ -464,62 +468,7 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 if (chassisVelocityCorrection) { - velocity = ChassisSpeeds.discretize(velocity, 0.02); - } - - // Heading Angular Velocity Deadband, might make a configuration option later. - // Originally made by Team 1466 Webb Robotics. - // Modified by Team 7525 Pioneers and BoiledBurntBagel of 6036 - if (headingCorrection) - { - if (Math.abs(velocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND - && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND - || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND)) - { - velocity.omegaRadiansPerSecond = - swerveController.headingCalculate(getOdometryHeading().getRadians(), lastHeadingRadians); - } else - { - lastHeadingRadians = getOdometryHeading().getRadians(); - } - } - - // Display commanded speed for testing - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.INFO) - { - SmartDashboard.putString("RobotVelocity", velocity.toString()); - } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) - { - SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond); - } - - // Calculate required module states via kinematics - SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity, centerOfRotationMeters); - - setRawModuleStates(swerveModuleStates, isOpenLoop); - } - - /** - * The primary method for controlling the drivebase. Takes a {@link ChassisSpeeds}, and calculates and commands module - * states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Applies - * heading correction if enabled and necessary. - * - * @param velocity The chassis speeds to set the robot to achieve. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. - * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. - * @param dtSeconds The duration of the timestep the speeds should be applied for. - */ - public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters, double dtSeconds) - { - - // Thank you to Jared Russell FRC254 for Open Loop Compensation Code - // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 - if (chassisVelocityCorrection) - { - velocity = ChassisSpeeds.discretize(velocity, dtSeconds); + velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds); } // Heading Angular Velocity Deadband, might make a configuration option later. @@ -557,7 +506,6 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent setRawModuleStates(swerveModuleStates, isOpenLoop); } - /** * Set the maximum speeds for desaturation. * @@ -1211,4 +1159,9 @@ public void setCosineCompensator(boolean enabled) } } + public void setChassisDiscretization(boolean enable, double dtSeconds){ + chassisVelocityCorrection = enable; + discretizationdtSeconds = dtSeconds; + } + } From 637e799f4160101f683e34f33610f86e49a34cdd Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Sun, 24 Mar 2024 12:55:12 -0500 Subject: [PATCH 3/6] Rename pushOffsetsToControllers to pushOffsetsToEncoders Renamed the method to push absolute encoder offsets to the encoders as it was poorly named for anything other than attached encoders. --- src/main/java/swervelib/SwerveDrive.java | 4 ++-- src/main/java/swervelib/SwerveModule.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 1965a53b..9d7ccdfc 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -1125,11 +1125,11 @@ public void resetDriveEncoders() * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the * internal offsets to prevent double offsetting. */ - public void pushOffsetsToControllers() + public void pushOffsetsToEncoders() { for (SwerveModule module : swerveModules) { - module.pushOffsetsToControllers(); + module.pushOffsetsToEncoders(); } } diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index c7dd56c1..9231e0f1 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -252,7 +252,7 @@ public void setAntiJitter(boolean antiJitter) this.antiJitterEnabled = antiJitter; if (antiJitter) { - pushOffsetsToControllers(); + pushOffsetsToEncoders(); } else { restoreInternalOffset(); @@ -580,7 +580,7 @@ public SwerveModuleConfiguration getConfiguration() /** * Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset. */ - public void pushOffsetsToControllers() + public void pushOffsetsToEncoders() { if (absoluteEncoder != null && angleOffset == configuration.angleOffset) { From 235c436daf6b0e824650b4d995e853ab50ae3e3a Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Sun, 24 Mar 2024 13:01:16 -0500 Subject: [PATCH 4/6] Fix reviewed issues --- src/main/java/swervelib/SwerveDrive.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 9d7ccdfc..d3e8a808 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -104,7 +104,7 @@ public class SwerveDrive /** * Amount of seconds the duration of the timestep the speeds should be applied for. */ - public double discretizationdtSeconds = 0.02; + private double discretizationdtSeconds = 0.02; /** * Deadband for speeds in heading correction. */ @@ -1159,6 +1159,12 @@ public void setCosineCompensator(boolean enabled) } } + /** + * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction + * + * @param enable + * @param dtSeconds + */ public void setChassisDiscretization(boolean enable, double dtSeconds){ chassisVelocityCorrection = enable; discretizationdtSeconds = dtSeconds; From e58d6bd2f454cbcce54ae7934956c463ca9c2972 Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Sun, 24 Mar 2024 13:27:53 -0500 Subject: [PATCH 5/6] Add Method to drive Field Oriented and Robot Oriented at the same time --- src/main/java/swervelib/SwerveDrive.java | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index d3e8a808..1745f2cc 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -346,6 +346,19 @@ public void setHeadingCorrection(boolean state, double deadband) HEADING_CORRECTION_DEADBAND = deadband; } + /** + * Tertiary method of controlling the drive base given velocity in both field oriented and robot oriented at the same time. + * The inputs are added together so this is not intneded to be used to give the driver both methods of control. + * + * @param fieldOrientedVelocity The field oriented velocties to use + * @param robotOrientedVelocity The robot oriented velocties to use + */ + public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVelocity, ChassisSpeeds robotOrientedVelocity) + { + ChassisSpeeds TotalVelocties = ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading()).plus(robotOrientedVelocity); + drive(TotalVelocties); + } + /** * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use. * From 9ab63278d39549332055ae745282cbf83b4931a2 Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Sun, 24 Mar 2024 13:46:39 -0500 Subject: [PATCH 6/6] Moved Maximum Speed to Constants.java and Added ac Comment to AbsoluteDriveAdv --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 9 +++++++++ .../subsystems/swervedrive/SwerveSubsystem.java | 13 +++++-------- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3821fc44..12accfb0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,6 +24,7 @@ public final class Constants public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; //s, 20ms + 110ms sprk max velocity lag + public static final double MAX_SPEED = Units.feetToMeters(14.5); // Maximum speed of the robot in meters per second, used to limit acceleration. public static final class AutonConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 34dceccd..2fedde3a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,6 +42,15 @@ public RobotContainer() // Configure the trigger bindings configureBindings(); + + + // Applies deadbands and inverts controls because joysticks + // are back-right positive while robot + // controls are front-left positive + // left stick controls translation + // right stick controls the rotational velocity + // buttons are quick rotation positions to different ways to face + // WARNING: default buttons are on the same buttons as the ones defined in configureBindings AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase, () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index b3c88a81..1513f2aa 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import frc.robot.Constants; import frc.robot.Constants.AutonConstants; import java.io.File; import java.util.function.DoubleSupplier; @@ -48,10 +49,6 @@ public class SwerveSubsystem extends SubsystemBase * Swerve drive object. */ private final SwerveDrive swerveDrive; - /** - * Maximum speed of the robot in meters per second, used to limit acceleration. - */ - public double maximumSpeed = Units.feetToMeters(14.5); /** * Initialize {@link SwerveDrive} with the directory provided. @@ -78,7 +75,7 @@ public SwerveSubsystem(File directory) SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { - swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed); + swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED); // Alternative method if you don't want to supply the conversion factor via JSON files. // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, angleConversionFactor, driveConversionFactor); } catch (Exception e) @@ -98,7 +95,7 @@ public SwerveSubsystem(File directory) */ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { - swerveDrive = new SwerveDrive(driveCfg, controllerCfg, maximumSpeed); + swerveDrive = new SwerveDrive(driveCfg, controllerCfg, Constants.MAX_SPEED); } /** @@ -516,7 +513,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headin headingX, headingY, getHeading().getRadians(), - maximumSpeed); + Constants.MAX_SPEED); } /** @@ -536,7 +533,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an yInput, angle.getRadians(), getHeading().getRadians(), - maximumSpeed); + Constants.MAX_SPEED); } /**