From 63b3a19bbf8d44d45c9d8a5539ce3f095575006d Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 15 Jan 2025 15:57:11 -0700 Subject: [PATCH 1/6] Add a command cancel in autoInit() Make sure all PathPlanner PIDs point to values in `Constants.java`. modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 3 +++ .../java/frc/robot/subsystems/drive/Drive.java | 14 ++++++-------- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6d6eeac6..9459f43c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -135,7 +135,7 @@ public static final class DrivebaseConstants { public static final double kMaxLinearAccel = 4.0; // m/s/s public static final double kMaxAngularAccel = Units.degreesToRadians(720); - // Drive and Turn PID constants + // Drive and Turn PID constants used for PathPlanner public static final PIDConstants drivePID = new PIDConstants(0.05, 0.0, 0.0); public static final PIDConstants steerPID = new PIDConstants(2.0, 0.0, 0.4); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f5bf2c26..61d2fd26 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -144,6 +144,9 @@ public void disabledPeriodic() { @Override public void autonomousInit() { + // Just in case, cancel all running commands + CommandScheduler.getInstance().cancelAll(); + // TODO: Make sure Gyro inits here with whatever is in the path planning thingie m_robotContainer.setMotorBrake(true); switch (Constants.getAutoType()) { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 7d468d9a..79b2375e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -22,7 +22,6 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.ModuleConfig; -import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; @@ -97,9 +96,6 @@ public class Drive extends SubsystemBase { private SwerveDrivePoseEstimator m_PoseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - // Choreo drive controller - private final PIDController xController = new PIDController(10.0, 0.0, 0.0); - // Constructor public Drive() { switch (Constants.getSwerveType()) { @@ -169,7 +165,7 @@ public Drive() { this::getChassisSpeeds, this::runVelocity, new PPHolonomicDriveController( - new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), + DrivebaseConstants.drivePID, DrivebaseConstants.steerPID), PP_CONFIG, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this); @@ -441,6 +437,7 @@ public Command resetOdometry(Pose2d orElseGet) { private final SwerveRequest.ApplyFieldSpeeds m_pathApplyFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds(); + // Choreo Controller Values private final PIDController m_pathXController = new PIDController(10, 0, 0); private final PIDController m_pathYController = new PIDController(10, 0, 0); private final PIDController m_pathThetaController = new PIDController(7, 0, 0); @@ -474,9 +471,10 @@ public void followTrajectory(SwerveSample sample) { // Generate the next speeds for the robot ChassisSpeeds speeds = new ChassisSpeeds( - sample.vx + xController.calculate(pose.getX(), sample.x), - sample.vy + xController.calculate(pose.getX(), sample.y), - sample.omega + xController.calculate(pose.getRotation().getRadians(), sample.heading)); + sample.vx + m_pathXController.calculate(pose.getX(), sample.x), + sample.vy + m_pathXController.calculate(pose.getX(), sample.y), + sample.omega + + m_pathXController.calculate(pose.getRotation().getRadians(), sample.heading)); // Apply the generated speeds runVelocity(speeds); From 0088bc8f74516a8693568c0769630e2335663a22 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 16 Jan 2025 15:04:23 -0700 Subject: [PATCH 2/6] Align PathPlanner calls to those in AK Template Aligned all of the PathPlanner calls and other bits in the `Robot.java` file to match what is in the AK2025 TalonFX Swerve Template. The latter works fine with PathPlanner, and here's hoping these adjustments make RBSI do the same! Also, a couple more vendordep updates. modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java modified: vendordeps/photonlib.json renamed: vendordeps/yagsl-2025.2.0.json -> vendordeps/yagsl-2025.2.1.json --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/Robot.java | 17 +++++------------ src/main/java/frc/robot/RobotContainer.java | 8 ++------ vendordeps/photonlib.json | 13 ++++++------- ...{yagsl-2025.2.0.json => yagsl-2025.2.1.json} | 10 +++++----- 5 files changed, 20 insertions(+), 32 deletions(-) rename vendordeps/{yagsl-2025.2.0.json => yagsl-2025.2.1.json} (91%) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9459f43c..56f5dae6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -136,8 +136,8 @@ public static final class DrivebaseConstants { public static final double kMaxAngularAccel = Units.degreesToRadians(720); // Drive and Turn PID constants used for PathPlanner - public static final PIDConstants drivePID = new PIDConstants(0.05, 0.0, 0.0); - public static final PIDConstants steerPID = new PIDConstants(2.0, 0.0, 0.4); + public static final PIDConstants drivePID = new PIDConstants(5.0, 0.0, 0.0); + public static final PIDConstants steerPID = new PIDConstants(5.0, 0.0, 0.0); // Hold time on motor brakes when disabled public static final double kWheelLockTime = 10; // seconds diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 61d2fd26..e800a3cf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -40,8 +40,7 @@ public class Robot extends LoggedRobot { * This function is run when the robot is first started up and should be used for any * initialization code. */ - @Override - public void robotInit() { + public Robot() { // Record metadata Logger.recordMetadata("Robot", Constants.getRobot().toString()); Logger.recordMetadata("TuningMode", Boolean.toString(Constants.tuningMode)); @@ -85,18 +84,14 @@ public void robotInit() { break; } - // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. - // Logger.disableDeterministicTimestamps() - // Start AdvantageKit logger Logger.start(); - // Instantiate our RobotContainer. This will perform all our button bindings, - // and put our autonomous chooser on the dashboard. + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - // Create a timer to disable motor brake a few seconds after disable. This will - // let the robot + // Create a timer to disable motor brake a few seconds after disable. This will let the robot // stop immediately when disabled, but then also let it be pushed more m_disabledTimer = new Timer(); } @@ -146,9 +141,9 @@ public void autonomousInit() { // Just in case, cancel all running commands CommandScheduler.getInstance().cancelAll(); + m_robotContainer.setMotorBrake(true); // TODO: Make sure Gyro inits here with whatever is in the path planning thingie - m_robotContainer.setMotorBrake(true); switch (Constants.getAutoType()) { case PATHPLANNER: m_autoCommandPathPlanner = m_robotContainer.getAutonomousCommandPathPlanner(); @@ -182,7 +177,6 @@ public void teleopInit() { } else { CommandScheduler.getInstance().cancelAll(); } - m_robotContainer.setDriveMode(); m_robotContainer.setMotorBrake(true); } @@ -195,7 +189,6 @@ public void teleopPeriodic() {} public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); - m_robotContainer.setDriveMode(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7cd9608a..b6411a59 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -185,12 +185,12 @@ public RobotContainer() { "Incorrect AUTO type selected in Constants: " + Constants.getAutoType()); } - // Configure the trigger bindings - configureBindings(); // Define Auto commands defineAutoCommands(); // Define SysIs Routines definesysIdRoutines(); + // Configure the button and trigger bindings + configureBindings(); } /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */ @@ -295,10 +295,6 @@ public void getAutonomousCommandChoreo() { RobotModeTriggers.autonomous().whileTrue(autoChooserChoreo.selectedCommandScheduler()); } - public void setDriveMode() { - configureBindings(); - } - /** Set the motor neutral mode to BRAKE / COAST for T/F */ public void setMotorBrake(boolean brake) { m_drivebase.setMotorBrake(brake); diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index f19551c2..a9089236 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,11 +1,10 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.1.1-rc1", + "version": "v2025.1.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ - "https://maven.photonvision.org/releases", "https://maven.photonvision.org/repository/internal", "https://maven.photonvision.org/repository/snapshots" ], @@ -14,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.1.1-rc1", + "version": "v2025.1.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -29,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.1.1-rc1", + "version": "v2025.1.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -44,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.1.1-rc1", + "version": "v2025.1.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -61,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.1.1-rc1" + "version": "v2025.1.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.1.1-rc1" + "version": "v2025.1.1" } ] } diff --git a/vendordeps/yagsl-2025.2.0.json b/vendordeps/yagsl-2025.2.1.json similarity index 91% rename from vendordeps/yagsl-2025.2.0.json rename to vendordeps/yagsl-2025.2.1.json index a0168ef2..8a2416a9 100644 --- a/vendordeps/yagsl-2025.2.0.json +++ b/vendordeps/yagsl-2025.2.1.json @@ -1,7 +1,7 @@ { - "fileName": "yagsl-2025.2.0.json", + "fileName": "yagsl-2025.2.1.json", "name": "YAGSL", - "version": "2025.2.0", + "version": "2025.2.1", "frcYear": "2025", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2025.2.0" + "version": "2025.2.1" } ], "requires": [ @@ -42,7 +42,7 @@ }, { "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "errorMessage": "NavX is required!", + "errorMessage": "Studica is required!", "offlineFileName": "Studica-2025.0.0.json", "onlineUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json" }, @@ -54,7 +54,7 @@ }, { "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", - "errorMessage": "MapleSim is requires for simulation", + "errorMessage": "maple-sim is required for simulation", "offlineFileName": "maple-sim.json", "onlineUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json" } From 26d27ce53eabf22f89e4e196db635371ec4d812e Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 17 Jan 2025 08:12:09 -0700 Subject: [PATCH 3/6] Remove gyro manual reset function Not sure if this is what's causing PathPlanner to behave oddly, but it's something not in the AK2025 templates. The other thing to try will be to remove the IMU accelerometer readout from the Accelerometer subsystem, with the thought that the extra read may be messing with the PathPlanner gyro settings. modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java modified: src/main/java/frc/robot/subsystems/drive/GyroIO.java modified: src/main/java/frc/robot/subsystems/drive/GyroIONavX.java modified: src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java --- src/main/java/frc/robot/RobotContainer.java | 5 ++- .../frc/robot/subsystems/drive/Drive.java | 10 +++-- .../frc/robot/subsystems/drive/GyroIO.java | 2 - .../robot/subsystems/drive/GyroIONavX.java | 38 +++++++++--------- .../robot/subsystems/drive/GyroIOPigeon2.java | 39 +++++++++---------- 5 files changed, 47 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b6411a59..bff7ac14 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,7 +26,6 @@ import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; @@ -164,6 +163,7 @@ public RobotContainer() { autoChooserChoreo = null; autoFactoryChoreo = null; break; + case CHOREO: autoFactoryChoreo = new AutoFactory( @@ -179,6 +179,7 @@ public RobotContainer() { // Set the others to null autoChooserPathPlanner = null; break; + default: // Then, throw the error throw new RuntimeException( @@ -196,7 +197,7 @@ public RobotContainer() { /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */ private void defineAutoCommands() { - NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.zero())); + // NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.zero())); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 79b2375e..88213fb9 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -301,10 +301,10 @@ public void runCharacterization(double output) { } } - /** Re-zero the gyro at the present heading */ - public void zero() { - gyroIO.zero(); - } + // /** Re-zero the gyro at the present heading */ + // public void zero() { + // gyroIO.zero(); + // } /** Stops the drive. */ public void stop() { @@ -428,6 +428,8 @@ public Object getGyro() { return gyroIO.getGyro(); } + /** CHOREO SECTION (Ignore if AutoType == PATHPLANNER) ******************* */ + /** Choreo: Reset odometry */ public Command resetOdometry(Pose2d orElseGet) { // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'resetOdometry'"); diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index c8c4d16f..93cf0b18 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -30,7 +30,5 @@ public static class GyroIOInputs { public default void updateInputs(GyroIOInputs inputs) {} - public default void zero() {} - public abstract T getGyro(); } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 6de5b723..219ee811 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -66,23 +66,23 @@ public void updateInputs(GyroIOInputs inputs) { yawPositionQueue.clear(); } - /** - * Zero the NavX - * - *

This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing, - * however, shows that it's not doing what I think it should be doing. There is likely - * interference with something else in the odometry - */ - @Override - public void zero() { - // With the Pigeon facing forward, forward depends on the alliance selected. - // Set Angle Adjustment based on alliance - if (DriverStation.getAlliance().get() == Alliance.Blue) { - navx.setAngleAdjustment(0.0); - } else { - navx.setAngleAdjustment(180.0); - } - System.out.println("Setting YAW to " + navx.getAngleAdjustment()); - navx.zeroYaw(); - } + // /** + // * Zero the NavX + // * + // *

This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing, + // * however, shows that it's not doing what I think it should be doing. There is likely + // * interference with something else in the odometry + // */ + // @Override + // public void zero() { + // // With the Pigeon facing forward, forward depends on the alliance selected. + // // Set Angle Adjustment based on alliance + // if (DriverStation.getAlliance().get() == Alliance.Blue) { + // navx.setAngleAdjustment(0.0); + // } else { + // navx.setAngleAdjustment(180.0); + // } + // System.out.println("Setting YAW to " + navx.getAngleAdjustment()); + // navx.zeroYaw(); + // } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index a4ae3d30..fe771eb6 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -26,8 +26,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.Queue; /** IO implementation for Pigeon2 */ @@ -60,6 +58,7 @@ public void updateInputs(GyroIOInputs inputs) { inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + inputs.odometryYawTimestamps = yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); inputs.odometryYawPositions = @@ -70,22 +69,22 @@ public void updateInputs(GyroIOInputs inputs) { yawPositionQueue.clear(); } - /** - * Zero the Pigeon2 - * - *

This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing, - * however, shows that it's not doing what I think it should be doing. There is likely - * interference with something else in the odometry - */ - @Override - public void zero() { - // With the Pigeon facing forward, forward depends on the alliance selected. - if (DriverStation.getAlliance().get() == Alliance.Blue) { - System.out.println("Alliance Blue: Setting YAW to 0"); - pigeon.setYaw(0.0); - } else { - System.out.println("Alliance Red: Setting YAW to 180"); - pigeon.setYaw(180.0); - } - } + // /** + // * Zero the Pigeon2 + // * + // *

This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing, + // * however, shows that it's not doing what I think it should be doing. There is likely + // * interference with something else in the odometry + // */ + // @Override + // public void zero() { + // // With the Pigeon facing forward, forward depends on the alliance selected. + // if (DriverStation.getAlliance().get() == Alliance.Blue) { + // System.out.println("Alliance Blue: Setting YAW to 0"); + // pigeon.setYaw(0.0); + // } else { + // System.out.println("Alliance Red: Setting YAW to 180"); + // pigeon.setYaw(180.0); + // } + // } } From 93c33eba87a2ceb7ee791527f3d7ef34275bd6f1 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 17 Jan 2025 17:53:47 -0700 Subject: [PATCH 4/6] Even more vendordeps, including AK 4.1.0 --- .../vision/VisionIOPhotonVision.java | 3 + vendordeps/AdvantageKit.json | 11 ++-- vendordeps/Phoenix6-frc2025-latest.json | 56 +++++++++---------- vendordeps/ThriftyLib.json | 4 +- 4 files changed, 39 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index e61fc331..df701639 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -90,6 +90,7 @@ public void updateInputs(VisionIOInputs inputs) { } else if (!result.targets.isEmpty()) { // Single tag result var target = result.targets.get(0); + // Calculate robot pose var tagPose = aprilTagLayout.getTagPose(target.fiducialId); if (tagPose.isPresent()) { @@ -99,8 +100,10 @@ public void updateInputs(VisionIOInputs inputs) { Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + // Add tag ID tagIds.add((short) target.fiducialId); + // Add observation poseObservations.add( new PoseObservation( diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index c7f5262f..fa81b2fc 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "4.0.0", + "version": "4.1.0", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2025", "mavenUrls": [ @@ -12,21 +12,22 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "4.0.0" + "version": "4.1.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "4.0.0", + "version": "4.1.0", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ "linuxathena", - "windowsx86-64", "linuxx86-64", - "osxuniversal" + "linuxarm64", + "osxuniversal", + "windowsx86-64" ] } ], diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index 4cd4250b..820c61a4 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.0", + "version": "25.2.1", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.0" + "version": "25.2.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json index 23948625..287f30cc 100644 --- a/vendordeps/ThriftyLib.json +++ b/vendordeps/ThriftyLib.json @@ -1,7 +1,7 @@ { "fileName": "ThriftyLib.json", "name": "ThriftyLib", - "version": "2025.0.1", + "version": "2025.0.2", "frcYear": "2025", "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.thethriftybot.frc", "artifactId": "ThriftyLib-java", - "version": "2025.0.1" + "version": "2025.0.2" } ], "jniDependencies": [], From 15e34bdf36ebbeae5c72a414db39f60e3dd1f42e Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 18 Jan 2025 16:55:31 -0700 Subject: [PATCH 5/6] Massage the PathPlanner generator In hopes this solves some of our driving issues. modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/generated/TunerConstants.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java new file: src/main/java/frc/robot/util/RBSIParsing.java renamed: vendordeps/yagsl-2025.2.1.json -> vendordeps/yagsl-2025.2.2.json --- src/main/java/frc/robot/Constants.java | 22 ++ src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/generated/TunerConstants.java | 8 +- .../frc/robot/subsystems/drive/Drive.java | 217 +++++++----------- src/main/java/frc/robot/util/RBSIParsing.java | 77 +++++++ ...agsl-2025.2.1.json => yagsl-2025.2.2.json} | 6 +- 6 files changed, 185 insertions(+), 147 deletions(-) create mode 100644 src/main/java/frc/robot/util/RBSIParsing.java rename vendordeps/{yagsl-2025.2.1.json => yagsl-2025.2.2.json} (96%) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 56f5dae6..75fac3cf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,17 +19,21 @@ import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.ObjectMapper; +import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; +import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.SwerveConstants; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; @@ -239,6 +243,24 @@ public static final class AutoConstants { public static final PIDConstants kAutoDrivePID = new PIDConstants(0.7, 0, 0); // PathPlanner Rotation PID constants public static final PIDConstants kAutoSteerPID = new PIDConstants(0.4, 0, 0.01); + + // PathPlanner Config constants + public static final RobotConfig kPathPlannerConfig = + new RobotConfig( + PhysicalConstants.kRobotMassKg, + PhysicalConstants.kRobotMOI, + new ModuleConfig( + SwerveConstants.kWheelRadiusMeters, + DrivebaseConstants.kMaxLinearSpeed, + PhysicalConstants.kWheelCOF, + DCMotor.getKrakenX60Foc(1).withReduction(SwerveConstants.kDriveGearRatio), + SwerveConstants.kDriveSlipCurrent, + 1), + Drive.getModuleTranslations()); + + // Alternatively, we can build this from the PathPlanner GUI: + // public static final RobotConfig kPathPlannerConfig = RobotConfig.fromGUISettings(); + } /** Vision Constants (Assuming PhotonVision) ***************************** */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bff7ac14..beb2eb19 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -258,7 +258,7 @@ private void configureBindings() { .onTrue( Commands.runOnce( () -> - m_drivebase.setPose( + m_drivebase.resetPose( new Pose2d(m_drivebase.getPose().getTranslation(), new Rotation2d())), m_drivebase) .ignoringDisable(true)); diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 4c4239af..929773ae 100755 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -138,7 +138,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 1; private static final int kFrontLeftSteerMotorId = 2; private static final int kFrontLeftEncoderId = 3; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.087158203125); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.088134765625); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -149,7 +149,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 4; private static final int kFrontRightSteerMotorId = 5; private static final int kFrontRightEncoderId = 6; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.410888671875); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.23779296875); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -160,7 +160,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 8; private static final int kBackLeftEncoderId = 9; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.1142578125); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.114990234375); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -171,7 +171,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 10; private static final int kBackRightSteerMotorId = 11; private static final int kBackRightEncoderId = 12; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.471923828125); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.47265625); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 88213fb9..40d8c521 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -21,8 +21,6 @@ import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.ModuleConfig; -import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; @@ -42,7 +40,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; @@ -51,10 +48,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; +import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DrivebaseConstants; -import frc.robot.Constants.PhysicalConstants; import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; +import frc.robot.util.RBSIParsing; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; @@ -62,20 +60,6 @@ public class Drive extends SubsystemBase { - // PathPlanner config constants - private static final RobotConfig PP_CONFIG = - new RobotConfig( - PhysicalConstants.kRobotMassKg, - PhysicalConstants.kRobotMOI, - new ModuleConfig( - kWheelRadiusMeters, - DrivebaseConstants.kMaxLinearSpeed, - PhysicalConstants.kWheelCOF, - DCMotor.getKrakenX60Foc(1).withReduction(kDriveGearRatio), - kDriveSlipCurrent, - 1), - getModuleTranslations()); - static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); @@ -98,6 +82,7 @@ public class Drive extends SubsystemBase { // Constructor public Drive() { + switch (Constants.getSwerveType()) { case PHOENIX6: // This one is easy because it's all CTRE all the time @@ -121,7 +106,7 @@ public Drive() { throw new RuntimeException("Invalid IMU type"); } // Then parse the module(s) - Byte modType = parseModuleType(); + Byte modType = RBSIParsing.parseModuleType(); for (int i = 0; i < 4; i++) { switch (modType) { case 0b00000000: // ALL-CTRE @@ -158,17 +143,22 @@ public Drive() { // Configure Autonomous Path Building for PathPlanner based on `AutoType` switch (Constants.getAutoType()) { case PATHPLANNER: - // Configure AutoBuilder for PathPlanner - AutoBuilder.configure( - this::getPose, - this::setPose, - this::getChassisSpeeds, - this::runVelocity, - new PPHolonomicDriveController( - DrivebaseConstants.drivePID, DrivebaseConstants.steerPID), - PP_CONFIG, - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this); + try { + // Configure AutoBuilder for PathPlanner + AutoBuilder.configure( + this::getPose, + this::resetPose, + this::getChassisSpeeds, + (speeds, feedforwards) -> runVelocity(speeds), + new PPHolonomicDriveController( + DrivebaseConstants.drivePID, DrivebaseConstants.steerPID), + AutoConstants.kPathPlannerConfig, + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this); + } catch (Exception e) { + DriverStation.reportError( + "Failed to load PathPlanner config and configure AutoBuilder", e.getStackTrace()); + } Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( (activePath) -> { @@ -199,6 +189,7 @@ public Drive() { (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); } + /** Periodic function that is called each robot cycle by the command scheduler */ @Override public void periodic() { odometryLock.lock(); // Prevents odometry updates while reading data @@ -257,6 +248,8 @@ public void periodic() { gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.getMode() != Mode.SIM); } + /** Drive Base Action Functions ****************************************** */ + /** * Sets the swerve drive motors to brake/coast mode. * @@ -270,6 +263,24 @@ public void setMotorBrake(boolean brake) { } } + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = getModuleTranslations()[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + /** * Runs the drive at the desired velocity. * @@ -301,28 +312,7 @@ public void runCharacterization(double output) { } } - // /** Re-zero the gyro at the present heading */ - // public void zero() { - // gyroIO.zero(); - // } - - /** Stops the drive. */ - public void stop() { - runVelocity(new ChassisSpeeds()); - } - - /** - * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will - * return to their normal orientations the next time a nonzero velocity is requested. - */ - public void stopWithX() { - Rotation2d[] headings = new Rotation2d[4]; - for (int i = 0; i < 4; i++) { - headings[i] = getModuleTranslations()[i].getAngle(); - } - kinematics.resetHeadings(headings); - stop(); - } + /** SysId Characterization Routines ************************************** */ /** Returns a command to run a quasistatic test in the specified direction. */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { @@ -336,6 +326,8 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); } + /** Getter Functions ***************************************************** */ + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { @@ -361,6 +353,27 @@ private ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return m_PoseEstimator.getEstimatedPosition(); + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d(kFLXPosMeters, kFLYPosMeters), + new Translation2d(kFRXPosMeters, kFRYPosMeters), + new Translation2d(kBLXPosMeters, kBLYPosMeters), + new Translation2d(kBRXPosMeters, kBRYPosMeters) + }; + } + /** Returns the position of each module in radians. */ public double[] getWheelRadiusCharacterizationPositions() { double[] values = new double[4]; @@ -379,19 +392,24 @@ public double getFFCharacterizationVelocity() { return output; } - /** Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Robot") - public Pose2d getPose() { - return m_PoseEstimator.getEstimatedPosition(); + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return DrivebaseConstants.kMaxLinearSpeed; } - /** Returns the current odometry rotation. */ - public Rotation2d getRotation() { - return getPose().getRotation(); + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return getMaxLinearSpeedMetersPerSec() / kDriveBaseRadiusMeters; + } + + public Object getGyro() { + return gyroIO.getGyro(); } + /* Setter Functions ****************************************************** */ + /** Resets the current odometry pose. */ - public void setPose(Pose2d pose) { + public void resetPose(Pose2d pose) { m_PoseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } @@ -404,30 +422,6 @@ public void addVisionMeasurement( visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } - /** Returns the maximum linear speed in meters per sec. */ - public double getMaxLinearSpeedMetersPerSec() { - return DrivebaseConstants.kMaxLinearSpeed; - } - - /** Returns the maximum angular speed in radians per sec. */ - public double getMaxAngularSpeedRadPerSec() { - return getMaxLinearSpeedMetersPerSec() / kDriveBaseRadiusMeters; - } - - /** Returns an array of module translations. */ - public static Translation2d[] getModuleTranslations() { - return new Translation2d[] { - new Translation2d(kFLXPosMeters, kFLYPosMeters), - new Translation2d(kFRXPosMeters, kFRYPosMeters), - new Translation2d(kBLXPosMeters, kBLYPosMeters), - new Translation2d(kBRXPosMeters, kBRYPosMeters) - }; - } - - public Object getGyro() { - return gyroIO.getGyro(); - } - /** CHOREO SECTION (Ignore if AutoType == PATHPLANNER) ******************* */ /** Choreo: Reset odometry */ public Command resetOdometry(Pose2d orElseGet) { @@ -481,59 +475,4 @@ public void followTrajectory(SwerveSample sample) { // Apply the generated speeds runVelocity(speeds); } - - /** - * Parse the module type given the type information for the FL module - * - * @return Byte The module type as bits in a byte. - */ - private Byte parseModuleType() { - // NOTE: This assumes all 4 modules have the same arrangement! - Byte b_drive; // [x,x,-,-,-,-,-,-] - Byte b_steer; // [-,-,x,x,-,-,-,-] - Byte b_encoder; // [-,-,-,-,x,x,-,-] - switch (kFLDriveType) { - case "falcon": - case "kraken": - case "talonfx": - // CTRE Drive Motor - b_drive = 0b00; - break; - case "sparkmax": - case "sparkflex": - // REV Drive Motor - b_drive = 0b01; - break; - default: - throw new RuntimeException("Invalid drive motor type"); - } - switch (kFLSteerType) { - case "falcon": - case "kraken": - case "talonfx": - // CTRE Drive Motor - b_steer = 0b00; - break; - case "sparkmax": - case "sparkflex": - // REV Drive Motor - b_steer = 0b01; - break; - default: - throw new RuntimeException("Invalid steer motor type"); - } - switch (kFLEncoderType) { - case "cancoder": - // CTRE CANcoder - b_encoder = 0b00; - break; - case "analog": - // Analog Encoder - b_encoder = 0b01; - break; - default: - throw new RuntimeException("Invalid swerve encoder type"); - } - return (byte) (0b00000000 | b_drive << 6 | b_steer << 4 | b_encoder << 2); - } } diff --git a/src/main/java/frc/robot/util/RBSIParsing.java b/src/main/java/frc/robot/util/RBSIParsing.java new file mode 100644 index 00000000..d61c5018 --- /dev/null +++ b/src/main/java/frc/robot/util/RBSIParsing.java @@ -0,0 +1,77 @@ +// Copyright (c) 2024-2025 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.util; + +import static frc.robot.subsystems.drive.SwerveConstants.kFLDriveType; +import static frc.robot.subsystems.drive.SwerveConstants.kFLEncoderType; +import static frc.robot.subsystems.drive.SwerveConstants.kFLSteerType; + +/** Container to hold various RBSI-specific parsing routines */ +public class RBSIParsing { + + /** + * Parse the module type given the type information for the FL module + * + * @return Byte The module type as bits in a byte. + */ + public static Byte parseModuleType() { + // NOTE: This assumes all 4 modules have the same arrangement! + Byte b_drive; // [x,x,-,-,-,-,-,-] + Byte b_steer; // [-,-,x,x,-,-,-,-] + Byte b_encoder; // [-,-,-,-,x,x,-,-] + switch (kFLDriveType) { + case "falcon": + case "kraken": + case "talonfx": + // CTRE Drive Motor + b_drive = 0b00; + break; + case "sparkmax": + case "sparkflex": + // REV Drive Motor + b_drive = 0b01; + break; + default: + throw new RuntimeException("Invalid drive motor type"); + } + switch (kFLSteerType) { + case "falcon": + case "kraken": + case "talonfx": + // CTRE Drive Motor + b_steer = 0b00; + break; + case "sparkmax": + case "sparkflex": + // REV Drive Motor + b_steer = 0b01; + break; + default: + throw new RuntimeException("Invalid steer motor type"); + } + switch (kFLEncoderType) { + case "cancoder": + // CTRE CANcoder + b_encoder = 0b00; + break; + case "analog": + // Analog Encoder + b_encoder = 0b01; + break; + default: + throw new RuntimeException("Invalid swerve encoder type"); + } + return (byte) (0b00000000 | b_drive << 6 | b_steer << 4 | b_encoder << 2); + } +} diff --git a/vendordeps/yagsl-2025.2.1.json b/vendordeps/yagsl-2025.2.2.json similarity index 96% rename from vendordeps/yagsl-2025.2.1.json rename to vendordeps/yagsl-2025.2.2.json index 8a2416a9..55fa43b7 100644 --- a/vendordeps/yagsl-2025.2.1.json +++ b/vendordeps/yagsl-2025.2.2.json @@ -1,7 +1,7 @@ { - "fileName": "yagsl-2025.2.1.json", + "fileName": "yagsl-2025.2.2.json", "name": "YAGSL", - "version": "2025.2.1", + "version": "2025.2.2", "frcYear": "2025", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2025.2.1" + "version": "2025.2.2" } ], "requires": [ From 25fe42ab50933ea238c5bb3e1cb339d41ecfefe4 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 20 Jan 2025 13:39:27 -0700 Subject: [PATCH 6/6] =?UTF-8?q?Mars=20Climate=20Orbiter=20=F0=9F=A4=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Assumed wheel radius value was in inches, so converted it to meters... problem is it was already in meters. Oops. This made the code think the wheel was tiny, hence needing to apply huge angular velocity to make the chassis run at the expected speed. Yeah... modified: src/main/deploy/pathplanner/paths/Consistancy Test.path modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/commands/ChoreoAutoController.java modified: src/main/java/frc/robot/commands/DriveCommands.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java modified: src/main/java/frc/robot/subsystems/drive/SwerveConstants.java renamed: vendordeps/ReduxLib-2025.0.0.json -> vendordeps/ReduxLib-2025.0.1.json --- .../deploy/pathplanner/paths/Consistancy Test.path | 6 +++--- src/main/java/frc/robot/Constants.java | 14 ++++++-------- .../frc/robot/commands/ChoreoAutoController.java | 6 +++--- .../java/frc/robot/commands/DriveCommands.java | 7 ++++--- .../java/frc/robot/subsystems/drive/Drive.java | 3 +-- .../robot/subsystems/drive/ModuleIOBlended.java | 8 ++++---- .../frc/robot/subsystems/drive/ModuleIOSpark.java | 14 +++++++------- .../subsystems/drive/ModuleIOSparkCANcoder.java | 14 +++++++------- .../robot/subsystems/drive/SwerveConstants.java | 6 ++++-- ...duxLib-2025.0.0.json => ReduxLib-2025.0.1.json} | 12 ++++++------ 10 files changed, 45 insertions(+), 45 deletions(-) rename vendordeps/{ReduxLib-2025.0.0.json => ReduxLib-2025.0.1.json} (89%) diff --git a/src/main/deploy/pathplanner/paths/Consistancy Test.path b/src/main/deploy/pathplanner/paths/Consistancy Test.path index 92b90b00..cc916709 100644 --- a/src/main/deploy/pathplanner/paths/Consistancy Test.path +++ b/src/main/deploy/pathplanner/paths/Consistancy Test.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 9.29, - "y": 6.22 + "x": 9.141987426415538, + "y": 6.318675049056306 }, "isLocked": false, "linkedName": null @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 0.5, "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 75fac3cf..08fb4a55 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -139,10 +139,6 @@ public static final class DrivebaseConstants { public static final double kMaxLinearAccel = 4.0; // m/s/s public static final double kMaxAngularAccel = Units.degreesToRadians(720); - // Drive and Turn PID constants used for PathPlanner - public static final PIDConstants drivePID = new PIDConstants(5.0, 0.0, 0.0); - public static final PIDConstants steerPID = new PIDConstants(5.0, 0.0, 0.0); - // Hold time on motor brakes when disabled public static final double kWheelLockTime = 10; // seconds @@ -239,10 +235,9 @@ public static class OperatorConstants { /** Autonomous Action Constants ****************************************** */ public static final class AutoConstants { - // PathPlanner Translation PID constants - public static final PIDConstants kAutoDrivePID = new PIDConstants(0.7, 0, 0); - // PathPlanner Rotation PID constants - public static final PIDConstants kAutoSteerPID = new PIDConstants(0.4, 0, 0.01); + // Drive and Turn PID constants used for PathPlanner + public static final PIDConstants kPPdrivePID = new PIDConstants(5.0, 0.0, 0.0); + public static final PIDConstants kPPsteerPID = new PIDConstants(5.0, 0.0, 0.0); // PathPlanner Config constants public static final RobotConfig kPathPlannerConfig = @@ -261,6 +256,9 @@ public static final class AutoConstants { // Alternatively, we can build this from the PathPlanner GUI: // public static final RobotConfig kPathPlannerConfig = RobotConfig.fromGUISettings(); + // Drive and Turn PID constants used for Chorep + public static final PIDConstants kChoreoDrivePID = new PIDConstants(10.0, 0.0, 0.0); + public static final PIDConstants kChoreoSteerPID = new PIDConstants(7.5, 0.0, 0.0); } /** Vision Constants (Assuming PhotonVision) ***************************** */ diff --git a/src/main/java/frc/robot/commands/ChoreoAutoController.java b/src/main/java/frc/robot/commands/ChoreoAutoController.java index 852cd6b6..ebe9c71b 100644 --- a/src/main/java/frc/robot/commands/ChoreoAutoController.java +++ b/src/main/java/frc/robot/commands/ChoreoAutoController.java @@ -27,11 +27,11 @@ public class ChoreoAutoController implements Consumer { private final Drive drive; // drive subsystem private final PIDController xController = - new PIDController(kAutoDrivePID.kP, 0.0, kAutoDrivePID.kD); + new PIDController(kChoreoDrivePID.kP, kChoreoDrivePID.kI, kChoreoDrivePID.kD); private final PIDController yController = - new PIDController(kAutoDrivePID.kP, 0.0, kAutoDrivePID.kD); + new PIDController(kChoreoDrivePID.kP, kChoreoDrivePID.kI, kChoreoDrivePID.kD); private final PIDController headingController = - new PIDController(kAutoSteerPID.kP, 0.0, kAutoSteerPID.kD); + new PIDController(kChoreoSteerPID.kP, kChoreoSteerPID.kI, kChoreoSteerPID.kD); public ChoreoAutoController(Drive drive) { this.drive = drive; diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 7909b911..5421e249 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DrivebaseConstants; import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.drive.Drive; @@ -125,9 +126,9 @@ public static Command fieldRelativeDriveAtAngle( // Create PID controller ProfiledPIDController angleController = new ProfiledPIDController( - DrivebaseConstants.steerPID.kP, - DrivebaseConstants.steerPID.kI, - DrivebaseConstants.steerPID.kD, + AutoConstants.kPPsteerPID.kP, + AutoConstants.kPPsteerPID.kI, + AutoConstants.kPPsteerPID.kD, new TrapezoidProfile.Constraints( DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); angleController.enableContinuousInput(-Math.PI, Math.PI); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 40d8c521..63664b68 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -150,8 +150,7 @@ public Drive() { this::resetPose, this::getChassisSpeeds, (speeds, feedforwards) -> runVelocity(speeds), - new PPHolonomicDriveController( - DrivebaseConstants.drivePID, DrivebaseConstants.steerPID), + new PPHolonomicDriveController(AutoConstants.kPPdrivePID, AutoConstants.kPPsteerPID), AutoConstants.kPathPlannerConfig, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index d7f6e9da..2ffe7fa8 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -53,7 +53,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants; -import frc.robot.Constants.DrivebaseConstants; +import frc.robot.Constants.AutoConstants; import frc.robot.util.PhoenixUtil; import frc.robot.util.SparkUtil; import java.util.Queue; @@ -205,9 +205,9 @@ public ModuleIOBlended(int module) { .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) .pidf( - DrivebaseConstants.steerPID.kP, - DrivebaseConstants.steerPID.kI, - DrivebaseConstants.steerPID.kD, + AutoConstants.kPPsteerPID.kP, + AutoConstants.kPPsteerPID.kI, + AutoConstants.kPPsteerPID.kD, 0.0); turnConfig .signals diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 841ac551..83d9d4aa 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -37,7 +37,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.Constants.DrivebaseConstants; +import frc.robot.Constants.AutoConstants; import java.util.Queue; import java.util.function.DoubleSupplier; @@ -135,9 +135,9 @@ public ModuleIOSpark(int module) { .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pidf( - DrivebaseConstants.drivePID.kP, - DrivebaseConstants.drivePID.kI, - DrivebaseConstants.drivePID.kD, + AutoConstants.kPPdrivePID.kP, + AutoConstants.kPPdrivePID.kI, + AutoConstants.kPPdrivePID.kD, 0.0); driveConfig .signals @@ -175,9 +175,9 @@ public ModuleIOSpark(int module) { .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) .pidf( - DrivebaseConstants.steerPID.kP, - DrivebaseConstants.steerPID.kI, - DrivebaseConstants.steerPID.kD, + AutoConstants.kPPsteerPID.kP, + AutoConstants.kPPsteerPID.kI, + AutoConstants.kPPsteerPID.kD, 0.0); turnConfig .signals diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index ce7744c3..f248e2bb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -42,7 +42,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import frc.robot.Constants.DrivebaseConstants; +import frc.robot.Constants.AutoConstants; import java.util.Queue; import java.util.function.DoubleSupplier; @@ -155,9 +155,9 @@ public ModuleIOSparkCANcoder(int module) { .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pidf( - DrivebaseConstants.drivePID.kP, - DrivebaseConstants.drivePID.kI, - DrivebaseConstants.drivePID.kD, + AutoConstants.kPPdrivePID.kP, + AutoConstants.kPPdrivePID.kI, + AutoConstants.kPPdrivePID.kD, 0.0); driveConfig .signals @@ -195,9 +195,9 @@ public ModuleIOSparkCANcoder(int module) { .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) .pidf( - DrivebaseConstants.steerPID.kP, - DrivebaseConstants.steerPID.kI, - DrivebaseConstants.steerPID.kD, + AutoConstants.kPPsteerPID.kP, + AutoConstants.kPPsteerPID.kI, + AutoConstants.kPPsteerPID.kD, 0.0); turnConfig .signals diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index d5119272..cffffbf9 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -33,6 +33,7 @@ public class SwerveConstants { public static final double kDriveGearRatio; public static final double kSteerGearRatio; public static final double kWheelRadiusInches; + public static final double kWheelRadiusMeters; public static final String kCANbusName; public static final int kPigeonId; public static final double kSteerInertia; @@ -112,7 +113,8 @@ public class SwerveConstants { kCoupleRatio = TunerConstants.FrontLeft.CouplingGearRatio; kDriveGearRatio = TunerConstants.FrontLeft.DriveMotorGearRatio; kSteerGearRatio = TunerConstants.FrontLeft.SteerMotorGearRatio; - kWheelRadiusInches = TunerConstants.FrontLeft.WheelRadius; + kWheelRadiusMeters = TunerConstants.FrontLeft.WheelRadius; + kWheelRadiusInches = Units.metersToInches(kWheelRadiusMeters); kCANbusName = TunerConstants.DrivetrainConstants.CANBusName; kPigeonId = TunerConstants.DrivetrainConstants.Pigeon2Id; kSteerInertia = TunerConstants.FrontLeft.SteerInertia; @@ -197,6 +199,7 @@ public class SwerveConstants { kDriveGearRatio = YagslConstants.kDriveGearRatio; kSteerGearRatio = YagslConstants.kSteerGearRatio; kWheelRadiusInches = YagslConstants.kWheelRadiusInches; + kWheelRadiusMeters = Units.inchesToMeters(kWheelRadiusInches); kCANbusName = YagslConstants.kCANbusName; kPigeonId = YagslConstants.kPigeonId; kSteerInertia = YagslConstants.kSteerInertia; @@ -286,7 +289,6 @@ public class SwerveConstants { Math.max( Math.hypot(kBLXPosMeters, kBLYPosMeters), Math.hypot(kBRXPosMeters, kBRYPosMeters))); public static final double kDriveBaseRadiusInches = Units.metersToInches(kDriveBaseRadiusMeters); - public static final double kWheelRadiusMeters = Units.inchesToMeters(kWheelRadiusInches); // Are we on the CANivore or not? public static final double kOdometryFrequency = diff --git a/vendordeps/ReduxLib-2025.0.0.json b/vendordeps/ReduxLib-2025.0.1.json similarity index 89% rename from vendordeps/ReduxLib-2025.0.0.json rename to vendordeps/ReduxLib-2025.0.1.json index 18ff5a11..3a66d722 100644 --- a/vendordeps/ReduxLib-2025.0.0.json +++ b/vendordeps/ReduxLib-2025.0.1.json @@ -1,7 +1,7 @@ { - "fileName": "ReduxLib-2025.0.0.json", + "fileName": "ReduxLib-2025.0.1.json", "name": "ReduxLib", - "version": "2025.0.0", + "version": "2025.0.1", "frcYear": "2025", "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-java", - "version": "2025.0.0" + "version": "2025.0.1" } ], "jniDependencies": [ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-driver", - "version": "2025.0.0", + "version": "2025.0.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-cpp", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "ReduxLib", "headerClassifier": "headers", "sourcesClassifier": "sources", @@ -54,7 +54,7 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-driver", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "ReduxCore", "headerClassifier": "headers", "sharedLibrary": true,