Merge pull request #14 from SciBorgs/imu

Imu
SciBorgs · Jan 20, 2024 · 4a90a33 · 4a90a33
2 parents a7a0f77 + b39940e
commit 4a90a33
Show file tree

Hide file tree

Showing 4 changed files with 147 additions and 57 deletions.
diff --git a/simgui.json b/simgui.json
@@ -23,6 +23,18 @@
     },
     "windows": {
       "/Robot/drive/field2d": {
+        "module- FL": {
+          "style": "Line"
+        },
+        "module- RR": {
+          "style": "Line"
+        },
+        "module-FR": {
+          "style": "Line"
+        },
+        "module-RL": {
+          "style": "Line"
+        },
         "window": {
           "visible": true
         }
@@ -38,6 +50,12 @@
     "transitory": {
       "Robot": {
         "drive": {
+          "Pose2d##v_/Robot/drive/getPose": {
+            "Rotation2d##v_rotation": {
+              "open": true
+            },
+            "open": true
+          },
           "frontRight": {
             "SwerveModulePosition##v_/Robot/drive/frontRight/position": {
               "Rotation2d##v_angle": {
@@ -53,13 +71,18 @@
             },
             "driveFeedback": {
               "open": true
-            },
-            "open": true
+            }
           },
           "open": true
         },
         "open": true
+      },
+      "Shuffleboard": {
+        "open": true
       }
     }
+  },
+  "NetworkTables Info": {
+    "visible": true
   }
 }
diff --git a/src/main/java/org/sciborgs1155/robot/drive/Drive.java b/src/main/java/org/sciborgs1155/robot/drive/Drive.java
@@ -4,7 +4,6 @@
 import static org.sciborgs1155.robot.Ports.Drive.*;
 import static org.sciborgs1155.robot.drive.DriveConstants.*;
 
-import com.kauailabs.navx.frc.AHRS;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -43,7 +42,8 @@ public class Drive extends SubsystemBase implements Logged, AutoCloseable {
 
   @IgnoreLogged private final List<SwerveModule> modules;
 
-  private final AHRS imu = new AHRS();
+  private final GyroIO gyro;
+  private static Rotation2d simRotation = new Rotation2d();
 
   public final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(MODULE_OFFSET);
 
@@ -67,16 +67,24 @@ public static Drive create() {
             new FlexModule(FRONT_LEFT_DRIVE, FRONT_LEFT_TURNING, ANGULAR_OFFSETS.get(0)),
             new FlexModule(FRONT_RIGHT_DRIVE, FRONT_RIGHT_TURNING, ANGULAR_OFFSETS.get(1)),
             new FlexModule(REAR_LEFT_DRIVE, REAR_LEFT_TURNING, ANGULAR_OFFSETS.get(2)),
-            new FlexModule(REAR_RIGHT_DRIVE, REAR_RIGHT_TURNING, ANGULAR_OFFSETS.get(3)))
-        : new Drive(new SimModule(), new SimModule(), new SimModule(), new SimModule());
+            new FlexModule(REAR_RIGHT_DRIVE, REAR_RIGHT_TURNING, ANGULAR_OFFSETS.get(3)),
+            new GyroIO.NavX())
+        : new Drive(
+            new SimModule(),
+            new SimModule(),
+            new SimModule(),
+            new SimModule(),
+            new GyroIO.NoGyro());
   }
 
   /** A swerve drive subsystem containing four {@link ModuleIO} modules. */
-  public Drive(ModuleIO frontLeft, ModuleIO frontRight, ModuleIO rearLeft, ModuleIO rearRight) {
+  public Drive(
+      ModuleIO frontLeft, ModuleIO frontRight, ModuleIO rearLeft, ModuleIO rearRight, GyroIO gyro) {
     this.frontLeft = new SwerveModule(frontLeft, ANGULAR_OFFSETS.get(0), " FL");
     this.frontRight = new SwerveModule(frontRight, ANGULAR_OFFSETS.get(1), "FR");
     this.rearLeft = new SwerveModule(rearLeft, ANGULAR_OFFSETS.get(2), "RL");
     this.rearRight = new SwerveModule(rearRight, ANGULAR_OFFSETS.get(3), " RR");
+    this.gyro = gyro;
 
     modules = List.of(this.frontLeft, this.frontRight, this.rearLeft, this.rearRight);
     modules2d = new FieldObject2d[modules.size()];
@@ -94,7 +102,8 @@ public Drive(ModuleIO frontLeft, ModuleIO frontRight, ModuleIO rearLeft, ModuleI
                 volts -> modules.forEach(m -> m.setTurnVoltage(volts.in(Volts))), null, this));
 
     odometry =
-        new SwerveDrivePoseEstimator(kinematics, getHeading(), getModulePositions(), new Pose2d());
+        new SwerveDrivePoseEstimator(
+            kinematics, gyro.getRotation2d(), getModulePositions(), new Pose2d());
 
     for (int i = 0; i < modules.size(); i++) {
       var module = modules.get(i);
@@ -114,22 +123,13 @@ public Pose2d getPose() {
     return odometry.getEstimatedPosition();
   }
 
-  /**
-   * Returns the heading of the robot, based on our pigeon.
-   *
-   * @return A Rotation2d of our angle.
-   */
-  public Rotation2d getHeading() {
-    return Robot.isReal() ? imu.getRotation2d() : Rotation2d.fromRadians(simulatedHeading);
-  }
-
   /**
    * Resets the odometry to the specified pose.
    *
    * @param pose The pose to which to set the odometry.
    */
   public void resetOdometry(Pose2d pose) {
-    odometry.resetPosition(getHeading(), getModulePositions(), pose);
+    odometry.resetPosition(gyro.getRotation2d(), getModulePositions(), pose);
   }
 
   /**
@@ -170,7 +170,8 @@ public Command drive(InputStream vx, InputStream vy, Supplier<Rotation2d> headin
                 new ChassisSpeeds(
                     vx.get(),
                     vy.get(),
-                    pid.calculate(getHeading().getRadians(), heading.get().getRadians()))));
+                    pid.calculate(
+                        getPose().getRotation().getRadians(), heading.get().getRadians()))));
   }
 
   /**
@@ -219,12 +220,7 @@ public void resetEncoders() {
 
   /** Zeroes the heading of the robot. */
   public Command zeroHeading() {
-    return runOnce(imu::reset);
-  }
-
-  /** Returns the pitch of the drive gyro */
-  public double getPitch() {
-    return imu.getPitch();
+    return runOnce(gyro::reset);
   }
 
   /** Returns the module states. */
@@ -239,6 +235,12 @@ private SwerveModulePosition[] getModulePositions() {
     return modules.stream().map(SwerveModule::position).toArray(SwerveModulePosition[]::new);
   }
 
+  /** Returns the chassis speed. */
+  @Log.NT
+  public ChassisSpeeds getChassisSpeed() {
+    return kinematics.toChassisSpeeds(getModuleStates());
+  }
+
   /** Updates pose estimation based on provided {@link EstimatedRobotPose} */
   public void updateEstimates(EstimatedRobotPose... poses) {
     for (int i = 0; i < poses.length; i++) {
@@ -249,7 +251,7 @@ public void updateEstimates(EstimatedRobotPose... poses) {
 
   @Override
   public void periodic() {
-    odometry.update(getHeading(), getModulePositions());
+    odometry.update(Robot.isReal() ? gyro.getRotation2d() : simRotation, getModulePositions());
 
     field2d.setRobotPose(getPose());
 
@@ -260,14 +262,12 @@ public void periodic() {
     }
   }
 
-  // jank
-  private double simulatedHeading = 0.0;
-
   @Override
   public void simulationPeriodic() {
-    simulatedHeading +=
-        kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond
-            * Constants.PERIOD.in(Seconds);
+    simRotation =
+        simRotation.rotateBy(
+            Rotation2d.fromRadians(
+                getChassisSpeed().omegaRadiansPerSecond * Constants.PERIOD.in(Seconds)));
   }
 
   /** Stops drivetrain */
@@ -318,5 +318,6 @@ public void close() throws Exception {
     frontRight.close();
     rearLeft.close();
     rearRight.close();
+    gyro.close();
   }
 }
diff --git a/src/main/java/org/sciborgs1155/robot/drive/GyroIO.java b/src/main/java/org/sciborgs1155/robot/drive/GyroIO.java
@@ -0,0 +1,66 @@
+package org.sciborgs1155.robot.drive;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+
+/** Generalized gyroscope. Pigeon2, Navx, and SimGyro are to be implemented */
+public interface GyroIO extends AutoCloseable {
+  /** Calibrates the gyroscope. Pigeon2 does not need to do anything here. */
+  public default void calibrate() {}
+
+  /** Returns the rate of rotation. */
+  public double getRate();
+
+  /** Returns the heading of the robot as a Rotation2d. */
+  public default Rotation2d getRotation2d() {
+    return getRotation3d().toRotation2d();
+  }
+
+  /** Returns the heading of the robot as a Rotation3d. */
+  public Rotation3d getRotation3d();
+
+  /** Resets heading to 0 */
+  public void reset();
+
+  /** GyroIO implementation for NavX */
+  public class NavX implements GyroIO {
+    private final AHRS ahrs = new AHRS();
+
+    public double getRate() {
+      return ahrs.getRate();
+    }
+
+    @Override
+    public Rotation3d getRotation3d() {
+      return ahrs.getRotation3d();
+    }
+
+    public void reset() {
+      ahrs.reset();
+    }
+
+    @Override
+    public void close() throws Exception {}
+  }
+
+  /** GyroIO implementation for nonexistent gyro */
+  public class NoGyro implements GyroIO {
+
+    @Override
+    public void close() throws Exception {}
+
+    @Override
+    public double getRate() {
+      return 0;
+    }
+
+    @Override
+    public Rotation3d getRotation3d() {
+      return new Rotation3d();
+    }
+
+    @Override
+    public void reset() {}
+  }
+}