Merge pull request #85 from strykeforce/gyro-offset

Add optional gyro offset to swerve drive

Author: jhh

build.gradle

@@ -9,7 +9,7 @@ plugins {
 }
 
 group = "org.strykeforce"
-version = "21.3.0"
+version = "21.3.1"
 
 sourceCompatibility = JavaVersion.VERSION_11
 targetCompatibility = JavaVersion.VERSION_11

src/main/java/org/strykeforce/swerve/SwerveDrive.java

@@ -27,6 +27,8 @@ public class SwerveDrive {
   private final SwerveDriveOdometry odometry;
   private final Gyro gyro;
   private final double maxSpeedMetersPerSecond;
+  private Rotation2d gyroOffset = new Rotation2d();
+  private boolean hasGyroOffset = false;
 
   /**
    * Construct a swerve drive object. Along with a gyro, this takes in four configured swerve
@@ -60,7 +62,7 @@ public SwerveDrive(Gyro gyro, SwerveModule... swerveModules) {
     maxSpeedMetersPerSecond = swerveModules[0].getMaxSpeedMetersPerSecond();
 
     kinematics = new SwerveDriveKinematics(translation2ds);
-    odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d());
+    odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d().rotateBy(gyroOffset));
   }
 
   /**
@@ -101,7 +103,7 @@ public Pose2d getPoseMeters() {
    * @return the Rotation2d of the robot relative to gyro zero
    */
   public Rotation2d getHeading() {
-    return gyro.getRotation2d();
+    return hasGyroOffset ? gyro.getRotation2d().rotateBy(gyroOffset) : gyro.getRotation2d();
   }
 
   /**
@@ -113,7 +115,8 @@ public Rotation2d getHeading() {
    *
    * @return the current heading in degrees of the robot relative to gyro zero
    */
-  public double getGyroAngle() {
+  double getGyroAngle() {
+    // FIXME: does not have gyro offset
     return gyro.getAngle();
   }
 
@@ -128,6 +131,29 @@ public double getGyroRate() {
     return gyro.getRate();
   }
 
+  /**
+   * Get the current gyro offset applied to the IMU gyro angle during field oriented driving.
+   *
+   * @return the gyro offset
+   */
+  public Rotation2d getGyroOffset() {
+    return gyroOffset;
+  }
+
+  /**
+   * Set the current gyro offset applied to the IMU gyro angle during field oriented driving,
+   * defaults to zero.
+   *
+   * @param gyroOffset the desired offset
+   */
+  public void setGyroOffset(Rotation2d gyroOffset) {
+    if (this.gyroOffset.equals(gyroOffset)) {
+      return;
+    }
+    this.gyroOffset = gyroOffset;
+    hasGyroOffset = true;
+  }
+
   /**
    * Get the configured swerve modules.
    *
@@ -144,7 +170,7 @@ public SwerveModule[] getSwerveModules() {
    * @param pose The robot's actual position on the field.
    */
   public void resetOdometry(Pose2d pose) {
-    odometry.resetPosition(pose, gyro.getRotation2d());
+    odometry.resetPosition(pose, gyro.getRotation2d().rotateBy(gyroOffset));
   }
 
   /** Resets the drive encoders to currently read a position of 0. */
@@ -165,7 +191,7 @@ public void resetGyro() {
    */
   public void periodic() {
     odometry.update(
-        gyro.getRotation2d(),
+        hasGyroOffset ? gyro.getRotation2d().rotateBy(gyroOffset) : gyro.getRotation2d(),
         swerveModules[0].getState(),
         swerveModules[1].getState(),
         swerveModules[2].getState(),
@@ -223,7 +249,10 @@ private SwerveModuleState[] getSwerveModuleStates(
     ChassisSpeeds chassisSpeeds =
         isFieldOriented
             ? ChassisSpeeds.fromFieldRelativeSpeeds(
-                vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond, gyro.getRotation2d())
+                vxMetersPerSecond,
+                vyMetersPerSecond,
+                omegaRadiansPerSecond,
+                hasGyroOffset ? gyro.getRotation2d().rotateBy(gyroOffset) : gyro.getRotation2d())
             : new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
 
     var swerveModuleStates = kinematics.toSwerveModuleStates(chassisSpeeds);

src/main/java/org/strykeforce/swerve/TalonSwerveModule.java

@@ -156,13 +156,13 @@ private int getAzimuthAbsoluteEncoderCounts() {
     return azimuthTalon.getSensorCollection().getPulseWidthPosition() & 0xFFF;
   }
 
-  private Rotation2d getAzimuthRotation2d() {
+  public Rotation2d getAzimuthRotation2d() {
     double azimuthCounts = azimuthTalon.getSelectedSensorPosition();
     double radians = 2.0 * Math.PI * azimuthCounts / azimuthCountsPerRev;
     return new Rotation2d(radians);
   }
 
-  private void setAzimuthRotation2d(Rotation2d angle) {
+  public void setAzimuthRotation2d(Rotation2d angle) {
     double countsBefore = azimuthTalon.getSelectedSensorPosition();
     double countsFromAngle = angle.getRadians() / (2.0 * Math.PI) * azimuthCountsPerRev;
     double countsDelta = Math.IEEEremainder(countsFromAngle - countsBefore, azimuthCountsPerRev);