From 9cfb3eb8aeb70d5f02cfa22362b0f921d49cc561 Mon Sep 17 00:00:00 2001 From: Tricks1228 <122404998+Tricks1228@users.noreply.github.com> Date: Fri, 20 Dec 2024 17:49:46 -0600 Subject: [PATCH] Joystick control (#5) Swerve drives. Co-authored-by: WispySparks --- .gitignore | 4 + library | 2 +- .../deploy/swerve/modules/frontright.json | 2 +- .../deploy/swerve/modules/pidfproperties.json | 4 +- src/main/java/frc/robot/Constants.java | 28 ++- .../frc/robot/DynamicSlewRateLimiter.java | 81 +++++++ src/main/java/frc/robot/Field.java | 1 + src/main/java/frc/robot/Main.java | 2 + src/main/java/frc/robot/Robot.java | 31 ++- .../java/frc/robot/subsystems/Swerve.java | 106 +++++++-- src/main/java/swervelib/SwerveDrive.java | 203 ++++++++++++++++-- src/main/java/swervelib/SwerveDriveTest.java | 4 +- src/main/java/swervelib/SwerveModule.java | 70 +++++- src/main/java/swervelib/commit.txt | 2 +- ...dCoderSwerve.java => CanAndMagSwerve.java} | 26 +-- .../encoders/SparkMaxAnalogEncoderSwerve.java | 6 +- .../encoders/TalonSRXEncoderSwerve.java | 9 +- .../java/swervelib/imu/ADIS16448Swerve.java | 4 +- .../java/swervelib/imu/ADIS16470Swerve.java | 4 +- .../java/swervelib/imu/ADXRS450Swerve.java | 4 +- .../java/swervelib/imu/AnalogGyroSwerve.java | 4 +- .../java/swervelib/imu/CanandgyroSwerve.java | 135 ++++++++++++ src/main/java/swervelib/imu/IMUVelocity.java | 144 +++++++++++++ src/main/java/swervelib/imu/NavXSwerve.java | 17 +- .../java/swervelib/imu/Pigeon2Swerve.java | 40 ++-- src/main/java/swervelib/imu/PigeonSwerve.java | 24 ++- src/main/java/swervelib/imu/SwerveIMU.java | 1 + .../math/IMULinearMovingAverageFilter.java | 57 +++++ src/main/java/swervelib/math/SwerveMath.java | 2 +- .../swervelib/motors/SparkFlexSwerve.java | 17 +- .../motors/SparkMaxBrushedMotorSwerve.java | 17 +- .../java/swervelib/motors/SparkMaxSwerve.java | 59 ++++- .../java/swervelib/motors/TalonFXSwerve.java | 14 +- .../java/swervelib/motors/TalonSRXSwerve.java | 18 +- .../swervelib/parser/json/DeviceJson.java | 16 +- .../swervelib/parser/json/ModuleJson.java | 16 +- .../telemetry/SwerveDriveTelemetry.java | 2 +- vendordeps/ReduxLib_2024.json | 8 +- 38 files changed, 1004 insertions(+), 180 deletions(-) create mode 100644 src/main/java/frc/robot/DynamicSlewRateLimiter.java rename src/main/java/swervelib/encoders/{CanAndCoderSwerve.java => CanAndMagSwerve.java} (63%) create mode 100644 src/main/java/swervelib/imu/CanandgyroSwerve.java create mode 100644 src/main/java/swervelib/imu/IMUVelocity.java create mode 100644 src/main/java/swervelib/math/IMULinearMovingAverageFilter.java diff --git a/.gitignore b/.gitignore index 5528d4f..e961f53 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,10 @@ # This gitignore has been specially created by the WPILib team. # If you remove items from this file, intellisense might break. +networktables.json +simgui-ds.json +simgui.json + ### C++ ### # Prerequisites *.d diff --git a/library b/library index 055cdd5..614e75a 160000 --- a/library +++ b/library @@ -1 +1 @@ -Subproject commit 055cdd5f88245809d7f4bcb670a737285b531675 +Subproject commit 614e75adbf621e2e169d8bc8429793c6cbd34644 diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index 2f79c1f..1296527 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -23,5 +23,5 @@ "drive": false, "angle": false }, - "absoluteEncoderInverted": false + "absoluteEncoderInverted": true } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json index 0ef1681..30361bf 100644 --- a/src/main/deploy/swerve/modules/pidfproperties.json +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -7,9 +7,9 @@ "iz": 0 }, "angle": { - "p": 0, + "p": 0.75, "i": 0, - "d": 0, + "d": 0.55, "f": 0, "iz": 0 } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 027dfd6..9d23c4a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,22 +14,32 @@ 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); - public static final double steerGearRatio = 12.8; //! Figure Out; - public static final double driveGearRatio = 6; //! Figure Out + public static final double steerGearRatio = 1; + public static final double driveGearRatio = 6; - public static final Measure> maxModuleSpeed = MetersPerSecond.of(0); //! Tune + public static final Measure> maxRobotSpeed = MetersPerSecond.of(4.24); - public static final PIDConstants translationConstants = new PIDConstants(1, 1, 1); //! Tune - public static final PIDConstants rotationConstants = new PIDConstants(1, 1, 1); //! Tune + public static final PIDConstants translationConstants = new PIDConstants(1, 1, 1); //! TODO: Tune + public static final PIDConstants rotationConstants = new PIDConstants(1, 1, 1); //! TODO: Tune public static final File swerveDirectory = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/swerve"); } + + public static class ControllerK { + public static final int xboxPort = 0; + public static final double leftJoystickDeadband = 0.05; + public static final double rightJoystickDeadband = 0.05; + } + + public static class DriveK { + 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 new file mode 100644 index 0000000..ab9c669 --- /dev/null +++ b/src/main/java/frc/robot/DynamicSlewRateLimiter.java @@ -0,0 +1,81 @@ +package frc.robot; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUtil; + +/** +* 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. + * 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(); + } + + /** + * 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; + 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 08ba675..de9f20f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,16 +4,43 @@ package frc.robot; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants.ControllerK; +import frc.robot.Constants.DriveK; import frc.robot.subsystems.Swerve; public class Robot extends TimedRobot { private final Swerve swerve = new Swerve(); + private final CommandXboxController xboxController = new CommandXboxController(ControllerK.xboxPort); public Robot() { - + DriverStation.silenceJoystickConnectionWarning(true); + addPeriodic(() -> CommandScheduler.getInstance().run(), kDefaultPeriod); + configureBindings(); + Command driveFieldOrientedAngularVelocity = swerve.driveCommand( + () -> 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 + ); + + swerve.setDefaultCommand(driveFieldOrientedAngularVelocity); + } + + private void configureBindings() { + // Reset forward direction for field relative + xboxController.x().and(xboxController.b()).onTrue(swerve.runOnce(swerve::zeroGyro)); } -} + @Override + public void robotPeriodic() { + } + +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 0e21ca4..e47037d 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -6,11 +6,13 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import java.io.IOException; +import java.util.function.DoubleSupplier; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.Angle; @@ -26,19 +28,14 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; /** - * ? Need to tune current limits, pidfproperties, controllerproperties, maybe find wheel grip coefficient of friction + * ? TODO: Need to tune current limits, controllerproperties/heading correction */ public class Swerve extends SubsystemBase { private final SwerveDrive swerveDrive; - // double maximumSpeed = Units.feetToMeters(4.5); - /** - * - * @param directory Directory of swerve drive config files. - */ public Swerve() { - double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(SwerveK.steerGearRatio); + double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(SwerveK.steerGearRatio, 4096); double driveConversionFactor = SwerveMath.calculateMetersPerRotation(SwerveK.driveGearRatio, SwerveK.wheelDiameter.in(Inches)); SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; @@ -49,9 +46,18 @@ public Swerve() { e.printStackTrace(); throw new RuntimeException("Swerve directory not found."); } - swerveDrive = parser.createSwerveDrive(SwerveK.maxModuleSpeed.in(MetersPerSecond), angleConversionFactor, driveConversionFactor); + swerveDrive = parser.createSwerveDrive(SwerveK.maxRobotSpeed.in(MetersPerSecond), angleConversionFactor, driveConversionFactor); + setupPathPlanner(); + } + + @Override + public void periodic() { + // Do nothing } + /** + * Configures PathPlanner + */ public void setupPathPlanner() { AutoBuilder.configureHolonomic( this::getPose, @@ -60,7 +66,7 @@ public void setupPathPlanner() { this::setChassisSpeeds, new HolonomicPathFollowerConfig(SwerveK.translationConstants, SwerveK.rotationConstants, - SwerveK.maxModuleSpeed.in(MetersPerSecond), + SwerveK.maxRobotSpeed.in(MetersPerSecond), SwerveK.driveBaseRadius.in(Meters), null), () -> { @@ -69,54 +75,114 @@ public void setupPathPlanner() { return alliance.get() == DriverStation.Alliance.Red; } return false; } - ,this); + , this); } /** - * - * @param TranslationX Translation in the X direction - * @param TranslationY Translation in the Y direction - * @param angularVelocity Angular Velocity to set - * @return + * Commands the robot to drive according to the given velocities + * @param TranslationX Translation in the X direction (Forwards, Backwards) between -1 and 1 + * @param TranslationY Translation in the Y direction (Left, Right) between -1 and 1 + * @param angularVelocity Angular Velocity to set between -1 and 1 + * @param fieldRelative Whether or not swerve is controlled using field relative speeds + * @return A command to drive the robot according to given velocities */ - public Command driveCommand(double TranslationX, double TranslationY, double angularVelocity) { + public Command driveCommand(DoubleSupplier TranslationX, DoubleSupplier TranslationY, DoubleSupplier angularVelocity, boolean fieldRelative) { return runOnce(() -> drive( - new Translation2d(TranslationX * swerveDrive.getMaximumVelocity(), TranslationY * swerveDrive.getMaximumVelocity()), - angularVelocity * swerveDrive.getMaximumAngularVelocity(), - true, false)); + new Translation2d(TranslationX.getAsDouble() * swerveDrive.getMaximumVelocity(), TranslationY.getAsDouble() * swerveDrive.getMaximumVelocity()), + angularVelocity.getAsDouble() * swerveDrive.getMaximumAngularVelocity(), + fieldRelative, true)); } + /** + * Turns the robot to the desired angle + * @param targetAngle Desired angle + * @param currentAngle Current angle + * @param fieldRelative Whether or not swerve is controlled using field relative speeds + * @return An instant command that turns the robot + */ public Command turnCommand(Measure targetAngle, Measure currentAngle, boolean fieldRelative) { return runOnce(() -> turn(targetAngle, currentAngle, fieldRelative)); } + /** + * Turns the robot to the desired angle + * @param targetAngle Desired angle + * @param currentAngle Current angle + * @param fieldRelative Whether or not swerve is controlled using field relative speeds + */ public void turn(Measure targetAngle, Measure currentAngle, boolean fieldRelative) { drive(getPose().getTranslation(), getTurningAngle(targetAngle, currentAngle).in(Degrees), fieldRelative, false); } + /** + * Gets the closest angle to turn to depending on the current heading of the robot + * @param desiredAngle Angle to turn to + * @param currentHeading Current heading + * @return Angle to turn to + */ public Measure getTurningAngle(Measure desiredAngle, Measure currentHeading) { double angle = (desiredAngle.minus(currentHeading).plus(Degrees.of(540))).in(Degrees); angle = (angle % 360) - 180; return Degrees.of(angle); } + /** + * Commands the drivebase to move according to the given linear and rotational velocities + * @param translation Linear velocity of the robot in meters per second + * @param rotation Rotation rate of the robot in Radians per second + * @param fieldRelative Whether the robot is field relative (true) or robot relative (false) + * @param isOpenLoop Whether it uses a closed loop velocity control or an open loop + */ public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { swerveDrive.drive(translation, rotation, fieldRelative, isOpenLoop); } + /** + * Resets the odometry to the given pose + * @param initialHolonomicPose Pose to reset the odemetry to + */ public void resetOdometry(Pose2d initialHolonomicPose) { swerveDrive.resetOdometry(initialHolonomicPose); } + /** + * Returns the robot's pose + * @return Current pose of the robot as a Pose2d + */ public Pose2d getPose() { return swerveDrive.getPose(); } + /** + * Returns the robot's velocity (x, y, and omega) + * @return Current velocity of the robot + */ public ChassisSpeeds getRobotVelocity() { return swerveDrive.getRobotVelocity(); } + /** + * Set the speed of the robot with closed loop velocity control + * @param chassisSpeeds to set speed with + */ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { swerveDrive.setChassisSpeeds(chassisSpeeds); } -} + + /** + * Returns the robot's heading as a Rotation2d + * @return Rotational component of the robots pose + */ + public Rotation2d getHeading() { + return getPose().getRotation(); + } + + /** + * Resets the gyro and odometry to the current position but the current direction is now seen as 0. + * Useful for resetting the forward direction for field relative driving + */ + public void zeroGyro() { + swerveDrive.zeroGyro(); + } + +} \ No newline at end of file diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index dfbb82c..7d83e5d 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -30,6 +30,7 @@ import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import swervelib.encoders.CANCoderSwerve; +import swervelib.imu.IMUVelocity; import swervelib.imu.Pigeon2Swerve; import swervelib.imu.SwerveIMU; import swervelib.math.SwerveMath; @@ -97,6 +98,25 @@ public class SwerveDrive * correction. */ public boolean chassisVelocityCorrection = true; + /** + * Correct chassis velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} (auto) using 254's + * correction during auto. + */ + public boolean autonomousChassisVelocityCorrection = false; + /** + * Correct for skew that scales with angular velocity in + * {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} + */ + public boolean angularVelocityCorrection = false; + /** + * Correct for skew that scales with angular velocity in + * {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} during auto. + */ + public boolean autonomousAngularVelocityCorrection = false; + /** + * Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15). + */ + public double angularVelocityCoefficient = 0; /** * Whether to correct heading when driving translationally. Set to true to enable. */ @@ -113,6 +133,11 @@ public class SwerveDrive * Swerve IMU device for sensing the heading of the robot. */ private SwerveIMU imu; + /** + * Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in + * {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}. + */ + private IMUVelocity imuVelocity; /** * Simulation of the swerve drive. */ @@ -479,12 +504,7 @@ public void drive( public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters) { - // 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, discretizationdtSeconds); - } + velocity = movementOptimizations(velocity, chassisVelocityCorrection, angularVelocityCorrection); // Heading Angular Velocity Deadband, might make a configuration option later. // Originally made by Team 1466 Webb Robotics. @@ -518,7 +538,7 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent // Calculate required module states via kinematics SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity, centerOfRotationMeters); - setRawModuleStates(swerveModuleStates, isOpenLoop); + setRawModuleStates(swerveModuleStates, velocity, isOpenLoop); } /** @@ -567,15 +587,17 @@ public double getMaximumAngularVelocity() /** * Set the module states (azimuth and velocity) directly. * - * @param desiredStates A list of SwerveModuleStates to send to the modules. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + * @param desiredStates A list of SwerveModuleStates to send to the modules. + * @param desiredChassisSpeed The desired chassis speeds to set the robot to achieve. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. */ - private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) + private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds desiredChassisSpeed, + boolean isOpenLoop) { // Desaturates wheel speeds if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, getRobotVelocity(), + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, desiredChassisSpeed, maxSpeedMPS, attainableMaxTranslationalSpeedMetersPerSecond, attainableMaxRotationalVelocityRadiansPerSecond); @@ -592,15 +614,24 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpe } /** - * Set the module states (azimuth and velocity) directly. Used primarily for auto paths. + * Set the module states (azimuth and velocity) directly. Used primarily for auto paths. Does not allow for usage of + * desaturateWheelSpeeds(SwerveModuleState[] moduleStates, ChassisSpeeds desiredChassisSpeed, double + * attainableMaxModuleSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond, double + * attainableMaxRotationalVelocityRadiansPerSecond) * * @param desiredStates A list of SwerveModuleStates to send to the modules. * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. */ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) { - setRawModuleStates(kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)), - isOpenLoop); + desiredStates = kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxSpeedMPS); + + // Sets states + for (SwerveModule module : swerveModules) + { + module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false); + } } /** @@ -610,11 +641,16 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo */ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { + + chassisSpeeds = movementOptimizations(chassisSpeeds, + autonomousChassisVelocityCorrection, + autonomousAngularVelocityCorrection); + SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond; SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond; SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond); - setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false); + setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), chassisSpeeds, false); } /** @@ -836,9 +872,25 @@ public void setMotorIdleMode(boolean brake) } } + /** + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a + * few seconds. + * + * @param enabled Enable state + * @param deadband Deadband in degrees, default is 3 degrees. + */ + public void setModuleEncoderAutoSynchronize(boolean enabled, double deadband) + { + for (SwerveModule swerveModule : swerveModules) + { + swerveModule.setEncoderAutoSynchronize(enabled, deadband); + } + } + + /** * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the - * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and + * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], ChassisSpeeds, boolean)} function and * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides * what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. * @@ -866,7 +918,7 @@ public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward /** * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the - * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and + * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], ChassisSpeeds, boolean)} function and * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides * what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. Overwrites the * {@link SwerveModule#setFeedforward(SimpleMotorFeedforward)}. @@ -1064,6 +1116,21 @@ public void addVisionMeasurement(Pose2d robotPose, double timestamp, odometryLock.unlock(); } + /** + * Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements + * after the autonomous period, or to change trust as distance to a vision target increases. + * + * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these numbers to trust + * global measurements from vision less. This matrix is in the form [x, y, theta], + * with units in meters and radians. + */ + public void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) + { + odometryLock.lock(); + swerveDrivePoseEstimator.setVisionMeasurementStdDevs(visionMeasurementStdDevs); + odometryLock.unlock(); + } + /** * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with * the given timestamp of the vision measurement. @@ -1190,7 +1257,7 @@ public void setCosineCompensator(boolean enabled) } /** - * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction + * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop * * @param enable Enable chassis velocity correction, which will use * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following. @@ -1198,8 +1265,104 @@ public void setCosineCompensator(boolean enabled) */ public void setChassisDiscretization(boolean enable, double dtSeconds) { - chassisVelocityCorrection = enable; - discretizationdtSeconds = dtSeconds; + if (!SwerveDriveTelemetry.isSimulation) + { + chassisVelocityCorrection = enable; + discretizationdtSeconds = dtSeconds; + } + } + + /** + * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop + * and/or auto + * + * @param useInTeleop Enable chassis velocity correction, which will use + * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop. + * @param useInAuto Enable chassis velocity correction, which will use + * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto. + * @param dtSeconds The duration of the timestep the speeds should be applied for. + */ + public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds) + { + if (!SwerveDriveTelemetry.isSimulation) + { + chassisVelocityCorrection = useInTeleop; + autonomousChassisVelocityCorrection = useInAuto; + discretizationdtSeconds = dtSeconds; + } + } + + /** + * Enables angular velocity skew correction in teleop and/or autonomous and sets the angular velocity coefficient for + * both modes + * + * @param useInTeleop Enables angular velocity correction in teleop. + * @param useInAuto Enables angular velocity correction in autonomous. + * @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15. Start with a + * value of 0.1, test in teleop. When enabling for the first time if the skew is + * significantly worse try inverting the value. Tune by moving in a straight line while + * rotating. Testing is best done with angular velocity controls on the right stick. + * Change the value until you are visually happy with the skew. Ensure your tune works + * with different translational and rotational magnitudes. If this reduces skew in teleop, + * it may improve auto. + */ + public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAuto, double angularVelocityCoeff) + { + if (!SwerveDriveTelemetry.isSimulation) + { + imuVelocity = IMUVelocity.createIMUVelocity(imu); + angularVelocityCorrection = useInTeleop; + autonomousAngularVelocityCorrection = useInAuto; + angularVelocityCoefficient = angularVelocityCoeff; + } + } + + /** + * Correct for skew that worsens as angular velocity increases + * + * @param velocity The chassis speeds to set the robot to achieve. + * @return {@link ChassisSpeeds} of the robot after angular velocity skew correction. + */ + public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity) + { + var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient); + if (angularVelocity.getRadians() != 0.0) + { + velocity = ChassisSpeeds.fromRobotRelativeSpeeds( + velocity.vxMetersPerSecond, + velocity.vyMetersPerSecond, + velocity.omegaRadiansPerSecond, + getOdometryHeading()); + velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity)); + } + return velocity; + } + + /** + * Enable desired drive corrections + * + * @param velocity The chassis speeds to set the robot to achieve. + * @param uesChassisDiscretize Correct chassis velocity using 254's correction. + * @param useAngularVelocitySkewCorrection Use the robot's angular velocity to correct for skew. + * @return The chassis speeds after optimizations. + */ + private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize, + boolean useAngularVelocitySkewCorrection) + { + + if (useAngularVelocitySkewCorrection) + { + velocity = angularVelocitySkewCorrection(velocity); + } + + // 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 (uesChassisDiscretize) + { + velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds); + } + + return velocity; } } diff --git a/src/main/java/swervelib/SwerveDriveTest.java b/src/main/java/swervelib/SwerveDriveTest.java index 848852c..a3d71df 100644 --- a/src/main/java/swervelib/SwerveDriveTest.java +++ b/src/main/java/swervelib/SwerveDriveTest.java @@ -356,8 +356,8 @@ public static void logAngularMotorVoltage(SwerveModule module, SysIdRoutineLog l public static void logAngularMotorActivity(SwerveModule module, SysIdRoutineLog log, Supplier powerSupplied) { double power = powerSupplied.get(); - double angle = module.getAbsolutePosition(); - double velocity = module.getAbsoluteEncoder().getVelocity(); + double angle = module.getAngleMotor().getPosition(); + double velocity = module.getAngleMotor().getVelocity(); SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Power", power); SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Position", angle); SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Absolute Encoder Velocity", diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 15312ce..8b5dec1 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -1,5 +1,7 @@ package swervelib; +import com.revrobotics.CANSparkMax; +import com.revrobotics.MotorFeedbackSensor; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -41,7 +43,7 @@ public class SwerveModule /** * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right. */ - public final int moduleNumber; + public final int moduleNumber; /** * Swerve Motors. */ @@ -93,7 +95,7 @@ public class SwerveModule /** * Anti-Jitter AKA auto-centering disabled. */ - private boolean antiJitterEnabled = true; + private boolean antiJitterEnabled = true; /** * Last swerve module state applied. */ @@ -109,7 +111,15 @@ public class SwerveModule /** * Encoder synchronization queued. */ - private boolean synchronizeEncoderQueued = false; + private boolean synchronizeEncoderQueued = false; + /** + * Encoder, Absolute encoder synchronization enabled. + */ + private boolean synchronizeEncoderEnabled = false; + /** + * Encoder synchronization deadband in degrees. + */ + private double synchronizeEncoderDeadband = 3; /** @@ -240,12 +250,36 @@ public void setDriveMotorVoltageCompensation(double optimalVoltage) */ public void queueSynchronizeEncoders() { - if (absoluteEncoder != null) + if (absoluteEncoder != null && synchronizeEncoderEnabled) { synchronizeEncoderQueued = true; } } + /** + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a + * few seconds. + * + * @param enabled Enable state + * @param deadband Deadband in degrees, default is 3 degrees. + */ + public void setEncoderAutoSynchronize(boolean enabled, double deadband) + { + synchronizeEncoderEnabled = enabled; + synchronizeEncoderDeadband = deadband; + } + + /** + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a + * few seconds. + * + * @param enabled Enable state + */ + public void setEncoderAutoSynchronize(boolean enabled) + { + synchronizeEncoderEnabled = enabled; + } + /** * Set the antiJitter functionality, if true the modules will NOT auto center. Pushes the offsets to the angle motor * controllers as well. @@ -353,10 +387,13 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, // Prevent module rotation if angle is the same as the previous angle. // Synchronize encoders if queued and send in the current position as the value from the absolute encoder. - if (absoluteEncoder != null && synchronizeEncoderQueued) + if (absoluteEncoder != null && synchronizeEncoderQueued && synchronizeEncoderEnabled) { double absoluteEncoderPosition = getAbsolutePosition(); - angleMotor.setPosition(absoluteEncoderPosition); + if (Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband) + { + angleMotor.setPosition(absoluteEncoderPosition); + } angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition); synchronizeEncoderQueued = false; } else @@ -591,13 +628,23 @@ public void pushOffsetsToEncoders() { if (absoluteEncoder != null && angleOffset == configuration.angleOffset) { - if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) + // If the absolute encoder is attached. + if (angleMotor.getMotor() instanceof CANSparkMax) { - angleOffset = 0; - } else - { - encoderOffsetWarning.set(true); + if (absoluteEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + { + angleMotor.setAbsoluteEncoder(absoluteEncoder); + if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) + { + angleOffset = 0; + } else + { + angleMotor.setAbsoluteEncoder(null); + encoderOffsetWarning.set(true); + } + } } + } else { noEncoderWarning.set(true); @@ -609,6 +656,7 @@ public void pushOffsetsToEncoders() */ public void restoreInternalOffset() { + angleMotor.setAbsoluteEncoder(null); absoluteEncoder.setAbsoluteEncoderOffset(0); angleOffset = configuration.angleOffset; } diff --git a/src/main/java/swervelib/commit.txt b/src/main/java/swervelib/commit.txt index 4d6308c..fe98918 100644 --- a/src/main/java/swervelib/commit.txt +++ b/src/main/java/swervelib/commit.txt @@ -1,2 +1,2 @@ WispySparks/YAGSL-Example/srx-abs -0b3c5dd0bc5ed0ff8c1b5c04ac1ad2d005600ddf \ No newline at end of file +25cc7f6ee8ea5aec137ea2258180ad31f151432d \ No newline at end of file diff --git a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java b/src/main/java/swervelib/encoders/CanAndMagSwerve.java similarity index 63% rename from src/main/java/swervelib/encoders/CanAndCoderSwerve.java rename to src/main/java/swervelib/encoders/CanAndMagSwerve.java index 1c6b7b1..91000c3 100644 --- a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CanAndMagSwerve.java @@ -1,26 +1,26 @@ package swervelib.encoders; -import com.reduxrobotics.sensors.canandcoder.Canandcoder; +import com.reduxrobotics.sensors.canandmag.Canandmag; /** - * HELIUM {@link Canandcoder} from ReduxRobotics absolute encoder, attached through the CAN bus. + * HELIUM {@link Canandmag} from ReduxRobotics absolute encoder, attached through the CAN bus. */ -public class CanAndCoderSwerve extends SwerveAbsoluteEncoder +public class CanAndMagSwerve extends SwerveAbsoluteEncoder { /** - * The {@link Canandcoder} representing the CANandCoder on the CAN bus. + * The {@link Canandmag} representing the CANandMag on the CAN bus. */ - public Canandcoder encoder; + public Canandmag encoder; /** - * Create the {@link Canandcoder} + * Create the {@link Canandmag} * - * @param canid The CAN ID whenever the CANandCoder is operating on the CANBus. + * @param canid The CAN ID whenever the CANandMag is operating on the CANBus. */ - public CanAndCoderSwerve(int canid) + public CanAndMagSwerve(int canid) { - encoder = new Canandcoder(canid); + encoder = new Canandmag(canid); } /** @@ -44,14 +44,14 @@ public void clearStickyFaults() } /** - * Configure the Canandcoder to read from [0, 360) per second. + * Configure the CANandMag to read from [0, 360) per second. * * @param inverted Whether the encoder is inverted. */ @Override public void configure(boolean inverted) { - encoder.setSettings(new Canandcoder.Settings().setInvertDirection(inverted)); + encoder.setSettings(new Canandmag.Settings().setInvertDirection(inverted)); } /** @@ -77,7 +77,7 @@ public Object getAbsoluteEncoder() } /** - * Cannot set the offset of the Canandcoder. + * Cannot set the offset of the CANandMag. * * @param offset the offset the Absolute Encoder uses as the zero point. * @return true if setting the zero point succeeded, false otherwise @@ -85,7 +85,7 @@ public Object getAbsoluteEncoder() @Override public boolean setAbsoluteEncoderOffset(double offset) { - return encoder.setSettings(new Canandcoder.Settings().setZeroOffset(offset)); + return encoder.setSettings(new Canandmag.Settings().setZeroOffset(offset)); } /** diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index 3024787..bbf9fb6 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -32,13 +32,15 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data * port analog pin. * - * @param motor Motor to create the encoder from. + * @param motor Motor to create the encoder from. + * @param maxVoltage Maximum voltage for analog input reading. */ - public SparkMaxAnalogEncoderSwerve(SwerveMotor motor) + public SparkMaxAnalogEncoderSwerve(SwerveMotor motor, double maxVoltage) { if (motor.getMotor() instanceof CANSparkMax) { encoder = ((CANSparkMax) motor.getMotor()).getAnalog(Mode.kAbsolute); + encoder.setPositionConversionFactor(360 / maxVoltage); } else { throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); diff --git a/src/main/java/swervelib/encoders/TalonSRXEncoderSwerve.java b/src/main/java/swervelib/encoders/TalonSRXEncoderSwerve.java index 7d72b92..6303a36 100644 --- a/src/main/java/swervelib/encoders/TalonSRXEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/TalonSRXEncoderSwerve.java @@ -4,6 +4,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import swervelib.motors.SwerveMotor; +import swervelib.motors.TalonSRXSwerve; /** * Talon SRX attached absolute encoder. @@ -25,9 +26,9 @@ public class TalonSRXEncoderSwerve extends SwerveAbsoluteEncoder { * @param feedbackDevice the feedback device the sensor uses e.g. PWM or Analog. */ public TalonSRXEncoderSwerve(SwerveMotor motor, FeedbackDevice feedbackDevice) { - if (motor.getMotor() instanceof WPI_TalonSRX talon) { - this.talon = talon; - talon.configSelectedFeedbackSensor(feedbackDevice); + if (motor instanceof TalonSRXSwerve talonSRXSwerve) { + talonSRXSwerve.setSelectedFeedbackDevice(feedbackDevice); + this.talon = (WPI_TalonSRX) talonSRXSwerve.getMotor(); // https://v5.docs.ctr-electronics.com/en/stable/ch14_MCSensor.html#sensor-resolution degreesPerSensorUnit = switch (feedbackDevice) { case Analog -> 360.0 / 1024.0; @@ -56,7 +57,7 @@ public void configure(boolean inverted) { @Override public double getAbsolutePosition() { - return talon.getSelectedSensorPosition() * degreesPerSensorUnit; + return (talon.getSelectedSensorPosition() * degreesPerSensorUnit) % 360; } @Override diff --git a/src/main/java/swervelib/imu/ADIS16448Swerve.java b/src/main/java/swervelib/imu/ADIS16448Swerve.java index 531b30d..be6efa5 100644 --- a/src/main/java/swervelib/imu/ADIS16448Swerve.java +++ b/src/main/java/swervelib/imu/ADIS16448Swerve.java @@ -112,9 +112,11 @@ public Optional getAccel() /** * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty. + * * @return {@link Double} of the rotation rate as an {@link Optional}. */ - public double getRate() { + public double getRate() + { return imu.getRate(); } diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.java index fd01a3d..81eb3b1 100644 --- a/src/main/java/swervelib/imu/ADIS16470Swerve.java +++ b/src/main/java/swervelib/imu/ADIS16470Swerve.java @@ -112,9 +112,11 @@ public Optional getAccel() /** * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty. + * * @return {@link Double} of the rotation rate as an {@link Optional}. */ - public double getRate() { + public double getRate() + { return imu.getRate(); } diff --git a/src/main/java/swervelib/imu/ADXRS450Swerve.java b/src/main/java/swervelib/imu/ADXRS450Swerve.java index dd9a72d..e0be2e8 100644 --- a/src/main/java/swervelib/imu/ADXRS450Swerve.java +++ b/src/main/java/swervelib/imu/ADXRS450Swerve.java @@ -110,9 +110,11 @@ public Optional getAccel() /** * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty. + * * @return {@link Double} of the rotation rate as an {@link Optional}. */ - public double getRate() { + public double getRate() + { return imu.getRate(); } diff --git a/src/main/java/swervelib/imu/AnalogGyroSwerve.java b/src/main/java/swervelib/imu/AnalogGyroSwerve.java index e789d97..1f3d134 100644 --- a/src/main/java/swervelib/imu/AnalogGyroSwerve.java +++ b/src/main/java/swervelib/imu/AnalogGyroSwerve.java @@ -117,9 +117,11 @@ public Optional getAccel() /** * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty. + * * @return {@link Double} of the rotation rate as an {@link Optional}. */ - public double getRate() { + public double getRate() + { return imu.getRate(); } diff --git a/src/main/java/swervelib/imu/CanandgyroSwerve.java b/src/main/java/swervelib/imu/CanandgyroSwerve.java new file mode 100644 index 0000000..aed3a14 --- /dev/null +++ b/src/main/java/swervelib/imu/CanandgyroSwerve.java @@ -0,0 +1,135 @@ +package swervelib.imu; + +import com.reduxrobotics.sensors.canandgyro.Canandgyro; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.Optional; + +/** + * SwerveIMU interface for the Boron {@link Canandgyro} by Redux Robotics + */ +public class CanandgyroSwerve extends SwerveIMU +{ + + /** + * Wait time for status frames to show up. + */ + public static double STATUS_TIMEOUT_SECONDS = 0.04; + /** + * Boron {@link Canandgyro} by Redux Robotics. + */ + private final Canandgyro imu; + /** + * Offset for the Boron {@link Canandgyro}. + */ + private Rotation3d offset = new Rotation3d(); + /** + * Inversion for the gyro + */ + private boolean invertedIMU = false; + + /** + * Generate the SwerveIMU for {@link Canandgyro}. + * + * @param canid CAN ID for the Boron {@link Canandgyro} + */ + public CanandgyroSwerve(int canid) + { + imu = new Canandgyro(canid); + } + + /** + * Reset {@link Canandgyro} to factory default. + */ + @Override + public void factoryDefault() + { + imu.resetFactoryDefaults(STATUS_TIMEOUT_SECONDS); + } + + /** + * Clear sticky faults on {@link Canandgyro}. + */ + @Override + public void clearStickyFaults() + { + imu.clearStickyFaults(); + } + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) + { + this.offset = offset; + } + + /** + * Set the gyro to invert its default direction + * + * @param invertIMU invert gyro direction + */ + public void setInverted(boolean invertIMU) + { + invertedIMU = invertIMU; + } + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRawRotation3d() + { + Rotation3d reading = imu.getRotation3d(); + return invertedIMU ? reading.unaryMinus() : reading; + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() + { + return getRawRotation3d().minus(offset); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() + { + + return Optional.of(new Translation3d(imu.getAccelerationFrame().getValue()).times(9.81 / 16384.0)); + } + + /** + * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty. + * + * @return {@link Double} of the rotation rate as an {@link Optional}. + */ + public double getRate() + { + return imu.getAngularVelocityYaw(); + } + + /** + * Get the instantiated {@link Canandgyro} IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() + { + return imu; + } +} diff --git a/src/main/java/swervelib/imu/IMUVelocity.java b/src/main/java/swervelib/imu/IMUVelocity.java new file mode 100644 index 0000000..c1c0f77 --- /dev/null +++ b/src/main/java/swervelib/imu/IMUVelocity.java @@ -0,0 +1,144 @@ +package swervelib.imu; + +import edu.wpi.first.hal.HALUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Notifier; +import swervelib.math.IMULinearMovingAverageFilter; + +/** + * Generic IMU Velocity filter. + */ +public class IMUVelocity +{ + + /** + * Swerve IMU. + */ + private final SwerveIMU gyro; + /** + * Linear filter used to calculate velocity, we use a custom filter class to prevent unwanted operations. + */ + private final IMULinearMovingAverageFilter velocityFilter; + /** + * WPILib {@link Notifier} to keep IMU velocity up to date. + */ + private final Notifier notifier; + + /** + * Prevents calculation when no previous measurement exists. + */ + private boolean firstCycle = true; + /** + * Tracks the previous loop's recorded time. + */ + private double timestamp = 0.0; + /** + * Tracks the previous loop's position as a Rotation2d. + */ + private Rotation2d position = new Rotation2d(); + /** + * The calculated velocity of the robot based on averaged IMU measurements. + */ + private double velocity = 0.0; + + /** + * Constructor for the IMU Velocity. + * + * @param gyro The SwerveIMU gyro. + * @param periodSeconds The rate to collect measurements from the gyro, in the form (1/number of samples per second), + * make sure this does not exceed the update rate of your IMU. + * @param averagingTaps The number of samples to used for the moving average linear filter. Higher values will not + * allow the system to update to changes in velocity, lower values may create a less smooth + * signal. Expected taps will probably be ~2-8, with the goal of having the lowest smooth value. + */ + public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps) + { + this.gyro = gyro; + velocityFilter = new IMULinearMovingAverageFilter(averagingTaps); + notifier = new Notifier(this::update); + notifier.startPeriodic(periodSeconds); + timestamp = HALUtil.getFPGATime(); + } + + /** + * Static factory for IMU Velocity. Supported IMU rates will be as quick as possible but will not exceed 100hz and + * will use 5 taps (supported IMUs are listed in swervelib's IMU folder). Other gyroscopes will default to 50hz and 5 + * taps. For custom rates please use the IMUVelocity constructor. + * + * @param gyro The SwerveIMU gyro. + * @return {@link IMUVelocity} for the given gyro with adjusted period readings for velocity. + */ + public static IMUVelocity createIMUVelocity(SwerveIMU gyro) + { + double desiredNotifierPeriod = 1.0 / 50.0; + // ADIS16448_IMU ~200HZ: + // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java#L277 + if (gyro instanceof ADIS16448Swerve) + { + desiredNotifierPeriod = 1.0 / 100.0; + } + // ADIS16470_IMU 200HZ + // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java#L345 + else if (gyro instanceof ADIS16470Swerve) + { + desiredNotifierPeriod = 1.0 / 100.0; + } + // ADXRS450_Gyro 2000HZ? + // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java#L31 + else if (gyro instanceof ADXRS450Swerve) + { + desiredNotifierPeriod = 1.0 / 100.0; + } + // NAX (AHRS): 60HZ + // https://github.com/kauailabs/navxmxp/blob/5e010ba810bb7f7eaab597e0b708e34f159984db/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java#L119C25-L119C61 + else if (gyro instanceof NavXSwerve) + { + desiredNotifierPeriod = 1.0 / 60.0; + } + // Pigeon2 100HZ + // https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf + else if (gyro instanceof Pigeon2Swerve) + { + desiredNotifierPeriod = 1.0 / 100.0; + } + // Pigeon 100HZ + // https://store.ctr-electronics.com/content/user-manual/Pigeon%20IMU%20User's%20Guide.pdf + else if (gyro instanceof PigeonSwerve) + { + desiredNotifierPeriod = 1.0 / 100.0; + } + return new IMUVelocity(gyro, desiredNotifierPeriod, 5); + } + + /** + * Update the robot's rotational velocity based on the current gyro position. + */ + private void update() + { + double newTimestamp = HALUtil.getFPGATime(); + Rotation2d newPosition = Rotation2d.fromRadians(gyro.getRotation3d().getZ()); + + synchronized (this) + { + if (!firstCycle) + { + velocityFilter.addValue((newPosition.minus(position).getRadians()) / (newTimestamp - timestamp)); + } + firstCycle = false; + timestamp = newTimestamp; + position = newPosition; + } + } + + /** + * Get the robot's angular velocity based on averaged meaasurements from the IMU. Velocity is multiplied by 1e+6 + * (1,000,000) because the timestamps are in microseconds. + * + * @return robot's angular velocity in rads/s as a double. + */ + public synchronized double getVelocity() + { + velocity = velocityFilter.calculate(); + return velocity * 1e+6; + } +} diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index 05c50a3..2d6454e 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -11,7 +11,7 @@ import swervelib.telemetry.Alert; /** - * Communicates with the NavX as the IMU. + * Communicates with the NavX({@link AHRS}) as the IMU. */ public class NavXSwerve extends SwerveIMU { @@ -34,7 +34,7 @@ public class NavXSwerve extends SwerveIMU private Alert navXError; /** - * Constructor for the NavX swerve. + * Constructor for the NavX({@link AHRS}) swerve. * * @param port Serial Port to connect to. */ @@ -57,7 +57,7 @@ public NavXSwerve(SerialPort.Port port) } /** - * Constructor for the NavX swerve. + * Constructor for the NavX({@link AHRS}) swerve. * * @param port SPI Port to connect to. */ @@ -79,7 +79,7 @@ public NavXSwerve(SPI.Port port) } /** - * Constructor for the NavX swerve. + * Constructor for the NavX({@link AHRS}) swerve. * * @param port I2C Port to connect to. */ @@ -101,7 +101,8 @@ public NavXSwerve(I2C.Port port) } /** - * Reset IMU to factory default. + * Reset offset to current gyro reading. Does not call NavX({@link AHRS#reset()}) because it has been reported to be + * too slow. */ @Override public void factoryDefault() @@ -179,14 +180,16 @@ public Optional getAccel() /** * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty. + * * @return {@link Double} of the rotation rate as an {@link Optional}. */ - public double getRate() { + public double getRate() + { return imu.getRate(); } /** - * Get the instantiated IMU object. + * Get the instantiated NavX({@link AHRS}) IMU object. * * @return IMU object. */ diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java index 6498dba..574da14 100644 --- a/src/main/java/swervelib/imu/Pigeon2Swerve.java +++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java @@ -10,7 +10,7 @@ import java.util.Optional; /** - * SwerveIMU interface for the Pigeon2 + * SwerveIMU interface for the {@link Pigeon2} */ public class Pigeon2Swerve extends SwerveIMU { @@ -18,36 +18,41 @@ public class Pigeon2Swerve extends SwerveIMU /** * Wait time for status frames to show up. */ - public static double STATUS_TIMEOUT_SECONDS = 0.04; + public static double STATUS_TIMEOUT_SECONDS = 0.04; /** - * Pigeon2 IMU device. + * {@link Pigeon2} IMU device. */ - Pigeon2 imu; + private final Pigeon2 imu; /** - * Offset for the Pigeon 2. + * Offset for the {@link Pigeon2}. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; + /** + * {@link Pigeon2} configurator. + */ + private Pigeon2Configurator cfg; /** - * Generate the SwerveIMU for pigeon. + * Generate the SwerveIMU for {@link Pigeon2}. * - * @param canid CAN ID for the pigeon - * @param canbus CAN Bus name the pigeon resides on. + * @param canid CAN ID for the {@link Pigeon2} + * @param canbus CAN Bus name the {@link Pigeon2} resides on. */ public Pigeon2Swerve(int canid, String canbus) { imu = new Pigeon2(canid, canbus); + this.cfg = imu.getConfigurator(); SmartDashboard.putData(imu); } /** - * Generate the SwerveIMU for pigeon. + * Generate the SwerveIMU for {@link Pigeon2}. * - * @param canid CAN ID for the pigeon + * @param canid CAN ID for the {@link Pigeon2} */ public Pigeon2Swerve(int canid) { @@ -55,12 +60,11 @@ public Pigeon2Swerve(int canid) } /** - * Reset IMU to factory default. + * Reset {@link Pigeon2} to factory default. */ @Override public void factoryDefault() { - Pigeon2Configurator cfg = imu.getConfigurator(); Pigeon2Configuration config = new Pigeon2Configuration(); // Compass utilization causes readings to jump dramatically in some cases. @@ -68,7 +72,7 @@ public void factoryDefault() } /** - * Clear sticky faults on IMU. + * Clear sticky faults on {@link Pigeon2}. */ @Override public void clearStickyFaults() @@ -140,14 +144,16 @@ public Optional getAccel() /** * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty. + * * @return {@link Double} of the rotation rate as an {@link Optional}. */ - public double getRate() { + public double getRate() + { return imu.getRate(); } /** - * Get the instantiated IMU object. + * Get the instantiated {@link Pigeon2} object. * * @return IMU object. */ diff --git a/src/main/java/swervelib/imu/PigeonSwerve.java b/src/main/java/swervelib/imu/PigeonSwerve.java index 25e12fb..e2dfafe 100644 --- a/src/main/java/swervelib/imu/PigeonSwerve.java +++ b/src/main/java/swervelib/imu/PigeonSwerve.java @@ -8,28 +8,28 @@ import java.util.Optional; /** - * SwerveIMU interface for the Pigeon. + * SwerveIMU interface for the {@link WPI_PigeonIMU}. */ public class PigeonSwerve extends SwerveIMU { /** - * Pigeon v1 IMU device. + * {@link WPI_PigeonIMU} IMU device. */ - WPI_PigeonIMU imu; + private final WPI_PigeonIMU imu; /** - * Offset for the Pigeon. + * Offset for the {@link WPI_PigeonIMU}. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** - * Generate the SwerveIMU for pigeon. + * Generate the SwerveIMU for {@link WPI_PigeonIMU}. * - * @param canid CAN ID for the pigeon, does not support CANBus. + * @param canid CAN ID for the {@link WPI_PigeonIMU}, does not support CANBus. */ public PigeonSwerve(int canid) { @@ -115,16 +115,18 @@ public Optional getAccel() return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0)); } - /** + /** * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty. + * * @return {@link Double} of the rotation rate as an {@link Optional}. */ - public double getRate() { + public double getRate() + { return imu.getRate(); } /** - * Get the instantiated IMU object. + * Get the instantiated {@link WPI_PigeonIMU} IMU object. * * @return IMU object. */ diff --git a/src/main/java/swervelib/imu/SwerveIMU.java b/src/main/java/swervelib/imu/SwerveIMU.java index ca2b234..1b1e5ec 100644 --- a/src/main/java/swervelib/imu/SwerveIMU.java +++ b/src/main/java/swervelib/imu/SwerveIMU.java @@ -58,6 +58,7 @@ public abstract class SwerveIMU /** * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty. + * * @return {@link Double} of the rotation rate as an {@link Optional}. */ public abstract double getRate(); diff --git a/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java b/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java new file mode 100644 index 0000000..09509b7 --- /dev/null +++ b/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java @@ -0,0 +1,57 @@ +package swervelib.math; + +import edu.wpi.first.util.DoubleCircularBuffer; + +/** + * A linear filter that does not calculate() each time a value is added to the DoubleCircularBuffer. + */ +public class IMULinearMovingAverageFilter +{ + + /** + * Circular buffer storing the current IMU readings + */ + private final DoubleCircularBuffer m_inputs; + /** + * Gain on each reading. + */ + private final double m_inputGain; + + /** + * Construct a linear moving average fitler + * + * @param bufferLength The number of values to average across + */ + public IMULinearMovingAverageFilter(int bufferLength) + { + m_inputs = new DoubleCircularBuffer(bufferLength); + m_inputGain = 1.0 / bufferLength; + } + + /** + * Add a value to the DoubleCircularBuffer + * + * @param input Value to add + */ + public void addValue(double input) + { + m_inputs.addFirst(input); + } + + /** + * Calculate the average of the samples in the buffer + * + * @return The average of the values in the buffer + */ + public double calculate() + { + double returnVal = 0.0; + + for (int i = 0; i < m_inputs.size(); i++) + { + returnVal += m_inputs.get(i) * m_inputGain; + } + + return returnVal; + } +} diff --git a/src/main/java/swervelib/math/SwerveMath.java b/src/main/java/swervelib/math/SwerveMath.java index ca278a9..b77838c 100644 --- a/src/main/java/swervelib/math/SwerveMath.java +++ b/src/main/java/swervelib/math/SwerveMath.java @@ -136,7 +136,7 @@ public static double calculateDegreesPerSteeringRotation( public static double calculateMaxAngularVelocity( double maxSpeed, double furthestModuleX, double furthestModuleY) { - return maxSpeed / new Rotation2d(furthestModuleX, furthestModuleY).getRadians(); + return maxSpeed / Math.hypot(furthestModuleX, furthestModuleY); } /** diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index 0c96826..400de96 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -12,6 +12,7 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkPIDController; +import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; @@ -25,9 +26,9 @@ public class SparkFlexSwerve extends SwerveMotor { /** - * SparkMAX Instance. + * {@link CANSparkFlex} Instance. */ - public CANSparkFlex motor; + private final CANSparkFlex motor; /** * Integrated encoder. */ @@ -108,6 +109,7 @@ private void configureSparkFlex(Supplier config) { return; } + Timer.delay(0.01); } failureConfiguring.set(true); } @@ -200,7 +202,11 @@ public void clearStickyFaults() @Override public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + if (encoder == null) + { + absoluteEncoder = null; + configureSparkFlex(() -> pid.setFeedbackDevice(this.encoder)); + } else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { absoluteEncoderOffsetWarning.set(true); absoluteEncoder = encoder; @@ -325,7 +331,10 @@ public void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { - motor.setInverted(inverted); + configureSparkFlex(() -> { + motor.setInverted(inverted); + return motor.getLastError(); + }); } /** diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index bb33fc4..f384352 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -11,13 +11,14 @@ import com.revrobotics.SparkMaxAlternateEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkRelativeEncoder.Type; +import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; import swervelib.telemetry.Alert; /** - * Brushed motor control with SparkMax. + * Brushed motor control with {@link CANSparkMax}. */ public class SparkMaxBrushedMotorSwerve extends SwerveMotor { @@ -25,7 +26,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * SparkMAX Instance. */ - public CANSparkMax motor; + private final CANSparkMax motor; /** * Absolute encoder attached to the SparkMax (if exists) @@ -145,6 +146,7 @@ private void configureSparkMax(Supplier config) { return; } + Timer.delay(0.01); } failureConfiguringAlert.set(true); } @@ -237,7 +239,11 @@ public void clearStickyFaults() @Override public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) + if (encoder == null) + { + absoluteEncoder = null; + configureSparkMax(() -> pid.setFeedbackDevice(this.encoder)); + } else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder(); configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder)); @@ -347,7 +353,10 @@ public void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { - motor.setInverted(inverted); + configureSparkMax(() -> { + motor.setInverted(inverted); + return motor.getLastError(); + }); } /** diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index aa19a0c..1479571 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -12,6 +12,7 @@ import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkPIDController; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; @@ -24,33 +25,33 @@ public class SparkMaxSwerve extends SwerveMotor { /** - * SparkMAX Instance. + * {@link CANSparkMax} Instance. */ - public CANSparkMax motor; + private final CANSparkMax motor; /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Absolute encoder attached to the SparkMax (if exists) */ - public SwerveAbsoluteEncoder absoluteEncoder; + public SwerveAbsoluteEncoder absoluteEncoder; /** * Closed-loop PID controller. */ - public SparkPIDController pid; + public SparkPIDController pid; /** * Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; + private boolean factoryDefaultOccurred = false; /** * Supplier for the velocity of the motor controller. */ - private Supplier velocity; + private Supplier velocity; /** * Supplier for the position of the motor controller. */ - private Supplier position; + private Supplier position; /** * Initialize the swerve motor. @@ -100,6 +101,7 @@ private void configureSparkMax(Supplier config) { return; } + Timer.delay(0.01); } DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); } @@ -192,10 +194,16 @@ public void clearStickyFaults() @Override public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + if (encoder == null) + { + absoluteEncoder = null; + configureSparkMax(() -> pid.setFeedbackDevice(this.encoder)); + velocity = this.encoder::getVelocity; + position = this.encoder::getPosition; + } else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { DriverStation.reportWarning( - "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" + + "IF possible configure the encoder offset in the REV Hardware Client instead of using the" + " absoluteEncoderOffset in the Swerve Module JSON!", false); absoluteEncoder = encoder; @@ -218,12 +226,38 @@ public void configureIntegratedEncoder(double positionConversionFactor) { configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor)); configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); + // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller + // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement) + // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag + // https://www.chiefdelphi.com/t/shooter-encoder/400211/11 + // This value was taken from: + // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133 + // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches + configureSparkMax(() -> encoder.setMeasurementPeriod(10)); + configureSparkMax(() -> encoder.setAverageDepth(2)); // Taken from // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 + // Unused frames can be set to 65535 to decrease CAN ultilization. configureCANStatusFrames(10, 20, 20, 500, 500, 200, 200); } else { + // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5 + // This needs to be set to 20ms or under to properly update the swerve module position for odometry + // Configuration taken from 3005, the team who helped develop the Max Swerve: + // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244 + // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max. + // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill, + // with limited testing 19ms did not return the same value while the module was constatntly rotating. + if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) + { + configureCANStatusFrames(100, 20, 20, 200, 20, 8, 50); + } + // Need to test on analog encoders but the same concept should apply for them, worst thing that could happen is slightly more can use + else if (absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor) + { + configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200); + } configureSparkMax(() -> { if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { @@ -325,7 +359,10 @@ public void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { - motor.setInverted(inverted); + configureSparkMax(() -> { + motor.setInverted(inverted); + return motor.getLastError(); + }); } /** diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 213f199..233a868 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -40,7 +40,7 @@ public class TalonFXSwerve extends SwerveMotor /** * TalonFX motor controller. */ - TalonFX motor; + private final TalonFX motor; /** * Conversion factor for the motor. */ @@ -49,6 +49,10 @@ public class TalonFXSwerve extends SwerveMotor * Current TalonFX configuration. */ private TalonFXConfiguration configuration = new TalonFXConfiguration(); + /** + * Current TalonFX Configurator. + */ + private TalonFXConfigurator cfg; /** @@ -61,6 +65,7 @@ public TalonFXSwerve(TalonFX motor, boolean isDriveMotor) { this.isDriveMotor = isDriveMotor; this.motor = motor; + this.cfg = motor.getConfigurator(); factoryDefaults(); clearStickyFaults(); @@ -102,7 +107,6 @@ public void factoryDefaults() { if (!factoryDefaultOccurred) { - TalonFXConfigurator cfg = motor.getConfigurator(); configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; configuration.ClosedLoopGeneral.ContinuousWrap = true; cfg.apply(configuration); @@ -156,7 +160,6 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) @Override public void configureIntegratedEncoder(double positionConversionFactor) { - TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration); positionConversionFactor = 1 / positionConversionFactor; @@ -246,7 +249,6 @@ public void configureCANStatusFrames( public void configurePIDF(PIDFConfig config) { - TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.Slot0); cfg.apply( configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f)); @@ -263,7 +265,6 @@ public void configurePIDF(PIDFConfig config) @Override public void configurePIDWrapping(double minInput, double maxInput) { - TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.ClosedLoopGeneral); configuration.ClosedLoopGeneral.ContinuousWrap = true; cfg.apply(configuration.ClosedLoopGeneral); @@ -414,7 +415,6 @@ public void setPosition(double position) if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) { position = position < 0 ? (position % 360) + 360 : position; - TalonFXConfigurator cfg = motor.getConfigurator(); cfg.setPosition(position / 360); } } @@ -439,7 +439,6 @@ public void setVoltageCompensation(double nominalVoltage) @Override public void setCurrentLimit(int currentLimit) { - TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.CurrentLimits); cfg.apply( configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) @@ -454,7 +453,6 @@ public void setCurrentLimit(int currentLimit) @Override public void setLoopRampRate(double rampRate) { - TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.ClosedLoopRamps); cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate)); } diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java index 30db007..174e81c 100644 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java @@ -33,7 +33,7 @@ public class TalonSRXSwerve extends SwerveMotor /** * TalonSRX motor controller. */ - WPI_TalonSRX motor; + private final WPI_TalonSRX motor; /** * The position conversion factor to convert raw sensor units to Meters Per 100ms, or Ticks to Degrees. */ @@ -358,11 +358,6 @@ public double getPosition() } else { var pos = motor.getSelectedSensorPosition() * positionConversionFactor; - pos %= 360; - if (pos < 360) - { - pos += 360; - } return pos; } } @@ -421,6 +416,17 @@ public void setLoopRampRate(double rampRate) configChanged = true; } + /** + * Set the selected feedback device for the TalonSRX. + * + * @param feedbackDevice Feedback device to select. + */ + public void setSelectedFeedbackDevice(FeedbackDevice feedbackDevice) + { + configuration.primaryPID.selectedFeedbackSensor = feedbackDevice; + configChanged = true; + } + /** * Get the motor object from the module. * diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 0d30b82..c792e63 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj.SerialPort.Port; import swervelib.encoders.AnalogAbsoluteEncoderSwerve; import swervelib.encoders.CANCoderSwerve; -import swervelib.encoders.CanAndCoderSwerve; +import swervelib.encoders.CanAndMagSwerve; import swervelib.encoders.PWMDutyCycleEncoderSwerve; import swervelib.encoders.SparkMaxAnalogEncoderSwerve; import swervelib.encoders.SparkMaxEncoderSwerve; @@ -23,6 +23,7 @@ import swervelib.imu.ADIS16470Swerve; import swervelib.imu.ADXRS450Swerve; import swervelib.imu.AnalogGyroSwerve; +import swervelib.imu.CanandgyroSwerve; import swervelib.imu.NavXSwerve; import swervelib.imu.Pigeon2Swerve; import swervelib.imu.PigeonSwerve; @@ -72,13 +73,16 @@ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) return null; case "integrated": case "attached": - return new SparkMaxEncoderSwerve(motor, 1); - case "sparkmax_analog": - return new SparkMaxAnalogEncoderSwerve(motor); + case "canandmag": case "canandcoder": return new SparkMaxEncoderSwerve(motor, 360); + case "sparkmax_analog": + return new SparkMaxAnalogEncoderSwerve(motor, 3.3); + case "sparkmax_analog5v": + return new SparkMaxAnalogEncoderSwerve(motor, 5); case "canandcoder_can": - return new CanAndCoderSwerve(id); + case "canandmag_can": + return new CanAndMagSwerve(id); case "ctre_mag": case "rev_hex": case "throughbore": @@ -121,6 +125,8 @@ public SwerveIMU createIMU() return new ADXRS450Swerve(); case "analog": return new AnalogGyroSwerve(id); + case "canandgyro": + return new CanandgyroSwerve(id); case "navx": case "navx_spi": return new NavXSwerve(SPI.Port.kMXP); diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index 5eb3471..2fa83c8 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -81,15 +81,6 @@ public SwerveModuleConfiguration createModuleConfiguration( SwerveMotor angleMotor = angle.createMotor(false); SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor); - // If the absolute encoder is attached. - if (absEncoder != null && angleMotor.getMotor() instanceof CANSparkMax) - { - if (absEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) - { - angleMotor.setAbsoluteEncoder(absEncoder); - } - } - // Setup deprecation notice. // if (this.conversionFactor.drive != 0 && this.conversionFactor.angle != 0) // { @@ -147,6 +138,13 @@ public SwerveModuleConfiguration createModuleConfiguration( "Conversion factors cannot be 0, please configure conversion factors in physicalproperties.json or the module JSON files."); } + // Backwards compatibility, auto-optimization. + if (conversionFactor.angle == 360 && absEncoder != null && + absEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor && angleMotor.getMotor() instanceof CANSparkMax) + { + angleMotor.setAbsoluteEncoder(absEncoder); + } + return new SwerveModuleConfiguration( drive.createMotor(true), angleMotor, diff --git a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java index 26094d3..3c90fc6 100644 --- a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java @@ -41,7 +41,7 @@ public class SwerveDriveTelemetry */ public static int moduleCount; /** - * The number of swerve modules + * The Locations of the swerve drive wheels. */ public static double[] wheelLocations; /** diff --git a/vendordeps/ReduxLib_2024.json b/vendordeps/ReduxLib_2024.json index 53bc91d..541b062 100644 --- a/vendordeps/ReduxLib_2024.json +++ b/vendordeps/ReduxLib_2024.json @@ -1,7 +1,7 @@ { "fileName": "ReduxLib_2024.json", "name": "ReduxLib", - "version": "2024.1.1", + "version": "2024.3.2", "frcYear": 2024, "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-java", - "version": "2024.1.1" + "version": "2024.3.2" } ], "jniDependencies": [ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-driver", - "version": "2024.1.1", + "version": "2024.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-cpp", - "version": "2024.1.1", + "version": "2024.3.2", "libName": "ReduxLib-cpp", "headerClassifier": "headers", "sourcesClassifier": "sources",