diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6096f632..d1207480 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.GeneralConstants; @@ -264,8 +263,8 @@ private void configureBindings() { controller .start() - .whileTrue( - new RunCommand( + .onTrue( + runOnce( () -> drive.setYaw( switch (DriverStation.getAlliance().get()) { @@ -273,6 +272,8 @@ private void configureBindings() { case Red -> Rotation2d.fromDegrees(180); default -> Rotation2d.fromDegrees(0); }))); + + controller.back().onTrue(runOnce(drive::zeroToGyro)); } private boolean IsEndGame() { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9a5cc33c..ac5a4763 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -367,15 +367,21 @@ public double getRawGyroYaw() { /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { - poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + gyroIO.setYaw(pose.getRotation()); + // We just pass the desired pose for gyro yaw, since we've just re-zeroed it. + poseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose); } /** Sets the current odometry yaw. */ public void setYaw(Rotation2d yaw) { - gyroIO.setYaw(yaw); setPose(new Pose2d(getPose().getTranslation(), yaw)); } + /** Quick and dirty method of "re-zeroing". Sets odometry rotation to gyro reported rotation. */ + public void zeroToGyro() { + setPose(new Pose2d(getPose().getTranslation(), gyroInputs.yawPosition)); + } + /** Returns an array of module translations. */ public static Translation2d[] getModuleTranslations() { return new Translation2d[] { diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 126fbbc0..7da8cf04 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -16,20 +16,29 @@ import static frc.robot.constants.DriveConstants.Module.ODOMETRY_FREQUENCY; import static frc.robot.constants.GeneralConstants.currentRobot; -import com.kauailabs.navx.frc.AHRS; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.SPI; import java.util.Queue; import org.littletonrobotics.junction.Logger; /** IO implementation for NavX */ public class GyroIONavX implements GyroIO { - private final AHRS navX = new AHRS(SPI.Port.kMXP, (byte) ODOMETRY_FREQUENCY); // 200Hz update rate + // TODO NavX documentation is not good and running into a lot of issues. Please get Pidgeon 2 next + // year + private final AHRS navX = + new AHRS(NavXComType.kMXP_SPI, (int) ODOMETRY_FREQUENCY); // 200Hz update rate private final Queue yawPositionQueue; private final Queue yawTimestampQueue; public GyroIONavX() { + while (navX.isCalibrating()) { + System.out.println("Waiting for NavX calibration"); + Logger.recordOutput("Gyro/NavXCalibrating", true); + Thread.yield(); + } + Logger.recordOutput("Gyro/NavXCalibrating", false); navX.reset(); yawPositionQueue = SparkMaxOdometryThread.getInstance().registerSignal(() -> navX.getAngle()); yawTimestampQueue = @@ -54,9 +63,8 @@ public void updateInputs(GyroIOInputs inputs) { (Double value) -> Rotation2d.fromDegrees( switch (currentRobot) { - case DORY -> - -value; // According to last years code, clef NavX is inverted - case NAUTILUS -> -value; + case DORY -> value; + case NAUTILUS -> value; })) .toArray(Rotation2d[]::new); yawTimestampQueue.clear(); @@ -65,4 +73,11 @@ public void updateInputs(GyroIOInputs inputs) { Logger.recordOutput("NavX Rotation", navX.getRotation2d()); Logger.recordOutput("NavX Last Sensor Timestamp", navX.getLastSensorTimestamp()); } + + @Override + public void setYaw(Rotation2d yaw) { + navX.setAngleAdjustment(0.0); // Reset angle adjustment so we can rezero + + navX.setAngleAdjustment(yaw.getDegrees() - navX.getAngle()); + } } diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json deleted file mode 100644 index 27937030..00000000 --- a/vendordeps/NavX.json +++ /dev/null @@ -1,40 +0,0 @@ -{ - "fileName": "NavX.json", - "name": "NavX", - "version": "2024.1.0", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2025", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2024/" - ], - "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-java", - "version": "2024.1.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-cpp", - "version": "2024.1.0", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} diff --git a/vendordeps/Studica-2025.0.1.json b/vendordeps/Studica-2025.0.1.json new file mode 100644 index 00000000..a3ed1fee --- /dev/null +++ b/vendordeps/Studica-2025.0.1.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.0.1.json", + "name": "Studica", + "version": "2025.0.1", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", + "cppDependencies": [ + { + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "Studica", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + }, + { + "artifactId": "Studica-driver", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.1" + } + ] +}