diff --git a/build.gradle b/build.gradle index 5b29be23..c07a40e3 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" } java { diff --git a/simgui.json b/simgui.json index 4eccb966..21efb849 100644 --- a/simgui.json +++ b/simgui.json @@ -85,6 +85,12 @@ "arrowWeight": 3.0, "style": "Hidden" }, + "bottom": 1476, + "height": 8.210550308227539, + "left": 150, + "right": 2961, + "top": 79, + "width": 16.541748046875, "window": { "visible": true } diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 6a699513..9ee23523 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -1,7 +1,6 @@ package swervelib; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.filter.SlewRateLimiter; @@ -72,9 +71,12 @@ public class SwerveDrive */ private final Lock odometryLock = new ReentrantLock(); /** - * Deadband for speeds in heading correction. + * Alert to recommend Tuner X if the configuration is compatible. */ - private double HEADING_CORRECTION_DEADBAND = 0.01; + private final Alert tunerXRecommendation = new Alert("Swerve Drive", + "Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" + + "https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html", + AlertType.WARNING); /** * Field object. */ @@ -83,26 +85,6 @@ public class SwerveDrive * Swerve controller for controlling heading of the robot. */ public SwerveController swerveController; - /** - * Standard deviation of encoders and gyroscopes, usually should not change. (meters of position and degrees of - * rotation) - */ - public Matrix stateStdDevs = VecBuilder.fill(0.1, - 0.1, - 0.1); - /** - * The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more - * points and fit a line to it and modify this using {@link SwerveDrive#addVisionMeasurement(Pose2d, double, Matrix)} - * with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get - * vision accurate to inches instead of feet. - */ - public Matrix visionMeasurementStdDevs = VecBuilder.fill(0.9, - 0.9, - 0.9); - /** - * Invert odometry readings of drive motor positions, used as a patch for debugging currently. - */ - public boolean invertOdometry = false; /** * Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's * correction. @@ -112,6 +94,10 @@ public class SwerveDrive * Whether to correct heading when driving translationally. Set to true to enable. */ public boolean headingCorrection = false; + /** + * Deadband for speeds in heading correction. + */ + private double HEADING_CORRECTION_DEADBAND = 0.01; /** * Whether heading correction PID is currently active. */ @@ -144,13 +130,6 @@ public class SwerveDrive * Maximum speed of the robot in meters per second. */ private double maxSpeedMPS; - /** - * Alert to recommend Tuner X if the configuration is compatible. - */ - private final Alert tunerXRecommendation = new Alert("Swerve Drive", - "Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" + - "https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html", - AlertType.WARNING); /** * Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the @@ -194,9 +173,8 @@ public SwerveDrive( kinematics, getYaw(), getModulePositions(), - new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)), - stateStdDevs, - visionMeasurementStdDevs); // x,y,heading in radians; Vision measurement std dev, higher=less weight + new Pose2d(new Translation2d(0, 0), + Rotation2d.fromDegrees(0))); // x,y,heading in radians; Vision measurement std dev, higher=less weight zeroGyro(); @@ -308,6 +286,16 @@ public void setDriveMotorConversionFactor(double conversionFactor) } } + /** + * Fetch the latest odometry heading, should be trusted over {@link SwerveDrive#getYaw()}. + * + * @return {@link Rotation2d} of the robot heading. + */ + public Rotation2d getOdometryHeading() + { + return swerveDrivePoseEstimator.getEstimatedPosition().getRotation(); + } + /** * Set the heading correction capabilities of YAGSL. * @@ -337,7 +325,7 @@ public void setHeadingCorrection(boolean state, double deadband) */ public void driveFieldOriented(ChassisSpeeds velocity) { - ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw()); + ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading()); drive(fieldOrientedVelocity); } @@ -349,7 +337,7 @@ public void driveFieldOriented(ChassisSpeeds velocity) */ public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters) { - ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw()); + ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading()); drive(fieldOrientedVelocity, centerOfRotationMeters); } @@ -401,7 +389,7 @@ public void drive( ChassisSpeeds velocity = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( - translation.getX(), translation.getY(), rotation, getYaw()) + translation.getX(), translation.getY(), rotation, getOdometryHeading()) : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); drive(velocity, isOpenLoop, centerOfRotationMeters); @@ -430,7 +418,7 @@ public void drive( ChassisSpeeds velocity = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( - translation.getX(), translation.getY(), rotation, getYaw()) + translation.getX(), translation.getY(), rotation, getOdometryHeading()) : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); drive(velocity, isOpenLoop, new Translation2d()); @@ -466,11 +454,11 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent { if (!correctionEnabled) { - lastHeadingRadians = getYaw().getRadians(); + lastHeadingRadians = getOdometryHeading().getRadians(); correctionEnabled = true; } velocity.omegaRadiansPerSecond = - swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians()); + swerveController.headingCalculate(lastHeadingRadians, getOdometryHeading().getRadians()); } else { correctionEnabled = false; @@ -623,7 +611,7 @@ public ChassisSpeeds getFieldVelocity() // angle // given as the robot angle reverses the direction of rotation, and the conversion is reversed. return ChassisSpeeds.fromFieldRelativeSpeeds( - kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus()); + kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus()); } /** @@ -680,8 +668,7 @@ public SwerveModuleState[] getStates() } /** - * Gets the current module positions (azimuth and wheel position (meters)). Inverts the distance from each module if - * {@link #invertOdometry} is true. + * Gets the current module positions (azimuth and wheel position (meters)). * * @return A list of SwerveModulePositions containg the current module positions */ @@ -692,10 +679,6 @@ public SwerveModulePosition[] getModulePositions() for (SwerveModule module : swerveModules) { positions[module.moduleNumber] = module.getPosition(); - if (invertOdometry) - { - positions[module.moduleNumber].distanceMeters *= -1; - } } return positions; } @@ -945,7 +928,7 @@ public void updateOdometry() SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond; SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond; SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond); - SwerveDriveTelemetry.robotRotation = getYaw().getDegrees(); + SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees(); } if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) @@ -961,7 +944,8 @@ public void updateOdometry() if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) { module.updateTelemetry(); - SmartDashboard.putNumber("Adjusted IMU Yaw", getYaw().getDegrees()); + SmartDashboard.putNumber("Raw IMU Yaw", getYaw().getDegrees()); + SmartDashboard.putNumber("Adjusted IMU Yaw", getOdometryHeading().getDegrees()); } if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { @@ -1019,28 +1003,6 @@ public void setGyroOffset(Rotation3d offset) } } - /** - * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with - * the given timestamp of the vision measurement.
IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET - * AFTER USING THIS FUNCTION!
To update your gyroscope readings directly use - * {@link SwerveDrive#setGyroOffset(Rotation3d)}. - * - * @param robotPose Robot {@link Pose2d} as measured by vision. - * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from - * {@link Timer#getFPGATimestamp()} or similar sources. - */ - public void addVisionMeasurement(Pose2d robotPose, double timestamp) - { - odometryLock.lock(); - swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp); - Pose2d newOdometry = new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(), - robotPose.getRotation()); - odometryLock.unlock(); - - setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians())); - resetOdometry(newOdometry); - } - /** * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with * the given timestamp of the vision measurement. @@ -1058,8 +1020,31 @@ public void addVisionMeasurement(Pose2d robotPose, double timestamp) public void addVisionMeasurement(Pose2d robotPose, double timestamp, Matrix visionMeasurementStdDevs) { - this.visionMeasurementStdDevs = visionMeasurementStdDevs; - addVisionMeasurement(robotPose, timestamp); + odometryLock.lock(); + swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp, 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.
IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET + * AFTER USING THIS FUNCTION!
To update your gyroscope readings directly use + * {@link SwerveDrive#setGyroOffset(Rotation3d)}. + * + * @param robotPose Robot {@link Pose2d} as measured by vision. + * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from + * {@link Timer#getFPGATimestamp()} or similar sources. + */ + public void addVisionMeasurement(Pose2d robotPose, double timestamp) + { + odometryLock.lock(); + swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp); +// Pose2d newOdometry = new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(), +// robotPose.getRotation()); + odometryLock.unlock(); + +// setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians())); +// resetOdometry(newOdometry); } diff --git a/src/main/java/swervelib/imu/ADIS16448Swerve.java b/src/main/java/swervelib/imu/ADIS16448Swerve.java index f70db8f1..4295d483 100644 --- a/src/main/java/swervelib/imu/ADIS16448Swerve.java +++ b/src/main/java/swervelib/imu/ADIS16448Swerve.java @@ -19,11 +19,11 @@ public class ADIS16448Swerve extends SwerveIMU /** * Offset for the ADIS16448. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** * Construct the ADIS16448 imu and reset default configurations. Publish the gyro to the SmartDashboard. @@ -66,7 +66,7 @@ public void setOffset(Rotation3d offset) /** * Set the gyro to invert its default direction - * + * * @param invertIMU invert gyro direction */ public void setInverted(boolean invertIMU) @@ -81,14 +81,10 @@ public void setInverted(boolean invertIMU) */ public Rotation3d getRawRotation3d() { - if(invertedIMU){ - return new Rotation3d(Math.toRadians(-imu.getGyroAngleX()), - Math.toRadians(-imu.getGyroAngleY()), - Math.toRadians(-imu.getGyroAngleZ())).unaryMinus(); - } - return new Rotation3d(Math.toRadians(-imu.getGyroAngleX()), - Math.toRadians(-imu.getGyroAngleY()), - Math.toRadians(-imu.getGyroAngleZ())); + Rotation3d reading = new Rotation3d(Math.toRadians(-imu.getGyroAngleX()), + Math.toRadians(-imu.getGyroAngleY()), + Math.toRadians(-imu.getGyroAngleZ())); + return invertedIMU ? reading.unaryMinus() : reading; } /** diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.java index 4dceb25c..94a31b76 100644 --- a/src/main/java/swervelib/imu/ADIS16470Swerve.java +++ b/src/main/java/swervelib/imu/ADIS16470Swerve.java @@ -20,11 +20,11 @@ public class ADIS16470Swerve extends SwerveIMU /** * Offset for the ADIS16470. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** * Construct the ADIS16470 imu and reset default configurations. Publish the gyro to the SmartDashboard. @@ -68,7 +68,7 @@ public void setOffset(Rotation3d offset) /** * Set the gyro to invert its default direction - * + * * @param invertIMU invert gyro direction */ public void setInverted(boolean invertIMU) @@ -83,10 +83,8 @@ public void setInverted(boolean invertIMU) */ public Rotation3d getRawRotation3d() { - if(invertedIMU){ - return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw))).unaryMinus(); - } - return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw))); + Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw))); + return invertedIMU ? reading.unaryMinus() : reading; } /** diff --git a/src/main/java/swervelib/imu/ADXRS450Swerve.java b/src/main/java/swervelib/imu/ADXRS450Swerve.java index 3aaf5858..718a8966 100644 --- a/src/main/java/swervelib/imu/ADXRS450Swerve.java +++ b/src/main/java/swervelib/imu/ADXRS450Swerve.java @@ -19,11 +19,11 @@ public class ADXRS450Swerve extends SwerveIMU /** * Offset for the ADXRS450. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** * Construct the ADXRS450 imu and reset default configurations. Publish the gyro to the SmartDashboard. @@ -66,7 +66,7 @@ public void setOffset(Rotation3d offset) /** * Set the gyro to invert its default direction - * + * * @param invertIMU invert gyro direction */ public void setInverted(boolean invertIMU) @@ -81,10 +81,8 @@ public void setInverted(boolean invertIMU) */ public Rotation3d getRawRotation3d() { - if(invertedIMU){ - return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle())).unaryMinus(); - } - return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle())); + Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-imu.getAngle())); + return invertedIMU ? reading.unaryMinus() : reading; } /** diff --git a/src/main/java/swervelib/imu/AnalogGyroSwerve.java b/src/main/java/swervelib/imu/AnalogGyroSwerve.java index b99a1a64..d104a0ff 100644 --- a/src/main/java/swervelib/imu/AnalogGyroSwerve.java +++ b/src/main/java/swervelib/imu/AnalogGyroSwerve.java @@ -19,11 +19,11 @@ public class AnalogGyroSwerve extends SwerveIMU /** * Offset for the analog gyro. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** * Analog port in which the gyroscope is connected. Can only be attached to analog ports 0 or 1. @@ -73,7 +73,7 @@ public void setOffset(Rotation3d offset) /** * Set the gyro to invert its default direction - * + * * @param invertIMU invert gyro direction */ public void setInverted(boolean invertIMU) @@ -88,10 +88,8 @@ public void setInverted(boolean invertIMU) */ public Rotation3d getRawRotation3d() { - if(invertedIMU){ - return new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle())).unaryMinus(); - } - return new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle())); + Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle())); + return invertedIMU ? reading.unaryMinus() : reading; } /** diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index 74f5b9f7..790d6b28 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -23,11 +23,11 @@ public class NavXSwerve extends SwerveIMU /** * Offset for the NavX. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** * An {@link Alert} for if there is an error instantiating the NavX. */ @@ -130,7 +130,7 @@ public void setOffset(Rotation3d offset) /** * Set the gyro to invert its default direction - * + * * @param invertIMU invert gyro direction */ public void setInverted(boolean invertIMU) @@ -146,10 +146,7 @@ public void setInverted(boolean invertIMU) @Override public Rotation3d getRawRotation3d() { - if(invertedIMU){ - return gyro.getRotation3d().unaryMinus(); - } - return gyro.getRotation3d(); + return invertedIMU ? gyro.getRotation3d().unaryMinus() : gyro.getRotation3d(); } /** diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java index 6b2e4ee9..d68e4f78 100644 --- a/src/main/java/swervelib/imu/Pigeon2Swerve.java +++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java @@ -23,11 +23,11 @@ public class Pigeon2Swerve extends SwerveIMU /** * Offset for the Pigeon 2. */ - 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. @@ -85,7 +85,7 @@ public void setOffset(Rotation3d offset) /** * Set the gyro to invert its default direction - * + * * @param invertIMU invert gyro direction */ public void setInverted(boolean invertIMU) @@ -102,15 +102,15 @@ public void setInverted(boolean invertIMU) public Rotation3d getRawRotation3d() { // TODO: Switch to suppliers. - StatusSignal w = imu.getQuatW(); - StatusSignal x = imu.getQuatX(); - StatusSignal y = imu.getQuatY(); - StatusSignal z = imu.getQuatZ(); - - if(invertedIMU){ - return new Rotation3d(new Quaternion(w.refresh().getValue(), x.refresh().getValue(), y.refresh().getValue(), z.refresh().getValue())).unaryMinus(); - } - return new Rotation3d(new Quaternion(w.refresh().getValue(), x.refresh().getValue(), y.refresh().getValue(), z.refresh().getValue())); + StatusSignal w = imu.getQuatW(); + StatusSignal x = imu.getQuatX(); + StatusSignal y = imu.getQuatY(); + StatusSignal z = imu.getQuatZ(); + Rotation3d reading = new Rotation3d(new Quaternion(w.refresh().getValue(), + x.refresh().getValue(), + y.refresh().getValue(), + z.refresh().getValue())); + return invertedIMU ? reading.unaryMinus() : reading; } /** diff --git a/src/main/java/swervelib/imu/PigeonSwerve.java b/src/main/java/swervelib/imu/PigeonSwerve.java index c010e4fb..b9bbcc5c 100644 --- a/src/main/java/swervelib/imu/PigeonSwerve.java +++ b/src/main/java/swervelib/imu/PigeonSwerve.java @@ -20,11 +20,11 @@ public class PigeonSwerve extends SwerveIMU /** * Offset for the Pigeon. */ - 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. @@ -68,7 +68,7 @@ public void setOffset(Rotation3d offset) /** * Set the gyro to invert its default direction - * + * * @param invertIMU invert gyro direction */ public void setInverted(boolean invertIMU) @@ -86,10 +86,8 @@ public Rotation3d getRawRotation3d() { double[] wxyz = new double[4]; imu.get6dQuaternion(wxyz); - if(invertedIMU){ - return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])).unaryMinus(); - } - return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); + Rotation3d reading = new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); + return invertedIMU ? reading.unaryMinus() : reading; } /** diff --git a/src/main/java/swervelib/imu/SwerveIMU.java b/src/main/java/swervelib/imu/SwerveIMU.java index d7403855..dd9a727a 100644 --- a/src/main/java/swervelib/imu/SwerveIMU.java +++ b/src/main/java/swervelib/imu/SwerveIMU.java @@ -28,9 +28,9 @@ public abstract class SwerveIMU public abstract void setOffset(Rotation3d offset); /** - * Set the gyro to invert its default direction - * - * @param invert gyro direction + * Set the gyro to invert its default direction. + * + * @param invertIMU gyro direction */ public abstract void setInverted(boolean invertIMU); diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 29cdf977..cf7effbe 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -33,14 +33,14 @@ public class TalonFXSwerve extends SwerveMotor * Velocity voltage setter for controlling drive motor. */ private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); - /** - * Conversion factor for the motor. - */ - private double conversionFactor; /** * TalonFX motor controller. */ TalonFX motor; + /** + * Conversion factor for the motor. + */ + private double conversionFactor; /** * Current TalonFX configuration. */ diff --git a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java index 2a4f9b96..e5434de2 100644 --- a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java +++ b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java @@ -19,10 +19,6 @@ public class SwerveDriveConfiguration * Swerve IMU */ public SwerveIMU imu; - /** - * Invert the imu measurements. - */ - public boolean invertedIMU = false; /** * Number of modules on the robot. */ @@ -54,7 +50,6 @@ public SwerveDriveConfiguration( { this.moduleCount = moduleConfigs.length; this.imu = swerveIMU; - this.invertedIMU = invertedIMU; swerveIMU.setInverted(invertedIMU); this.modules = createModules(moduleConfigs, driveFeedforward); this.moduleLocationsMeters = new Translation2d[moduleConfigs.length]; @@ -93,10 +88,11 @@ public double getDriveBaseRadiusMeters() Translation2d centerOfModules = moduleLocationsMeters[0]; //Calculate the Center by adding all module offsets together. - for(int i=1; i groups = new HashMap(); + private static Map groups = new HashMap(); /** * Type of the Alert to raise. */ - private final AlertType type; + private final AlertType type; /** * Activation state of alert. */ - private boolean active = false; + private boolean active = false; /** * When the alert was raised. */ - private double activeStartTime = 0.0; + private double activeStartTime = 0.0; /** * Text of the alert. */ - private String text; + private String text; /** * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, the appropriate @@ -213,6 +213,7 @@ private static class SendableAlerts implements Sendable /** * Get alerts based off of type. + * * @param type Type of alert to fetch. * @return Active alert strings. */