From 8f0ed77b6ca8cda3abf772bc6148522d13c4d718 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Fri, 26 Jan 2024 11:26:21 -0600 Subject: [PATCH] Added SwerveDrive.getGyro(). reformatted code. Signed-off-by: thenetworkgrinch --- src/main/java/swervelib/SwerveDrive.java | 48 +++++++++++-------- src/main/java/swervelib/SwerveModule.java | 3 +- .../parser/SwerveModuleConfiguration.java | 2 +- .../swervelib/parser/json/ModuleJson.java | 2 +- 4 files changed, 32 insertions(+), 23 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index f1d35a02..96e165b6 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -451,8 +451,7 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent { velocity.omegaRadiansPerSecond = swerveController.headingCalculate(getOdometryHeading().getRadians(), lastHeadingRadians); - } - else + } else { lastHeadingRadians = getOdometryHeading().getRadians(); } @@ -679,6 +678,33 @@ public SwerveModulePosition[] getModulePositions() return positions; } + /** + * Getter for the {@link SwerveIMU}. + * + * @return generated {@link SwerveIMU} + */ + public SwerveIMU getGyro() + { + return swerveDriveConfiguration.imu; + } + + /** + * Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d} + * subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}. + * + * @param gyro expected gyroscope angle as {@link Rotation3d}. + */ + public void setGyro(Rotation3d gyro) + { + if (SwerveDriveTelemetry.isSimulation) + { + setGyroOffset(simIMU.getGyroRotation3d().minus(gyro)); + } else + { + setGyroOffset(imu.getRawRotation3d().minus(gyro)); + } + } + /** * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. */ @@ -1042,24 +1068,6 @@ public void addVisionMeasurement(Pose2d robotPose, double timestamp) // resetOdometry(newOdometry); } - - /** - * Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d} - * subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}. - * - * @param gyro expected gyroscope angle as {@link Rotation3d}. - */ - public void setGyro(Rotation3d gyro) - { - if (SwerveDriveTelemetry.isSimulation) - { - setGyroOffset(simIMU.getGyroRotation3d().minus(gyro)); - } else - { - setGyroOffset(imu.getRawRotation3d().minus(gyro)); - } - } - /** * Helper function to get the {@link SwerveDrive#swerveController} for the {@link SwerveDrive} which can be used to * generate {@link ChassisSpeeds} for the robot to orient it correctly given axis or angles, and apply diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index bcc3eb6a..260cdf06 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -205,7 +205,8 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, } else { double cosineScalar = 1.0; - if (configuration.useCosineCompensator) { + if (configuration.useCosineCompensator) + { // Taken from the CTRE SwerveModule class. // https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46 /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */ diff --git a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java index 0fbe0553..24d06257 100644 --- a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java +++ b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java @@ -164,7 +164,7 @@ public SwerveModuleConfiguration( false, false, false, - name, + name, useCosineCompensator); } diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index 9b9e6aae..b9f24043 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -56,7 +56,7 @@ public class ModuleJson /** * Should do cosine compensation when not pointing correct direction;. */ - public boolean useCosineCompensator = true; + public boolean useCosineCompensator = true; /** * Create the swerve module configuration based off of parsed data.