diff --git a/simgui.json b/simgui.json index 05b9908..0e2bb2f 100644 --- 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 index 8abaf59..3ff11d5 100644 --- 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 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 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 new file mode 100644 index 0000000..d558b9c --- /dev/null +++ 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() {} + } +} diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 7020206..69a4079 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,13 +1,13 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.0.0-beta-8", + "version": "24.1.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", "conflictsWith": [ { "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.0.0-beta-8" + "version": "24.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true,