From d092f4b9da7204a6706152a38af545efc6f3a314 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 20 Sep 2025 17:26:25 -0700 Subject: [PATCH 1/9] add snapping to rotations --- .../org/team5924/frc2025/BuildConstants.java | 12 +-- .../org/team5924/frc2025/RobotContainer.java | 3 + .../frc2025/subsystems/drive/Drive.java | 44 +++++++++ vendordeps/AdvantageKit.json | 6 +- ....2.3.json => PathplannerLib-2025.2.7.json} | 8 +- ....3.1.json => Phoenix6-frc2025-latest.json} | 92 ++++++++++++------- 6 files changed, 121 insertions(+), 44 deletions(-) rename vendordeps/{PathplannerLib-2025.2.3.json => PathplannerLib-2025.2.7.json} (87%) rename vendordeps/{Phoenix6-25.3.1.json => Phoenix6-frc2025-latest.json} (86%) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index eab8475..53a031d 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 270; - public static final String GIT_SHA = "335559c04de83aa24f7aa7c1d4e54041da73cda5"; - public static final String GIT_DATE = "2025-04-05 20:00:04 EDT"; - public static final String GIT_BRANCH = "elevator-hotfix"; - public static final String BUILD_DATE = "2025-04-05 20:25:07 EDT"; - public static final long BUILD_UNIX_TIME = 1743899107859L; + public static final int GIT_REVISION = 271; + public static final String GIT_SHA = "871c7ab6f027745c401984afed7478d5bfb16d97"; + public static final String GIT_DATE = "2025-04-06 10:04:02 EDT"; + public static final String GIT_BRANCH = "86-drive-heading"; + public static final String BUILD_DATE = "2025-09-20 19:48:02 EDT"; + public static final long BUILD_UNIX_TIME = 1758412082360L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index aa2cd0b..5c4290b 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -262,6 +262,9 @@ private void configureButtonBindings() { .whileTrue( DriveCommands.turnToLeftCoralStation( drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); + + driveController.rightStick().onTrue(Commands.runOnce(() -> drive.toggleSnapToHeading())); + // Coral In and Out driveController.y().onTrue(new TeleopShoot(coralInAndOut).withTimeout(Seconds.of(1))); diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java index 8edfaa8..e185f10 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java @@ -31,7 +31,9 @@ import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -142,6 +144,14 @@ public class Drive extends SubsystemBase { private final Field2d field = new Field2d(); + // in radians + private double desiredHeading = 0.0; + private boolean snapToHeading = false; + + public boolean toggleSnapToHeading() { + return snapToHeading = !snapToHeading; + } + public Drive( GyroIO gyroIO, ModuleIO flModuleIO, @@ -352,6 +362,35 @@ public void periodic() { } } + /** + * rotates the speeds towards the desired heading with a ±3 degree tolerance + * + * @param speeds input speeds that will be updated + * @param targetHeading the desired heading (rotation) + * @return updated speeds + */ + public ChassisSpeeds updateSpeedsWithDesiredHeading(ChassisSpeeds speeds, double targetHeading) { + // reset omega + speeds.omegaRadiansPerSecond = 0; + + // tolerance of ±3 deg + boolean isWithinTolerance = Math.abs(getRotation().getRadians() - targetHeading) <= 0.0523599; + + if (isWithinTolerance) return speeds; // within tolerance; don't rotate + + // otherwise, calculate omega + PIDController pid = new PIDController(3, 0, 0); + + double omega = pid.calculate(MathUtil.angleModulus(getRotation().getRadians()), targetHeading); + omega = MathUtil.clamp(omega, -getMaxAngularSpeedRadPerSec(), getMaxAngularSpeedRadPerSec()); + pid.close(); + + // update omega + speeds.omegaRadiansPerSecond = omega; + + return speeds; + } + /** * Runs the drive at the desired velocity. * @@ -359,6 +398,11 @@ public void periodic() { */ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints + + if (snapToHeading) { + speeds = updateSpeedsWithDesiredHeading(speeds, desiredHeading); + } + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); previousSetpoint = setpointGenerator.generateSetpoint( diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index fa81b2f..2707c2b 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "4.1.0", + "version": "4.1.2", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2025", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "4.1.0" + "version": "4.1.2" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "4.1.0", + "version": "4.1.2", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/PathplannerLib-2025.2.3.json b/vendordeps/PathplannerLib-2025.2.7.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.3.json rename to vendordeps/PathplannerLib-2025.2.7.json index 8d2727f..d0b6939 100644 --- a/vendordeps/PathplannerLib-2025.2.3.json +++ b/vendordeps/PathplannerLib-2025.2.7.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.3.json", + "fileName": "PathplannerLib-2025.2.7.json", "name": "PathplannerLib", - "version": "2025.2.3", + "version": "2025.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.3" + "version": "2025.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.3", + "version": "2025.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.3.1.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 86% rename from vendordeps/Phoenix6-25.3.1.json rename to vendordeps/Phoenix6-frc2025-latest.json index 3ff25f8..ce44ce4 100644 --- a/vendordeps/Phoenix6-25.3.1.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.3.1.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.3.1", + "version": "25.4.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.3.1" + "version": "25.4.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.1", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.3.1", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +318,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +334,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +350,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +366,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +382,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +398,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +430,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +446,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.3.1", + "version": "25.4.0", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -444,6 +458,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } From 542420c9b371577cef3f978ca8e8d160be52d050 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 27 Sep 2025 14:33:53 -0700 Subject: [PATCH 2/9] fix folder and import capitalization --- src/main/java/org/team5924/frc2025/BuildConstants.java | 10 +++++----- src/main/java/org/team5924/frc2025/RobotContainer.java | 10 +++++----- src/main/java/org/team5924/frc2025/RobotState.java | 2 +- .../frc2025/commands/coralInAndOut/RunIntake.java | 5 +++-- .../frc2025/commands/coralInAndOut/RunShooter.java | 5 +++-- .../frc2025/commands/coralInAndOut/TeleopShoot.java | 4 ++-- .../rollers/CoralInAndOut/CoralInAndOut.java | 3 ++- .../rollers/CoralInAndOut/CoralInAndOutIO.java | 2 +- .../CoralInAndOut/CoralInAndOutIOKrakenFOC.java | 2 +- .../rollers/CoralInAndOut/CoralInAndOutIOSim.java | 2 +- 10 files changed, 24 insertions(+), 21 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 53a031d..8fec29e 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 271; - public static final String GIT_SHA = "871c7ab6f027745c401984afed7478d5bfb16d97"; - public static final String GIT_DATE = "2025-04-06 10:04:02 EDT"; + public static final int GIT_REVISION = 272; + public static final String GIT_SHA = "d092f4b9da7204a6706152a38af545efc6f3a314"; + public static final String GIT_DATE = "2025-09-20 20:26:25 EDT"; public static final String GIT_BRANCH = "86-drive-heading"; - public static final String BUILD_DATE = "2025-09-20 19:48:02 EDT"; - public static final long BUILD_UNIX_TIME = 1758412082360L; + public static final String BUILD_DATE = "2025-09-27 17:20:13 EDT"; + public static final long BUILD_UNIX_TIME = 1759008013542L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index 5c4290b..bcf3afd 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -50,11 +50,11 @@ import org.team5924.frc2025.subsystems.elevator.Elevator; import org.team5924.frc2025.subsystems.elevator.ElevatorIO; import org.team5924.frc2025.subsystems.elevator.ElevatorIOTalonFXGamma; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIO; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOKrakenFOC; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOSim; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOutIO; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOutIOKrakenFOC; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOutIOSim; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState; import org.team5924.frc2025.subsystems.vision.Vision; import org.team5924.frc2025.subsystems.vision.VisionIO; import org.team5924.frc2025.subsystems.vision.VisionIOLimelight; diff --git a/src/main/java/org/team5924/frc2025/RobotState.java b/src/main/java/org/team5924/frc2025/RobotState.java index fc6df05..41f9130 100644 --- a/src/main/java/org/team5924/frc2025/RobotState.java +++ b/src/main/java/org/team5924/frc2025/RobotState.java @@ -24,8 +24,8 @@ import org.team5924.frc2025.subsystems.climber.Climber.ClimberState; import org.team5924.frc2025.subsystems.elevator.Elevator.ElevatorState; import org.team5924.frc2025.subsystems.pivot.AlgaePivot.AlgaePivotState; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; import org.team5924.frc2025.subsystems.rollers.algae.AlgaeRoller.AlgaeRollerState; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState; import org.team5924.frc2025.util.VisionFieldPoseEstimate; @Getter diff --git a/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunIntake.java b/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunIntake.java index 1ff615c..4c689c4 100644 --- a/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunIntake.java +++ b/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunIntake.java @@ -16,10 +16,11 @@ package org.team5924.frc2025.commands.coralInAndOut; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.*; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState; + import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.*; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; public class RunIntake extends Command { private CoralInAndOut coralInAndOut; diff --git a/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunShooter.java b/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunShooter.java index 4ea2ae6..ce3427d 100644 --- a/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunShooter.java +++ b/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunShooter.java @@ -16,10 +16,11 @@ package org.team5924.frc2025.commands.coralInAndOut; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.*; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState; + import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.*; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; public class RunShooter extends Command { private CoralInAndOut coralInAndOut; diff --git a/src/main/java/org/team5924/frc2025/commands/coralInAndOut/TeleopShoot.java b/src/main/java/org/team5924/frc2025/commands/coralInAndOut/TeleopShoot.java index ed3d407..afb57d2 100644 --- a/src/main/java/org/team5924/frc2025/commands/coralInAndOut/TeleopShoot.java +++ b/src/main/java/org/team5924/frc2025/commands/coralInAndOut/TeleopShoot.java @@ -20,8 +20,8 @@ import edu.wpi.first.wpilibj2.command.Command; import org.team5924.frc2025.RobotState; import org.team5924.frc2025.subsystems.elevator.Elevator.ElevatorState; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class TeleopShoot extends Command { diff --git a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java index 8671a91..8888758 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java +++ b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java @@ -14,7 +14,7 @@ * If you did not, see . */ -package org.team5924.frc2025.subsystems.rollers.CoralInAndOut; +package org.team5924.frc2025.subsystems.rollers.coralInAndOut; import java.util.function.DoubleSupplier; import lombok.Getter; @@ -24,6 +24,7 @@ import org.team5924.frc2025.RobotState; import org.team5924.frc2025.subsystems.elevator.Elevator.ElevatorState; import org.team5924.frc2025.subsystems.rollers.GenericRollerSystem; +import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOInputsAutoLogged; import org.team5924.frc2025.subsystems.rollers.GenericRollerSystem.VoltageState; import org.team5924.frc2025.util.LoggedTunableNumber; diff --git a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIO.java b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIO.java index f7de675..24ea1c0 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIO.java +++ b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIO.java @@ -14,7 +14,7 @@ * If you did not, see . */ -package org.team5924.frc2025.subsystems.rollers.CoralInAndOut; +package org.team5924.frc2025.subsystems.rollers.coralInAndOut; import org.littletonrobotics.junction.AutoLog; import org.team5924.frc2025.subsystems.rollers.GenericRollerSystemIO; diff --git a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIOKrakenFOC.java b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIOKrakenFOC.java index 8f3e3d6..c353be2 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIOKrakenFOC.java +++ b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIOKrakenFOC.java @@ -14,7 +14,7 @@ * If you did not, see . */ -package org.team5924.frc2025.subsystems.rollers.CoralInAndOut; +package org.team5924.frc2025.subsystems.rollers.coralInAndOut; import au.grapplerobotics.LaserCan; import edu.wpi.first.wpilibj.Alert; diff --git a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIOSim.java b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIOSim.java index 59c74b4..e0d3bea 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIOSim.java +++ b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIOSim.java @@ -14,7 +14,7 @@ * If you did not, see . */ -package org.team5924.frc2025.subsystems.rollers.CoralInAndOut; +package org.team5924.frc2025.subsystems.rollers.coralInAndOut; import edu.wpi.first.math.system.plant.DCMotor; import org.team5924.frc2025.Constants; From 741bc9b2ae4779e646b36cd1e6a30de7646db3c6 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 27 Sep 2025 16:23:46 -0700 Subject: [PATCH 3/9] add heading lock to coral station --- .../org/team5924/frc2025/RobotContainer.java | 26 +++++---- .../frc2025/commands/drive/DriveCommands.java | 53 +++++++++++++++++++ .../frc2025/subsystems/drive/Drive.java | 9 ++++ 3 files changed, 77 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index bcf3afd..213589d 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -251,17 +251,21 @@ private void configureButtonBindings() { .whileTrue( new DeferredCommand(() -> DriveCommands.driveToReef(drive, false), Set.of(drive))); - driveController - .rightTrigger() - .whileTrue( - DriveCommands.turnToRightCoralStation( - drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); - - driveController - .leftTrigger() - .whileTrue( - DriveCommands.turnToLeftCoralStation( - drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); + // driveController + // .rightTrigger() + // .whileTrue( + // DriveCommands.turnToRightCoralStation( + // drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); + + // driveController + // .leftTrigger() + // .whileTrue( + // DriveCommands.turnToLeftCoralStation( + // drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); + + driveController.rightTrigger().onTrue(DriveCommands.lockOnCoralStation(drive, true)); + driveController.leftTrigger().onTrue(DriveCommands.lockOnCoralStation(drive, false)); + driveController.rightTrigger().or(driveController.leftTrigger()).onFalse(DriveCommands.unlockRotation(drive)); driveController.rightStick().onTrue(Commands.runOnce(() -> drive.toggleSnapToHeading())); diff --git a/src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java b/src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java index 90b291c..69809e8 100644 --- a/src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java +++ b/src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java @@ -72,6 +72,59 @@ private static Translation2d getLinearVelocityFromJoysticks(double x, double y) .getTranslation(); } + /** + * returns whether rotations should be flipped (same as if the alliance is red) + */ + public static boolean isFlipped() + { + return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; + } + + /** + * gets the needed rotation to align to the corresponding coral station + * @param right true = right coral station; false = left coral station + */ + public static double getCoralStationRotation(boolean right) + { + double heading; + + if (isFlipped()) { + heading = Constants.CORAL_STATION_RADIANS_NORMAL - Math.PI; + } else { + heading = Constants.CORAL_STATION_RADIANS_NORMAL; + } + + return right ? heading : -heading; + } + + /** + * Locks the robot's rotation in the direction of the desired coral station + * @param drive a refrence to the {@link Drive} subsystem + * @param right true = right coral station; false = left coral station + */ + public static Command lockOnCoralStation(Drive drive, boolean right) + { + return Commands.run( + () -> { + double heading = getCoralStationRotation(right); + drive.setDesiredHeading(heading); + drive.setSnapToHeading(true); + } + ); + } + + /** + * Unlocks the robots rotation, stops it from snapping to the desired heading + */ + public static Command unlockRotation(Drive drive) + { + return Commands.run( + () -> { + drive.setSnapToHeading(false); + } + ); + } + /** * Field relative drive command using two joysticks (controlling linear and angular velocities). */ diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java index e185f10..91ddcc8 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java @@ -152,6 +152,15 @@ public boolean toggleSnapToHeading() { return snapToHeading = !snapToHeading; } + public void setSnapToHeading(boolean snap) { + snapToHeading = snap; + } + + public void setDesiredHeading(double heading) + { + desiredHeading = heading; + } + public Drive( GyroIO gyroIO, ModuleIO flModuleIO, From 925fc8b7f38a1978654714995a3961d8a6ee7057 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 27 Sep 2025 16:36:16 -0700 Subject: [PATCH 4/9] fix errors in imports --- .../org/team5924/frc2025/BuildConstants.java | 10 ++--- .../org/team5924/frc2025/RobotContainer.java | 9 ++-- .../commands/coralInAndOut/RunIntake.java | 5 +-- .../commands/coralInAndOut/RunShooter.java | 5 +-- .../frc2025/commands/drive/DriveCommands.java | 43 ++++++++----------- .../frc2025/subsystems/drive/Drive.java | 3 +- .../rollers/CoralInAndOut/CoralInAndOut.java | 1 - 7 files changed, 34 insertions(+), 42 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 8fec29e..0512334 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 272; - public static final String GIT_SHA = "d092f4b9da7204a6706152a38af545efc6f3a314"; - public static final String GIT_DATE = "2025-09-20 20:26:25 EDT"; + public static final int GIT_REVISION = 274; + public static final String GIT_SHA = "741bc9b2ae4779e646b36cd1e6a30de7646db3c6"; + public static final String GIT_DATE = "2025-09-27 19:23:46 EDT"; public static final String GIT_BRANCH = "86-drive-heading"; - public static final String BUILD_DATE = "2025-09-27 17:20:13 EDT"; - public static final long BUILD_UNIX_TIME = 1759008013542L; + public static final String BUILD_DATE = "2025-09-27 19:35:14 EDT"; + public static final long BUILD_UNIX_TIME = 1759016114521L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index 213589d..c7afda6 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -51,10 +51,10 @@ import org.team5924.frc2025.subsystems.elevator.ElevatorIO; import org.team5924.frc2025.subsystems.elevator.ElevatorIOTalonFXGamma; import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState; import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOutIO; import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOutIOKrakenFOC; import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOutIOSim; -import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState; import org.team5924.frc2025.subsystems.vision.Vision; import org.team5924.frc2025.subsystems.vision.VisionIO; import org.team5924.frc2025.subsystems.vision.VisionIOLimelight; @@ -256,7 +256,7 @@ private void configureButtonBindings() { // .whileTrue( // DriveCommands.turnToRightCoralStation( // drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); - + // driveController // .leftTrigger() // .whileTrue( @@ -265,7 +265,10 @@ private void configureButtonBindings() { driveController.rightTrigger().onTrue(DriveCommands.lockOnCoralStation(drive, true)); driveController.leftTrigger().onTrue(DriveCommands.lockOnCoralStation(drive, false)); - driveController.rightTrigger().or(driveController.leftTrigger()).onFalse(DriveCommands.unlockRotation(drive)); + driveController + .rightTrigger() + .or(driveController.leftTrigger()) + .onFalse(DriveCommands.unlockRotation(drive)); driveController.rightStick().onTrue(Commands.runOnce(() -> drive.toggleSnapToHeading())); diff --git a/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunIntake.java b/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunIntake.java index 4c689c4..5088254 100644 --- a/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunIntake.java +++ b/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunIntake.java @@ -16,11 +16,10 @@ package org.team5924.frc2025.commands.coralInAndOut; -import org.team5924.frc2025.subsystems.rollers.coralInAndOut.*; -import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState; - import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.*; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState; public class RunIntake extends Command { private CoralInAndOut coralInAndOut; diff --git a/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunShooter.java b/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunShooter.java index ce3427d..314d091 100644 --- a/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunShooter.java +++ b/src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunShooter.java @@ -16,11 +16,10 @@ package org.team5924.frc2025.commands.coralInAndOut; -import org.team5924.frc2025.subsystems.rollers.coralInAndOut.*; -import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState; - import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.*; +import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState; public class RunShooter extends Command { private CoralInAndOut coralInAndOut; diff --git a/src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java b/src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java index 69809e8..b5a3d7a 100644 --- a/src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java +++ b/src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java @@ -72,20 +72,18 @@ private static Translation2d getLinearVelocityFromJoysticks(double x, double y) .getTranslation(); } - /** - * returns whether rotations should be flipped (same as if the alliance is red) - */ - public static boolean isFlipped() - { - return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; + /** returns whether rotations should be flipped (same as if the alliance is red) */ + public static boolean isFlipped() { + return DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; } /** * gets the needed rotation to align to the corresponding coral station + * * @param right true = right coral station; false = left coral station */ - public static double getCoralStationRotation(boolean right) - { + public static double getCoralStationRotation(boolean right) { double heading; if (isFlipped()) { @@ -99,30 +97,25 @@ public static double getCoralStationRotation(boolean right) /** * Locks the robot's rotation in the direction of the desired coral station + * * @param drive a refrence to the {@link Drive} subsystem * @param right true = right coral station; false = left coral station */ - public static Command lockOnCoralStation(Drive drive, boolean right) - { + public static Command lockOnCoralStation(Drive drive, boolean right) { return Commands.run( - () -> { - double heading = getCoralStationRotation(right); - drive.setDesiredHeading(heading); - drive.setSnapToHeading(true); - } - ); + () -> { + double heading = getCoralStationRotation(right); + drive.setDesiredHeading(heading); + drive.setSnapToHeading(true); + }); } - /** - * Unlocks the robots rotation, stops it from snapping to the desired heading - */ - public static Command unlockRotation(Drive drive) - { + /** Unlocks the robots rotation, stops it from snapping to the desired heading */ + public static Command unlockRotation(Drive drive) { return Commands.run( - () -> { - drive.setSnapToHeading(false); - } - ); + () -> { + drive.setSnapToHeading(false); + }); } /** diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java index 91ddcc8..4273003 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java @@ -156,8 +156,7 @@ public void setSnapToHeading(boolean snap) { snapToHeading = snap; } - public void setDesiredHeading(double heading) - { + public void setDesiredHeading(double heading) { desiredHeading = heading; } diff --git a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java index 8888758..d48add7 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java +++ b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java @@ -24,7 +24,6 @@ import org.team5924.frc2025.RobotState; import org.team5924.frc2025.subsystems.elevator.Elevator.ElevatorState; import org.team5924.frc2025.subsystems.rollers.GenericRollerSystem; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOInputsAutoLogged; import org.team5924.frc2025.subsystems.rollers.GenericRollerSystem.VoltageState; import org.team5924.frc2025.util.LoggedTunableNumber; From 3034c093e09a9a175c4c0ec649aff2239647be39 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Thu, 16 Oct 2025 18:32:21 -0700 Subject: [PATCH 5/9] add constant SLOW_MODE_MULTI and change slow mode multiplier to 0.5 (was previously 0.25) --- .../java/org/team5924/frc2025/BuildConstants.java | 12 ++++++------ src/main/java/org/team5924/frc2025/Constants.java | 1 + .../java/org/team5924/frc2025/RobotContainer.java | 8 ++++---- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 0512334..8b855a7 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,13 +21,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 274; - public static final String GIT_SHA = "741bc9b2ae4779e646b36cd1e6a30de7646db3c6"; - public static final String GIT_DATE = "2025-09-27 19:23:46 EDT"; + public static final int GIT_REVISION = 275; + public static final String GIT_SHA = "925fc8b7f38a1978654714995a3961d8a6ee7057"; + public static final String GIT_DATE = "2025-09-27 19:36:16 EDT"; public static final String GIT_BRANCH = "86-drive-heading"; - public static final String BUILD_DATE = "2025-09-27 19:35:14 EDT"; - public static final long BUILD_UNIX_TIME = 1759016114521L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2025-10-14 21:17:17 EDT"; + public static final long BUILD_UNIX_TIME = 1760491037740L; + public static final int DIRTY = 0; private BuildConstants() {} } diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index cb4c8c5..271ec0e 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -56,6 +56,7 @@ public static enum Mode { public static final boolean TUNING_MODE = false; public static final boolean ALLOW_ASSERTS = false; + public static final double SLOW_MODE_MULTI = 0.5; /* Field */ public static final double FIELD_BORDER_MARGIN = 0.5; diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index c7afda6..68f6b53 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -217,15 +217,15 @@ private void configureButtonBindings() { () -> -driveController.getLeftX(), () -> -driveController.getRightX())); - // Nope. It's slow mode now. Quarter speed + // Nope. It's slow mode now. driveController .a() .whileTrue( DriveCommands.joystickDrive( drive, - () -> -driveController.getLeftY() * .25, - () -> -driveController.getLeftX() * .25, - () -> -driveController.getRightX() * .25)); + () -> -driveController.getLeftY() * Constants.SLOW_MODE_MULTI, + () -> -driveController.getLeftX() * Constants.SLOW_MODE_MULTI, + () -> -driveController.getRightX() * Constants.SLOW_MODE_MULTI)); // Switch to X pattern when X button is pressed driveController.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); From 62b7b64512c65a2baa748f204b7063a675f6dc27 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 18 Oct 2025 13:20:59 -0700 Subject: [PATCH 6/9] built robot code --- src/main/java/org/team5924/frc2025/BuildConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 8b855a7..f546ee9 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 275; - public static final String GIT_SHA = "925fc8b7f38a1978654714995a3961d8a6ee7057"; - public static final String GIT_DATE = "2025-09-27 19:36:16 EDT"; + public static final int GIT_REVISION = 276; + public static final String GIT_SHA = "3034c093e09a9a175c4c0ec649aff2239647be39"; + public static final String GIT_DATE = "2025-10-16 21:32:21 EDT"; public static final String GIT_BRANCH = "86-drive-heading"; - public static final String BUILD_DATE = "2025-10-14 21:17:17 EDT"; - public static final long BUILD_UNIX_TIME = 1760491037740L; + public static final String BUILD_DATE = "2025-10-16 23:35:34 EDT"; + public static final long BUILD_UNIX_TIME = 1760672134396L; public static final int DIRTY = 0; private BuildConstants() {} From 8a97b9bcca84ee0e6c261e9b9b75f4c35d489a50 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 18 Oct 2025 14:31:25 -0700 Subject: [PATCH 7/9] add logic for angle wraparounds, fix command runonce, fix minor typos in comments --- .../org/team5924/frc2025/BuildConstants.java | 12 +++++----- .../frc2025/commands/drive/DriveCommands.java | 6 ++--- .../frc2025/subsystems/drive/Drive.java | 23 +++++++++++-------- 3 files changed, 23 insertions(+), 18 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index f546ee9..5205ee3 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,13 +21,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 276; - public static final String GIT_SHA = "3034c093e09a9a175c4c0ec649aff2239647be39"; - public static final String GIT_DATE = "2025-10-16 21:32:21 EDT"; + public static final int GIT_REVISION = 277; + public static final String GIT_SHA = "62b7b64512c65a2baa748f204b7063a675f6dc27"; + public static final String GIT_DATE = "2025-10-18 16:20:59 EDT"; public static final String GIT_BRANCH = "86-drive-heading"; - public static final String BUILD_DATE = "2025-10-16 23:35:34 EDT"; - public static final long BUILD_UNIX_TIME = 1760672134396L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2025-10-18 17:29:41 EDT"; + public static final long BUILD_UNIX_TIME = 1760822981500L; + public static final int DIRTY = 1; private BuildConstants() {} } diff --git a/src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java b/src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java index b5a3d7a..fde45ea 100644 --- a/src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java +++ b/src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java @@ -98,11 +98,11 @@ public static double getCoralStationRotation(boolean right) { /** * Locks the robot's rotation in the direction of the desired coral station * - * @param drive a refrence to the {@link Drive} subsystem + * @param drive a reference to the {@link Drive} subsystem * @param right true = right coral station; false = left coral station */ public static Command lockOnCoralStation(Drive drive, boolean right) { - return Commands.run( + return Commands.runOnce( () -> { double heading = getCoralStationRotation(right); drive.setDesiredHeading(heading); @@ -112,7 +112,7 @@ public static Command lockOnCoralStation(Drive drive, boolean right) { /** Unlocks the robots rotation, stops it from snapping to the desired heading */ public static Command unlockRotation(Drive drive) { - return Commands.run( + return Commands.runOnce( () -> { drive.setSnapToHeading(false); }); diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java index 4273003..5012b24 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java @@ -148,6 +148,8 @@ public class Drive extends SubsystemBase { private double desiredHeading = 0.0; private boolean snapToHeading = false; + PIDController headingPid = new PIDController(3, 0, 0); + public boolean toggleSnapToHeading() { return snapToHeading = !snapToHeading; } @@ -246,6 +248,8 @@ public void initSendable(SendableBuilder builder) { builder.addDoubleProperty("Robot Angle", () -> getRotation().getRadians(), null); } }); + + headingPid.enableContinuousInput(-Math.PI, Math.PI); } @Override @@ -378,20 +382,21 @@ public void periodic() { * @return updated speeds */ public ChassisSpeeds updateSpeedsWithDesiredHeading(ChassisSpeeds speeds, double targetHeading) { - // reset omega - speeds.omegaRadiansPerSecond = 0; - // tolerance of ±3 deg - boolean isWithinTolerance = Math.abs(getRotation().getRadians() - targetHeading) <= 0.0523599; + boolean isWithinTolerance = + Math.abs(MathUtil.angleModulus(targetHeading - getRotation().getRadians())) <= 0.0523599; - if (isWithinTolerance) return speeds; // within tolerance; don't rotate + if (isWithinTolerance) + return new ChassisSpeeds( + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + 0.0); // within tolerance; don't rotate // otherwise, calculate omega - PIDController pid = new PIDController(3, 0, 0); - - double omega = pid.calculate(MathUtil.angleModulus(getRotation().getRadians()), targetHeading); + double omega = + headingPid.calculate(MathUtil.angleModulus(getRotation().getRadians()), targetHeading); omega = MathUtil.clamp(omega, -getMaxAngularSpeedRadPerSec(), getMaxAngularSpeedRadPerSec()); - pid.close(); + headingPid.close(); // update omega speeds.omegaRadiansPerSecond = omega; From 55b0919c472147eaf933d0012357c97d430a8c1b Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 18 Oct 2025 14:41:21 -0700 Subject: [PATCH 8/9] more coderabbit logic fixes --- .../java/org/team5924/frc2025/BuildConstants.java | 10 +++++----- .../team5924/frc2025/subsystems/drive/Drive.java | 15 ++++++--------- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 5205ee3..ccbe647 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 277; - public static final String GIT_SHA = "62b7b64512c65a2baa748f204b7063a675f6dc27"; - public static final String GIT_DATE = "2025-10-18 16:20:59 EDT"; + public static final int GIT_REVISION = 278; + public static final String GIT_SHA = "8a97b9bcca84ee0e6c261e9b9b75f4c35d489a50"; + public static final String GIT_DATE = "2025-10-18 17:31:25 EDT"; public static final String GIT_BRANCH = "86-drive-heading"; - public static final String BUILD_DATE = "2025-10-18 17:29:41 EDT"; - public static final long BUILD_UNIX_TIME = 1760822981500L; + public static final String BUILD_DATE = "2025-10-18 17:40:58 EDT"; + public static final long BUILD_UNIX_TIME = 1760823658603L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java index 5012b24..020cd22 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java @@ -383,8 +383,9 @@ public void periodic() { */ public ChassisSpeeds updateSpeedsWithDesiredHeading(ChassisSpeeds speeds, double targetHeading) { // tolerance of ±3 deg - boolean isWithinTolerance = - Math.abs(MathUtil.angleModulus(targetHeading - getRotation().getRadians())) <= 0.0523599; + double currentHeading = getRotation().getRadians(); + double error = MathUtil.angleModulus(targetHeading - currentHeading); + boolean isWithinTolerance = Math.abs(error) <= 3.0; if (isWithinTolerance) return new ChassisSpeeds( @@ -392,16 +393,12 @@ public ChassisSpeeds updateSpeedsWithDesiredHeading(ChassisSpeeds speeds, double speeds.vyMetersPerSecond, 0.0); // within tolerance; don't rotate - // otherwise, calculate omega - double omega = - headingPid.calculate(MathUtil.angleModulus(getRotation().getRadians()), targetHeading); + // calculate omega + double omega = headingPid.calculate(currentHeading, targetHeading); omega = MathUtil.clamp(omega, -getMaxAngularSpeedRadPerSec(), getMaxAngularSpeedRadPerSec()); headingPid.close(); - // update omega - speeds.omegaRadiansPerSecond = omega; - - return speeds; + return new ChassisSpeeds(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, omega); } /** From 1bad658af4d8957994b3a5b1851d580baeb10d1e Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 18 Oct 2025 14:46:35 -0700 Subject: [PATCH 9/9] change angle to radians instead of degerees --- src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java index 020cd22..4b81c28 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java @@ -385,7 +385,7 @@ public ChassisSpeeds updateSpeedsWithDesiredHeading(ChassisSpeeds speeds, double // tolerance of ±3 deg double currentHeading = getRotation().getRadians(); double error = MathUtil.angleModulus(targetHeading - currentHeading); - boolean isWithinTolerance = Math.abs(error) <= 3.0; + boolean isWithinTolerance = Math.abs(error) <= Math.toRadians(3.0); if (isWithinTolerance) return new ChassisSpeeds(