From 039e5c2867690cfdd5ebd0c1e84eefc6b165adee Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Mon, 22 Jan 2024 14:27:43 -0600 Subject: [PATCH] Improved inverted IMU's using ternary operations. Removed debug parameter. Removed redundant inversion from SwerveDriveConfiguration. Added getOdometryHeading. Utilize getOdometryHeading alot within SwerveDrive. Fixed SwerveSubsystem. Fixed vision measurement issue. Removed dedicated visionStdDevs and stateStdDevs. Signed-off-by: thenetworkgrinch --- build.gradle | 2 +- simgui.json | 6 + src/main/java/swervelib/SwerveDrive.java | 129 ++++++++---------- .../java/swervelib/imu/ADIS16448Swerve.java | 18 +-- .../java/swervelib/imu/ADIS16470Swerve.java | 12 +- .../java/swervelib/imu/ADXRS450Swerve.java | 12 +- .../java/swervelib/imu/AnalogGyroSwerve.java | 12 +- src/main/java/swervelib/imu/NavXSwerve.java | 11 +- .../java/swervelib/imu/Pigeon2Swerve.java | 24 ++-- src/main/java/swervelib/imu/PigeonSwerve.java | 12 +- src/main/java/swervelib/imu/SwerveIMU.java | 6 +- .../java/swervelib/motors/TalonFXSwerve.java | 8 +- .../parser/SwerveDriveConfiguration.java | 12 +- src/main/java/swervelib/telemetry/Alert.java | 11 +- 14 files changed, 124 insertions(+), 151 deletions(-) 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. */