From 89a8a659dd2d49ed05f7d37f8fa6cac0063fed9b Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 20 Dec 2024 12:22:20 -0700 Subject: [PATCH 01/30] Power monitoring now works Power monitoring now available in logs with consolidated mechanism current (e.g., drive vs steer vs other subsystems), a measure of total Amp-hours used since the start of the program, and an estimate percentage of battery charge left, with a settable starting Ah in the SmartDashboard. modified: INSTALL.md modified: README.md 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: src/main/java/frc/robot/util/PowerMonitoring.java --- INSTALL.md | 12 +++- README.md | 5 ++ src/main/java/frc/robot/Constants.java | 3 - src/main/java/frc/robot/Robot.java | 5 -- src/main/java/frc/robot/RobotContainer.java | 23 +++----- .../java/frc/robot/util/PowerMonitoring.java | 55 ++++++++++++++----- 6 files changed, 67 insertions(+), 36 deletions(-) diff --git a/INSTALL.md b/INSTALL.md index d6087910..ce8cb1c8 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -44,7 +44,17 @@ steps you need to complete: generic Az-RBSI template contains a team number "0", and your code will not deploy properly if this variable is not set (*e.g.*, VSCode looks for the RoboRIO on IP address `10.TE.AM.2`, and it will not find anything if it - tries to contact `10.0.0.2`.) + tries to contact `10.0.0.2`.) If you forget to change this value, you will + get an error message when deploying code to your robot like: + + ``` + Missing Target! + ============================================= + Are you connected to the robot, and is it on? + ============================================= + GradleRIO detected this build failed due to not being able to find "roborio"! + Scroll up in this error log for more information. + ``` 2. If you have an all-CTRE swerve base (*i.e.*, 8x TalonFX-controlled motors, 4x CANCoders, and 1x Pigeon2), use Phoenix Tuner X to create a swerve diff --git a/README.md b/README.md index af25f09d..e923c7fd 100644 --- a/README.md +++ b/README.md @@ -53,3 +53,8 @@ effective logging for troubleshooting. * [PhotonVision](https://docs.photonvision.org/en/latest/) / [Limelight]( https://docs.limelightvision.io/docs/docs-limelight/getting-started/summary) -- Robot vision / tracking + +## Further Reading + +For tips on command-based programming, see this post: +https://bovlb.github.io/frc-tips/commands/best-practices.html diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8daa1dce..0721a04c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -30,7 +30,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.generated.TunerConstants; import frc.robot.util.Alert; @@ -102,8 +101,6 @@ public static final class PhysicalConstants { /** Power Distribution Constants ********************************** */ public static final class PowerConstants { - // Set this to either kRev or kCTRE for the type of Power Distribution Module - public static final ModuleType kPowerModule = ModuleType.kRev; // Current Limits public static final double kTotalMaxCurrent = 120.; public static final double kMotorPortMaxCurrent = 40.; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7389245a..2b702ef2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,13 +18,10 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.Constants.PowerConstants; -import frc.robot.RobotContainer.Ports; import frc.robot.util.VirtualSubsystem; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.inputs.LoggedPowerDistribution; import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; @@ -74,8 +71,6 @@ public void robotInit() { // Running on a real robot, log to a USB stick ("/U/logs") Logger.addDataReceiver(new WPILOGWriter()); Logger.addDataReceiver(new NT4Publisher()); - LoggedPowerDistribution.getInstance( - Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerConstants.kPowerModule); break; case SIM: diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a4047227..eaec1f6d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -44,7 +44,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; -import frc.robot.Constants.PowerConstants; import frc.robot.commands.ChoreoAutoController; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.accelerometer.Accelerometer; @@ -79,8 +78,10 @@ public class RobotContainer { Field2d m_field = new Field2d(); // Declare the robot subsystems here + // These are the "Active Subsystems" that the robot controlls private final Drive m_drivebase; private final Flywheel m_flywheel; + // These are "Virtual Subsystems" that report information but have no motors private final Accelerometer m_accel; private final Vision m_vision; private final PowerMonitoring m_power; @@ -91,6 +92,9 @@ public class RobotContainer { private final AutoChooser autoChooserChoreo; private final AutoFactory autoFactoryChoreo; private final ChoreoAutoController choreoController; + // Input estimated battery capacity (if full, use printed value) + private final LoggedTunableNumber batteryCapacity = + new LoggedTunableNumber("Battery Amp-Hours", 18.0); // EXAMPLE TUNABLE FLYWHEEL SPEED INPUT FROM DASHBOARD private final LoggedTunableNumber flywheelSpeedInput = new LoggedTunableNumber("Flywheel Speed", 1500.0); @@ -108,7 +112,6 @@ public static AprilTagLayoutType staticGetAprilTagLayoutType() { } /** The container for the robot. Contains subsystems, OI devices, and commands. */ - @SuppressWarnings("unchecked") public RobotContainer() { // Instantiate Robot Subsystems based on RobotType @@ -160,10 +163,10 @@ public RobotContainer() { break; } - // ``PowerMonitoring`` takes all the non-drivebase subsystems for which - // you wish to have power monitoring; DO NOT include ``m_drivebase``, - // as that is automatically monitored. - m_power = null; // new PowerMonitoring(m_flywheel); + // In addition to the initial battery capacity from the Dashbaord, ``PowerMonitoring`` takes all + // the non-drivebase subsystems for which you wish to have power monitoring; DO NOT include + // ``m_drivebase``, as that is automatically monitored. + m_power = new PowerMonitoring(batteryCapacity, m_flywheel); // Set up the SmartDashboard Auto Chooser based on auto type switch (Constants.getAutoType()) { @@ -371,14 +374,6 @@ public static class Ports { public static final RobotDeviceId PIGEON = new RobotDeviceId(13, "DriveTrain", null); - /* POWER DISTRIBUTION CAN ID (set by device type in PowerConstants) */ - public static final RobotDeviceId POWER_CAN_DEVICE_ID = - switch (PowerConstants.kPowerModule) { - case kRev -> new RobotDeviceId(1, null); - case kCTRE -> new RobotDeviceId(0, null); - default -> null; - }; - /* SUBSYSTEM CAN DEVICE IDS */ // This is where mechanism subsystem devices are defined // Example: diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java index 735054e2..0b7f12c9 100644 --- a/src/main/java/frc/robot/util/PowerMonitoring.java +++ b/src/main/java/frc/robot/util/PowerMonitoring.java @@ -13,12 +13,14 @@ package frc.robot.util; -import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.PowerConstants; import frc.robot.RobotContainer.Ports; import frc.robot.util.Alert.AlertType; +import org.littletonrobotics.conduit.ConduitApi; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggedPowerDistribution; /** * Power monitoring virtual subsystem that periodically polls the Power Distribution Module. Each @@ -30,13 +32,23 @@ public class PowerMonitoring extends VirtualSubsystem { private final RBSISubsystem[] subsystems; - /** Define the Power Distribution Hardware */ - private PowerDistribution m_powerModule = - new PowerDistribution( - Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerConstants.kPowerModule); + // Get the AdvantageKit conduit for pulling PDM information + private LoggedPowerDistribution loggedPowerDistribution = LoggedPowerDistribution.getInstance(); + private ConduitApi conduit = ConduitApi.getInstance(); - private int NUM_PDH_CHANNELS = m_powerModule.getNumChannels(); + // Define local variables + private int NUM_PDH_CHANNELS = conduit.getPDPChannelCount(); private double[] channelCurrents = new double[NUM_PDH_CHANNELS]; + private double totalAmpHours = 0.0; + private long lastTimestamp = RobotController.getFPGATime(); // In microseconds + private double deltaTime = 0.0; // In seconds + private double batteryVoltage; + private double totalCurrent; + private double driveCurrent; + private double steerCurrent; + private double subsystemCurrent; + private LoggedTunableNumber batteryCapacity; + private double batteryPercent; // DRIVE and STEER motor power ports private final int[] m_drivePowerPorts = { @@ -53,7 +65,8 @@ public class PowerMonitoring extends VirtualSubsystem { }; // Class method definition, including inputs of optional subsystems - public PowerMonitoring(RBSISubsystem... subsystems) { + public PowerMonitoring(LoggedTunableNumber batteryCapacity, RBSISubsystem... subsystems) { + this.batteryCapacity = batteryCapacity; this.subsystems = subsystems; } @@ -61,20 +74,30 @@ public PowerMonitoring(RBSISubsystem... subsystems) { public void periodic() { // Check the total robot current and individual port currents against Constants - double totalCurrent = m_powerModule.getTotalCurrent(); + batteryVoltage = conduit.getPDPVoltage(); + totalCurrent = conduit.getPDPTotalCurrent(); if (totalCurrent > PowerConstants.kTotalMaxCurrent) { new Alert("Total current draw exceeds limit!", AlertType.WARNING).set(true); } for (int i = 0; i < NUM_PDH_CHANNELS; i++) { - channelCurrents[i] = m_powerModule.getCurrent(i); + channelCurrents[i] = conduit.getPDPChannelCurrent(i); if (channelCurrents[i] > PowerConstants.kMotorPortMaxCurrent) { new Alert("Port " + i + " current draw exceeds limit!", AlertType.WARNING).set(true); } } + // Compute the total energy (charge) used in this loop + deltaTime = + (RobotController.getFPGATime() - lastTimestamp) / 1.0e6; // Time in seconds since last loop + lastTimestamp = RobotController.getFPGATime(); // Update timestamp + totalAmpHours += (totalCurrent * deltaTime / 3600.); + + // Compute estimated battery life left as a percent + batteryPercent = + (batteryCapacity.getAsDouble() - totalAmpHours) / batteryCapacity.getAsDouble() * 100; // Compute DRIVE and STEER summed currents - double driveCurrent = 0.0; - double steerCurrent = 0.0; + driveCurrent = 0.0; + steerCurrent = 0.0; for (int port : m_drivePowerPorts) { driveCurrent += channelCurrents[port]; } @@ -82,16 +105,22 @@ public void periodic() { steerCurrent += channelCurrents[port]; } // Log values to AdvantageKit and to SmartDashboard + Logger.recordOutput("PowerMonitor/Voltage", batteryVoltage); + Logger.recordOutput("PowerMonitor/EstimatedBatteryPercent", batteryPercent); + Logger.recordOutput("PowerMonitor/AmpHoursUsed", totalAmpHours); Logger.recordOutput("PowerMonitor/TotalCurrent", totalCurrent); Logger.recordOutput("PowerMonitor/DriveCurrent", driveCurrent); Logger.recordOutput("PowerMonitor/SteerCurrent", steerCurrent); + SmartDashboard.putNumber("BatteryVoltage", batteryVoltage); + SmartDashboard.putNumber("EstimatedBatteryPercent", batteryPercent); + SmartDashboard.putNumber("AmpHoursUsed", totalAmpHours); SmartDashboard.putNumber("TotalCurrent", totalCurrent); SmartDashboard.putNumber("DriveCurrent", driveCurrent); SmartDashboard.putNumber("SteerCurrent", steerCurrent); // Compute and log any passed-in subsystems for (RBSISubsystem subsystem : subsystems) { - double subsystemCurrent = 0.0; + subsystemCurrent = 0.0; for (int port : subsystem.getPowerPorts()) { subsystemCurrent += channelCurrents[port]; } @@ -99,7 +128,7 @@ public void periodic() { SmartDashboard.putNumber(subsystem.getName() + "Current", subsystemCurrent); } - // Do something about setting priorities if drawing too much current + // TODO: Do something about setting priorities if drawing too much current } } From c25f4bb6d7c86a581eee45743b5dfaabb446f51d Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 20 Dec 2024 12:37:10 -0700 Subject: [PATCH 02/30] Adjust CI to run on PR's to both `main` and `develop` modified: .github/workflows/main.yml --- .github/workflows/main.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index af1c2e1b..0c9bc405 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -6,9 +6,9 @@ name: CI # events but only for the main branch. on: push: - branches: [ main ] + branches: [ main, develop ] pull_request: - branches: [ main ] + branches: [ main, develop ] # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: From 694931a92173dc99587cb423bb8beafa1c80cdf7 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 20 Dec 2024 09:35:24 -0700 Subject: [PATCH 03/30] Update vendoreps; fix Choreo Udate vendor libraries to latest beta. Fix resulting breaking changes for Choreo. modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java modified: vendordeps/ChoreoLib2025Beta.json modified: vendordeps/PathplannerLib-beta.json modified: vendordeps/REVLib.json modified: vendordeps/ReduxLib_2025.json modified: vendordeps/yagsl.json --- src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 72 +++++++++---------- .../frc/robot/subsystems/drive/Drive.java | 18 +++++ vendordeps/ChoreoLib2025Beta.json | 6 +- vendordeps/PathplannerLib-beta.json | 6 +- vendordeps/REVLib.json | 10 +-- vendordeps/ReduxLib_2025.json | 27 +++++-- vendordeps/yagsl.json | 4 +- 8 files changed, 90 insertions(+), 59 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2b702ef2..b5799700 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -155,11 +155,7 @@ public void autonomousInit() { } break; case CHOREO: - m_autoCommandChoreo = m_robotContainer.getAutonomousCommandChoreo(); - // schedule the autonomous command (example) - if (m_autoCommandChoreo != null) { - CommandScheduler.getInstance().schedule(m_autoCommandChoreo.cmd()); - } + m_robotContainer.getAutonomousCommandChoreo(); break; default: throw new RuntimeException( diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eaec1f6d..ff9b5fab 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,6 @@ import static frc.robot.subsystems.vision.VisionConstants.*; -import choreo.Choreo; import choreo.auto.AutoChooser; import choreo.auto.AutoFactory; import choreo.auto.AutoFactory.AutoBindings; @@ -29,7 +28,6 @@ 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.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; @@ -38,13 +36,14 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; -import frc.robot.commands.ChoreoAutoController; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.drive.Drive; @@ -91,7 +90,6 @@ public class RobotContainer { private final LoggedDashboardChooser autoChooserPathPlanner; private final AutoChooser autoChooserChoreo; private final AutoFactory autoFactoryChoreo; - private final ChoreoAutoController choreoController; // Input estimated battery capacity (if full, use printed value) private final LoggedTunableNumber batteryCapacity = new LoggedTunableNumber("Battery Amp-Hours", 18.0); @@ -176,21 +174,20 @@ public RobotContainer() { // Set the others to null autoChooserChoreo = null; autoFactoryChoreo = null; - choreoController = null; break; case CHOREO: - choreoController = new ChoreoAutoController(m_drivebase); autoFactoryChoreo = - Choreo.createAutoFactory( + new AutoFactory( m_drivebase::getPose, // A function that returns the current robot pose - choreoController, // The controller for the drive subsystem - this::isRedAlliance, // A function that returns true if the robot is on the red - // alliance - m_drivebase, - new AutoBindings() // An empty `AutoBindings` object, you can learn more below + m_drivebase::resetOdometry, // A function that resets the current robot pose to the + // provided Pose2d + m_drivebase::followTrajectory, // The drive subsystem trajectory follower + true, // If alliance flipping should be enabled + m_drivebase, // The drive subsystem + new AutoBindings() // An empty AutoBindings object ); - autoChooserChoreo = new AutoChooser(autoFactoryChoreo, ""); - autoChooserChoreo.addAutoRoutine("twoPieceAuto", this::twoPieceAuto); + autoChooserChoreo = new AutoChooser(); + autoChooserChoreo.addRoutine("twoPieceAuto", this::twoPieceAuto); // Set the others to null autoChooserPathPlanner = null; break; @@ -322,8 +319,12 @@ public Command getAutonomousCommandPathPlanner() { * * @return the command to run in autonomous */ - public AutoRoutine getAutonomousCommandChoreo() { - return autoChooserChoreo.getSelectedAutoRoutine(); + public void getAutonomousCommandChoreo() { + // Put the auto chooser on the dashboard + SmartDashboard.putData(autoChooserChoreo); + + // Schedule the selected auto during the autonomous period + RobotModeTriggers.autonomous().whileTrue(autoChooserChoreo.selectedCommandScheduler()); } public void setDriveMode() { @@ -435,28 +436,27 @@ public static class Cameras { * *

NOTE: This would normally be in a spearate file. */ - private AutoRoutine twoPieceAuto(AutoFactory factory) { - final AutoRoutine routine = factory.newRoutine("twoPieceAuto"); + private AutoRoutine twoPieceAuto() { + AutoRoutine routine = autoFactoryChoreo.newRoutine("twoPieceAuto"); - final AutoTrajectory trajectory = routine.trajectory("twoPieceAuto"); + // Load the routine's trajectories + AutoTrajectory pickupTraj = routine.trajectory("pickupGamepiece"); + AutoTrajectory scoreTraj = routine.trajectory("scoreGamepiece"); - routine - .running() - .onTrue( - m_drivebase - .resetOdometry( - trajectory - .getInitialPose() - .orElseGet( - () -> { - routine.kill(); - return new Pose2d(); - })) - .andThen(trajectory.cmd()) - .withName("twoPieceAuto entry point")); - - // trajectory.atTime("intake").onTrue(intake.extend()); - // trajectory.atTime("shoot").onTrue(shooter.launch()); + // When the routine begins, reset odometry and start the first trajectory + routine.active().onTrue(Commands.sequence(routine.resetOdometry(pickupTraj), pickupTraj.cmd())); + + // Starting at the event marker named "intake", run the intake + // pickupTraj.atTime("intake").onTrue(intakeSubsystem.intake()); + + // When the trajectory is done, start the next trajectory + pickupTraj.done().onTrue(scoreTraj.cmd()); + + // While the trajectory is active, prepare the scoring subsystem + // scoreTraj.active().whileTrue(scoringSubsystem.getReady()); + + // When the trajectory is done, score + // scoreTraj.done().onTrue(scoringSubsystem.score()); return routine; } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 26700e39..bb004e7c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -105,6 +105,9 @@ 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()) { @@ -468,6 +471,21 @@ public void choreoController(Pose2d pose, SwerveSample sample) { // .withWheelForceFeedforwardsY(sample.moduleForcesY())); } + public void followTrajectory(SwerveSample sample) { + // Get the current pose of the robot + Pose2d pose = getPose(); + + // 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)); + + // Apply the generated speeds + runVelocity(speeds); + } + /** * Parse the module type given the type information for the FL module * diff --git a/vendordeps/ChoreoLib2025Beta.json b/vendordeps/ChoreoLib2025Beta.json index e3682576..914a8593 100644 --- a/vendordeps/ChoreoLib2025Beta.json +++ b/vendordeps/ChoreoLib2025Beta.json @@ -1,7 +1,7 @@ { "fileName": "ChoreoLib2025Beta.json", "name": "ChoreoLib", - "version": "2025.0.0-beta-7", + "version": "2025.0.0-beta-9", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2025.0.0-beta-7" + "version": "2025.0.0-beta-9" }, { "groupId": "com.google.code.gson", @@ -26,7 +26,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2025.0.0-beta-7", + "version": "2025.0.0-beta-9", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/PathplannerLib-beta.json b/vendordeps/PathplannerLib-beta.json index 3c999183..35959504 100644 --- a/vendordeps/PathplannerLib-beta.json +++ b/vendordeps/PathplannerLib-beta.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib-beta.json", "name": "PathplannerLib", - "version": "2025.0.0-beta-5", + "version": "2025.0.0-beta-6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.0.0-beta-5" + "version": "2025.0.0-beta-6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.0.0-beta-5", + "version": "2025.0.0-beta-6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 440b1324..6b3a1574 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.0-beta-3", + "version": "2025.0.0-beta-4", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.0-beta-3" + "version": "2025.0.0-beta-4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0-beta-3", + "version": "2025.0.0-beta-4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.0-beta-3", + "version": "2025.0.0-beta-4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0-beta-3", + "version": "2025.0.0-beta-4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/ReduxLib_2025.json b/vendordeps/ReduxLib_2025.json index b5e4a1f4..5305e85a 100644 --- a/vendordeps/ReduxLib_2025.json +++ b/vendordeps/ReduxLib_2025.json @@ -1,7 +1,7 @@ { "fileName": "ReduxLib_2025.json", "name": "ReduxLib", - "version": "2025.0.0-beta0", + "version": "2025.0.0-beta2", "frcYear": 2025, "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-java", - "version": "2025.0.0-beta0" + "version": "2025.0.0-beta2" } ], "jniDependencies": [ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-driver", - "version": "2025.0.0-beta0", + "version": "2025.0.0-beta2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -36,8 +36,8 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-cpp", - "version": "2025.0.0-beta0", - "libName": "ReduxLib-cpp", + "version": "2025.0.0-beta2", + "libName": "ReduxLib", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": true, @@ -50,6 +50,23 @@ "osxuniversal", "windowsx86-64" ] + }, + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.0-beta2", + "libName": "ReduxCore", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] } ] } diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json index 1b3bc77a..cb05dc8d 100644 --- a/vendordeps/yagsl.json +++ b/vendordeps/yagsl.json @@ -1,7 +1,7 @@ { "fileName": "yagsl.json", "name": "YAGSL", - "version": "2025.1.0.0.10", + "version": "2025.1.0.4", "frcYear": "2025", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2025.1.0.0.10" + "version": "2025.1.0.4" } ], "requires": [ From a49575515955d8ecf87386d154d4ba02408ee145 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 20 Dec 2024 13:12:22 -0700 Subject: [PATCH 04/30] Update function calls deprecated in 2025 WPILib has deprecated several standard function calls this year; this commit adjusts the calls to match the recommended new format. 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 --- src/main/java/frc/robot/Constants.java | 2 +- .../robot/commands/ChoreoAutoController.java | 6 +++-- .../frc/robot/commands/DriveCommands.java | 25 ++++++++----------- 3 files changed, 15 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0721a04c..0bf234f9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -238,7 +238,7 @@ public static class AprilTagConstants { public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; public static final AprilTagFieldLayout aprilTagFieldLayout = - AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); @Getter public enum AprilTagLayoutType { diff --git a/src/main/java/frc/robot/commands/ChoreoAutoController.java b/src/main/java/frc/robot/commands/ChoreoAutoController.java index 20f7900b..ec3d784f 100644 --- a/src/main/java/frc/robot/commands/ChoreoAutoController.java +++ b/src/main/java/frc/robot/commands/ChoreoAutoController.java @@ -50,9 +50,11 @@ public void accept(SwerveSample referenceState) { double rotationFeedback = headingController.calculate(pose.getRotation().getRadians(), referenceState.heading); + // Convert to field relative speeds & send command ChassisSpeeds out = - ChassisSpeeds.fromFieldRelativeSpeeds( - xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, pose.getRotation()); + new ChassisSpeeds(xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback); + // Convert from field-relative to robot-relative speeds + out.toRobotRelativeSpeeds(pose.getRotation()); drive.runVelocity(out); } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 9521ac09..bfb7a622 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -70,14 +70,15 @@ public static Command fieldRelativeDrive( boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; - drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( + ChassisSpeeds chassisSpeeds = + new ChassisSpeeds( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec(), - isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation())); + omega * drive.getMaxAngularSpeedRadPerSec()); + // Convert from field-relative to robot-relative speeds + chassisSpeeds.toRobotRelativeSpeeds( + isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation()); + drive.runVelocity(chassisSpeeds); }, drive); } @@ -97,18 +98,12 @@ public static Command robotRelativeDrive( Translation2d linearVelocity = getLinearVelocity(xSupplier, ySupplier); double omega = getOmega(omegaSupplier); - // Convert to robot relative speeds & send command - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; + // Run with straight-up velocities w.r.t. the robot! drive.runVelocity( - ChassisSpeeds.fromRobotRelativeSpeeds( + new ChassisSpeeds( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec(), - isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation())); + omega * drive.getMaxAngularSpeedRadPerSec())); }, drive); } From f9a95f3acc32872172f5217d9e5cc6d2f6eed2ff Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 20 Dec 2024 13:33:41 -0700 Subject: [PATCH 05/30] Update to WPILib 2025.1.1-beta3; match AK updates In addition to updating to the latest WPILib beta, also match other AdvantageKit commits as they test their beta. modified: .vscode/settings.json modified: build.gradle 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/vision/VisionIOLimelight.java --- .vscode/settings.json | 30 +++++++++++++++++ build.gradle | 2 +- .../robot/commands/ChoreoAutoController.java | 6 ++-- .../frc/robot/commands/DriveCommands.java | 33 ++++++++++--------- .../frc/robot/subsystems/drive/Drive.java | 6 ++-- .../subsystems/vision/VisionIOLimelight.java | 18 +++++----- 6 files changed, 63 insertions(+), 32 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index ed950a7f..39a65d96 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -36,6 +36,36 @@ "spotlessGradle.format.enable": true, "spotlessGradle.diagnostics.enable": false, "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*" + ], "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle", "[json]": { "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" diff --git a/build.gradle b/build.gradle index 8a3142e4..9c88d910 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2" + id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3" id "com.peterabeles.gversion" version "1.10.3" id "com.diffplug.spotless" version "6.25.0" id "io.freefair.lombok" version "8.11" diff --git a/src/main/java/frc/robot/commands/ChoreoAutoController.java b/src/main/java/frc/robot/commands/ChoreoAutoController.java index ec3d784f..241e8f0c 100644 --- a/src/main/java/frc/robot/commands/ChoreoAutoController.java +++ b/src/main/java/frc/robot/commands/ChoreoAutoController.java @@ -52,10 +52,8 @@ public void accept(SwerveSample referenceState) { // Convert to field relative speeds & send command ChassisSpeeds out = - new ChassisSpeeds(xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback); - // Convert from field-relative to robot-relative speeds - out.toRobotRelativeSpeeds(pose.getRotation()); - + ChassisSpeeds.fromFieldRelativeSpeeds( + xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, pose.getRotation()); drive.runVelocity(out); } } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index bfb7a622..c3b59a98 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -70,15 +70,14 @@ public static Command fieldRelativeDrive( boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; - ChassisSpeeds chassisSpeeds = - new ChassisSpeeds( + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec()); - // Convert from field-relative to robot-relative speeds - chassisSpeeds.toRobotRelativeSpeeds( - isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation()); - drive.runVelocity(chassisSpeeds); + omega * drive.getMaxAngularSpeedRadPerSec(), + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); }, drive); } @@ -183,9 +182,12 @@ public static Command joystickDrive( boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; - speeds.toRobotRelativeSpeeds( - isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation()); - drive.runVelocity(speeds); + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); }, drive); } @@ -231,11 +233,12 @@ public static Command joystickDriveAtAngle( boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; - speeds.toRobotRelativeSpeeds( - isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation()); - drive.runVelocity(speeds); + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); }, drive) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index bb004e7c..71396e38 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -285,13 +285,13 @@ public void setMotorBrake(boolean brake) { */ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints - speeds.discretize(Constants.loopPeriodSecs); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds); + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DrivebaseConstants.kMaxLinearSpeed); // Log unoptimized setpoints and setpoint speeds Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveChassisSpeeds/Setpoints", speeds); + Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds); // Send setpoints to modules for (int i = 0; i < 4; i++) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index ffa3a86a..fdf07b79 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -80,38 +80,38 @@ public void updateInputs(VisionIOInputs inputs) { List poseObservations = new LinkedList<>(); for (var rawSample : megatag1Subscriber.readQueue()) { if (rawSample.value.length == 0) continue; - for (int i = 10; i < rawSample.value.length; i += 7) { + for (int i = 11; i < rawSample.value.length; i += 7) { tagIds.add((int) rawSample.value[i]); } poseObservations.add( new PoseObservation( // Timestamp, based on server timestamp of publish and latency - rawSample.timestamp * 1.0e-9 - rawSample.value[7] * 1.0e-3, + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, // 3D pose estimate parsePose(rawSample.value), // Ambiguity, using only the first tag because ambiguity isn't applicable for multitag - rawSample.value.length >= 17 ? rawSample.value[16] : 0.0, + rawSample.value.length >= 18 ? rawSample.value[17] : 0.0, // Tag count - (int) rawSample.value[8], + (int) rawSample.value[7], // Average tag distance - rawSample.value[10], + rawSample.value[9], // Observation type PoseObservationType.MEGATAG_1)); } for (var rawSample : megatag2Subscriber.readQueue()) { if (rawSample.value.length == 0) continue; - for (int i = 10; i < rawSample.value.length; i += 7) { + for (int i = 11; i < rawSample.value.length; i += 7) { tagIds.add((int) rawSample.value[i]); } poseObservations.add( new PoseObservation( // Timestamp, based on server timestamp of publish and latency - rawSample.timestamp * 1.0e-9 - rawSample.value[7] * 1.0e-3, + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, // 3D pose estimate parsePose(rawSample.value), @@ -120,10 +120,10 @@ public void updateInputs(VisionIOInputs inputs) { 0.0, // Tag count - (int) rawSample.value[8], + (int) rawSample.value[7], // Average tag distance - rawSample.value[10], + rawSample.value[9], // Observation type PoseObservationType.MEGATAG_2)); From 76cfc436f96f4c6f5dad5e63476d35815162a1bd Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 20 Dec 2024 13:40:35 -0700 Subject: [PATCH 06/30] Cleanup from vendorlib bump modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java --- src/main/java/frc/robot/Robot.java | 2 -- src/main/java/frc/robot/RobotContainer.java | 6 ------ 2 files changed, 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b5799700..d5ba409e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,7 +13,6 @@ package frc.robot; -import choreo.auto.AutoRoutine; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -34,7 +33,6 @@ */ public class Robot extends LoggedRobot { private Command m_autoCommandPathPlanner; - private AutoRoutine m_autoCommandChoreo; private RobotContainer m_robotContainer; private Timer m_disabledTimer; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ff9b5fab..774c0a7f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,8 +31,6 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -460,8 +458,4 @@ private AutoRoutine twoPieceAuto() { return routine; } - - private boolean isRedAlliance() { - return DriverStation.getAlliance().orElseGet(() -> Alliance.Blue).equals(Alliance.Red); - } } From 040b5724d28a9364c4cf984f409b7580b2bee40b Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 20 Dec 2024 17:10:02 -0700 Subject: [PATCH 07/30] Latest photonlib modified: vendordeps/photonlib.json --- vendordeps/photonlib.json | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 6a23ba8f..1163429e 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.0.0-beta-6", + "version": "v2025.0.0-beta-8", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-6", + "version": "v2025.0.0-beta-8", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.0.0-beta-6", + "version": "v2025.0.0-beta-8", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-6", + "version": "v2025.0.0-beta-8", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.0.0-beta-6" + "version": "v2025.0.0-beta-8" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.0.0-beta-6" + "version": "v2025.0.0-beta-8" } ] } From 3a53c03fce9e30854eb8fbfed1bdf32a9c008608 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 20 Dec 2024 15:45:20 -0700 Subject: [PATCH 08/30] Clean up where constants are defined Trying to keep all constant definitions in easy-to-find locations. modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.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/DriveConstants.java modified: src/main/java/frc/robot/subsystems/drive/GyroIONavX.java modified: src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.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/ModuleIOTalonFX.java modified: src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java modified: src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java modified: src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java --- src/main/java/frc/robot/Constants.java | 36 +- src/main/java/frc/robot/RobotContainer.java | 57 +- .../robot/commands/ChoreoAutoController.java | 2 +- .../frc/robot/commands/DriveCommands.java | 28 +- .../frc/robot/subsystems/drive/Drive.java | 32 +- .../subsystems/drive/DriveConstants.java | 485 +++++++----------- .../robot/subsystems/drive/GyroIONavX.java | 2 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 2 +- .../subsystems/drive/ModuleIOBlended.java | 89 ++-- .../robot/subsystems/drive/ModuleIOSpark.java | 67 ++- .../drive/ModuleIOSparkCANcoder.java | 75 ++- .../subsystems/drive/ModuleIOTalonFX.java | 2 +- .../drive/PhoenixOdometryThread.java | 4 +- .../subsystems/drive/SparkOdometryThread.java | 2 +- .../subsystems/flywheel_example/Flywheel.java | 4 +- .../flywheel_example/FlywheelIOSpark.java | 4 +- 16 files changed, 440 insertions(+), 451 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0bf234f9..312bc1f3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,8 +17,6 @@ package frc.robot; -import static frc.robot.util.RBSIEnum.*; - import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.ObjectMapper; import com.pathplanner.lib.config.PIDConstants; @@ -34,6 +32,11 @@ import frc.robot.generated.TunerConstants; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; +import frc.robot.util.RBSIEnum.AutoType; +import frc.robot.util.RBSIEnum.Mode; +import frc.robot.util.RBSIEnum.RobotType; +import frc.robot.util.RBSIEnum.SwerveType; +import frc.robot.util.RBSIEnum.VisionType; import java.io.IOException; import java.nio.file.Path; import lombok.Getter; @@ -96,6 +99,10 @@ public static final class PhysicalConstants { public static final Matter kChassis = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), kRobotMass); public static final double kLoopTime = 0.13; // s, 20ms + 110ms sprk max velocity lag + + public static final double robotMassKg = 74.088; + public static final double robotMOI = 6.883; + public static final double wheelCOF = 1.2; } /** Power Distribution Constants ********************************** */ @@ -154,6 +161,15 @@ public static final class DrivebaseConstants { public static final double kDelay = 3.0; // seconds public static final double kQuasiTimeout = 5.0; // seconds public static final double kDynamicTimeout = 3.0; // seconds + + public static final PIDConstants drivePID = new PIDConstants(0.05, 0.0, 0.0); + public static final PIDConstants steerPID = new PIDConstants(5.0, 0.0, 0.4); + + // kDriveF = 0.13; + // kDriveIZ = 0.0; + + // kSteerF = 0.0; + // kSteerIZ = 0.0; } /** Example Flywheel Mechanism Constants ********************************* */ @@ -167,18 +183,14 @@ public static final class FlywheelConstants { public static final double kStaticGainReal = 0.1; public static final double kVelocityGainReal = 0.05; // Feedback (PID) constants - public static final double kPReal = 1.0; - public static final double kIReal = 0.0; - public static final double kDReal = 0.0; + public static final PIDConstants pidReal = new PIDConstants(1.0, 0.0, 0.0); // MODE == SIM // Feedforward constants public static final double kStaticGainSim = 0.0; public static final double kVelocityGainSim = 0.03; // Feedback (PID) constants - public static final double kPSim = 1.0; - public static final double kISim = 0.0; - public static final double kDSim = 0.0; + public static final PIDConstants pidSim = new PIDConstants(1.0, 0.0, 0.0); } /** Operator Constants *************************************************** */ @@ -191,16 +203,20 @@ public static class OperatorConstants { } /** Autonomous Action Constants ****************************************** */ - public static final class AutonConstants { + public static final class AutoConstantsPathPlanner { // Translation PID constants public static final PIDConstants kAutoTranslatePID = new PIDConstants(0.7, 0, 0); // Rotation PID constants public static final PIDConstants kAutoAnglePID = new PIDConstants(0.4, 0, 0.01); + + public static final double ROBOT_MASS_KG = 74.088; + public static final double ROBOT_MOI = 6.883; + public static final double WHEEL_COF = 1.2; } /** Choreo Autonomous Action Constants *********************************** */ - public static final class AutoConstants { + public static final class AutoConstantsChoreo { public static final double kMaxSpeedMetersPerSecond = 3; public static final double kMaxAccelerationMetersPerSecondSquared = 3; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 774c0a7f..06130c5a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -45,6 +45,7 @@ import frc.robot.commands.DriveCommands; import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; @@ -349,29 +350,39 @@ public void updateAlerts() { public static class Ports { /* DRIVETRAIN CAN DEVICE IDS */ - // This is the default setup for the Az-RBSI swerve base - // Swerve Modules go: - // FL,FR,BL,BR - // - // 0 1 - // 2 3 - public static final RobotDeviceId FL_DRIVE = new RobotDeviceId(1, "DriveTrain", 18); - public static final RobotDeviceId FL_ROTATION = new RobotDeviceId(2, "DriveTrain", 19); - public static final RobotDeviceId FL_CANCODER = new RobotDeviceId(3, "DriveTrain", null); - - public static final RobotDeviceId FR_DRIVE = new RobotDeviceId(4, "DriveTrain", 17); - public static final RobotDeviceId FR_ROTATION = new RobotDeviceId(5, "DriveTrain", 16); - public static final RobotDeviceId FR_CANCODER = new RobotDeviceId(6, "DriveTrain", null); - - public static final RobotDeviceId BL_DRIVE = new RobotDeviceId(7, "DriveTrain", 1); - public static final RobotDeviceId BL_ROTATION = new RobotDeviceId(8, "DriveTrain", 0); - public static final RobotDeviceId BL_CANCODER = new RobotDeviceId(9, "DriveTrain", null); - - public static final RobotDeviceId BR_DRIVE = new RobotDeviceId(10, "DriveTrain", 2); - public static final RobotDeviceId BR_ROTATION = new RobotDeviceId(11, "DriveTrain", 3); - public static final RobotDeviceId BR_CANCODER = new RobotDeviceId(12, "DriveTrain", null); - - public static final RobotDeviceId PIGEON = new RobotDeviceId(13, "DriveTrain", null); + // Input the correct Power Distribution Module port for each motor!!!! + + // Front Left + public static final RobotDeviceId FL_DRIVE = + new RobotDeviceId(DriveConstants.kFLDriveMotorId, DriveConstants.kFLDriveCanbus, 18); + public static final RobotDeviceId FL_ROTATION = + new RobotDeviceId(DriveConstants.kFLSteerMotorId, DriveConstants.kFLSteerCanbus, 19); + public static final RobotDeviceId FL_CANCODER = + new RobotDeviceId(DriveConstants.kFLEncoderId, DriveConstants.kFLEncoderCanbus, null); + // Front Right + public static final RobotDeviceId FR_DRIVE = + new RobotDeviceId(DriveConstants.kFRDriveMotorId, DriveConstants.kFRDriveCanbus, 17); + public static final RobotDeviceId FR_ROTATION = + new RobotDeviceId(DriveConstants.kFRSteerMotorId, DriveConstants.kFRSteerCanbus, 16); + public static final RobotDeviceId FR_CANCODER = + new RobotDeviceId(DriveConstants.kFREncoderId, DriveConstants.kFREncoderCanbus, null); + // Back Left + public static final RobotDeviceId BL_DRIVE = + new RobotDeviceId(DriveConstants.kBLDriveMotorId, DriveConstants.kBLDriveCanbus, 1); + public static final RobotDeviceId BL_ROTATION = + new RobotDeviceId(DriveConstants.kBLSteerMotorId, DriveConstants.kBLSteerCanbus, 0); + public static final RobotDeviceId BL_CANCODER = + new RobotDeviceId(DriveConstants.kBLEncoderId, DriveConstants.kBLEncoderCanbus, null); + // Back Right + public static final RobotDeviceId BR_DRIVE = + new RobotDeviceId(DriveConstants.kBRDriveMotorId, DriveConstants.kBRSteerCanbus, 2); + public static final RobotDeviceId BR_ROTATION = + new RobotDeviceId(DriveConstants.kBRSteerMotorId, DriveConstants.kBRSteerCanbus, 3); + public static final RobotDeviceId BR_CANCODER = + new RobotDeviceId(DriveConstants.kBREncoderId, DriveConstants.kBREncoderCanbus, null); + // Pigeon + public static final RobotDeviceId PIGEON = + new RobotDeviceId(DriveConstants.kPigeonId, DriveConstants.kCANbusName, null); /* SUBSYSTEM CAN DEVICE IDS */ // This is where mechanism subsystem devices are defined diff --git a/src/main/java/frc/robot/commands/ChoreoAutoController.java b/src/main/java/frc/robot/commands/ChoreoAutoController.java index 241e8f0c..44810ba8 100644 --- a/src/main/java/frc/robot/commands/ChoreoAutoController.java +++ b/src/main/java/frc/robot/commands/ChoreoAutoController.java @@ -15,7 +15,7 @@ package frc.robot.commands; -import static frc.robot.Constants.AutonConstants.*; +import static frc.robot.Constants.AutoConstantsPathPlanner.*; import choreo.trajectory.SwerveSample; import edu.wpi.first.math.controller.PIDController; diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index c3b59a98..ba208961 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -30,8 +30,10 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.DrivebaseConstants; import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants; import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; @@ -40,11 +42,9 @@ import java.util.function.Supplier; public class DriveCommands { - private static final double DEADBAND = 0.1; - private static final double ANGLE_KP = 5.0; - private static final double ANGLE_KD = 0.4; - private static final double ANGLE_MAX_VELOCITY = 8.0; - private static final double ANGLE_MAX_ACCELERATION = 20.0; + + // Needed for Characterization routines, not normal robot operations + // DO NOT ADJUST private static final double FF_START_DELAY = 2.0; // Secs private static final double FF_RAMP_RATE = 0.1; // Volts/Sec private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec @@ -141,7 +141,8 @@ private static double getOmega(DoubleSupplier omegaSupplier) { private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { // Apply deadband - double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), DEADBAND); + double linearMagnitude = + MathUtil.applyDeadband(Math.hypot(x, y), OperatorConstants.kLeftDeadband); Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); // Square magnitude for more precise control @@ -168,7 +169,8 @@ public static Command joystickDrive( getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); // Apply rotation deadband - double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + double omega = + MathUtil.applyDeadband(omegaSupplier.getAsDouble(), OperatorConstants.kRightDeadband); // Square rotation value for more precise control omega = Math.copySign(omega * omega, omega); @@ -206,10 +208,11 @@ public static Command joystickDriveAtAngle( // Create PID controller ProfiledPIDController angleController = new ProfiledPIDController( - ANGLE_KP, - 0.0, - ANGLE_KD, - new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); + DrivebaseConstants.steerPID.kP, + DrivebaseConstants.steerPID.kI, + DrivebaseConstants.steerPID.kD, + new TrapezoidProfile.Constraints( + DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); angleController.enableContinuousInput(-Math.PI, Math.PI); // Construct command @@ -360,7 +363,8 @@ public static Command wheelRadiusCharacterization(Drive drive) { for (int i = 0; i < 4; i++) { wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; } - double wheelRadius = (state.gyroDelta * Drive.DRIVE_BASE_RADIUS) / wheelDelta; + double wheelRadius = + (state.gyroDelta * DriveConstants.kDriveBaseRadiusMeters) / wheelDelta; NumberFormat formatter = new DecimalFormat("#0.000"); System.out.println( diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 71396e38..9631dd16 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -19,7 +19,6 @@ import static frc.robot.subsystems.drive.DriveConstants.*; import choreo.trajectory.SwerveSample; -import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.ModuleConfig; @@ -53,6 +52,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; +import frc.robot.Constants.AutoConstantsPathPlanner; import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; @@ -63,23 +63,15 @@ public class Drive extends SubsystemBase { - // TunerConstants doesn't include these constants, so they are declared locally - public static final double DRIVE_BASE_RADIUS = kDriveBaseRadiusMeters; - // Are we on the CANivore or not? - static final double ODOMETRY_FREQUENCY = new CANBus(kCANbusName).isNetworkFD() ? 250.0 : 100.0; - // PathPlanner config constants - private static final double ROBOT_MASS_KG = 74.088; - private static final double ROBOT_MOI = 6.883; - private static final double WHEEL_COF = 1.2; private static final RobotConfig PP_CONFIG = new RobotConfig( - ROBOT_MASS_KG, - ROBOT_MOI, + AutoConstantsPathPlanner.ROBOT_MASS_KG, + AutoConstantsPathPlanner.ROBOT_MOI, new ModuleConfig( kWheelRadiusMeters, DrivebaseConstants.kMaxLinearSpeed.magnitude(), - WHEEL_COF, + AutoConstantsPathPlanner.WHEEL_COF, DCMotor.getKrakenX60Foc(1).withReduction(kDriveGearRatio), kDriveSlipCurrent, 1), @@ -419,16 +411,16 @@ public double getMaxLinearSpeedMetersPerSec() { /** Returns the maximum angular speed in radians per sec. */ public double getMaxAngularSpeedRadPerSec() { - return getMaxLinearSpeedMetersPerSec() / DRIVE_BASE_RADIUS; + return getMaxLinearSpeedMetersPerSec() / kDriveBaseRadiusMeters; } /** Returns an array of module translations. */ public static Translation2d[] getModuleTranslations() { return new Translation2d[] { - new Translation2d(kFrontLeftXPosMeters, kFrontLeftYPosMeters), - new Translation2d(kFrontRightXPosMeters, kFrontRightYPosMeters), - new Translation2d(kBackLeftXPosMeters, kBackLeftYPosMeters), - new Translation2d(kBackRightXPosMeters, kBackRightYPosMeters) + new Translation2d(kFLXPosMeters, kFLYPosMeters), + new Translation2d(kFRXPosMeters, kFRYPosMeters), + new Translation2d(kBLXPosMeters, kBLYPosMeters), + new Translation2d(kBRXPosMeters, kBRYPosMeters) }; } @@ -496,7 +488,7 @@ private Byte parseModuleType() { Byte b_drive; // [x,x,-,-,-,-,-,-] Byte b_steer; // [-,-,x,x,-,-,-,-] Byte b_encoder; // [-,-,-,-,x,x,-,-] - switch (kFrontLeftDriveType) { + switch (kFLDriveType) { case "falcon": case "kraken": case "talonfx": @@ -511,7 +503,7 @@ private Byte parseModuleType() { default: throw new RuntimeException("Invalid drive motor type"); } - switch (kFrontLeftSteerType) { + switch (kFLSteerType) { case "falcon": case "kraken": case "talonfx": @@ -526,7 +518,7 @@ private Byte parseModuleType() { default: throw new RuntimeException("Invalid steer motor type"); } - switch (kFrontLeftEncoderType) { + switch (kFLEncoderType) { case "cancoder": // CTRE CANcoder b_encoder = 0b00; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index c7aa0929..07e078e4 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -15,10 +15,7 @@ package frc.robot.subsystems.drive; -import com.pathplanner.lib.config.ModuleConfig; -import com.pathplanner.lib.config.RobotConfig; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.system.plant.DCMotor; +import com.ctre.phoenix6.CANBus; import edu.wpi.first.math.util.Units; import frc.robot.Constants; import frc.robot.generated.TunerConstants; @@ -48,76 +45,66 @@ public class DriveConstants { public static final double kDriveCurrentLimit; public static final double kDriveSlipCurrent; public static final double kOptimalVoltage; - public static final int kFrontLeftDriveMotorId; - public static final int kFrontLeftSteerMotorId; - public static final int kFrontLeftEncoderId; - public static final String kFrontLeftDriveCanbus; - public static final String kFrontLeftSteerCanbus; - public static final String kFrontLeftEncoderCanbus; - public static final String kFrontLeftDriveType; - public static final String kFrontLeftSteerType; - public static final String kFrontLeftEncoderType; - public static final double kFrontLeftEncoderOffset; // In Radians - public static final boolean kFrontLeftDriveInvert; - public static final boolean kFrontLeftSteerInvert; - public static final boolean kFrontLeftEncoderInvert; - public static final double kFrontLeftXPosMeters; - public static final double kFrontLeftYPosMeters; - public static final int kFrontRightDriveMotorId; - public static final int kFrontRightSteerMotorId; - public static final int kFrontRightEncoderId; - public static final String kFrontRightDriveCanbus; - public static final String kFrontRightSteerCanbus; - public static final String kFrontRightEncoderCanbus; - public static final String kFrontRightDriveType; - public static final String kFrontRightSteerType; - public static final String kFrontRightEncoderType; - public static final double kFrontRightEncoderOffset; // In Radians - public static final boolean kFrontRightDriveInvert; - public static final boolean kFrontRightSteerInvert; - public static final boolean kFrontRightEncoderInvert; - public static final double kFrontRightXPosMeters; - public static final double kFrontRightYPosMeters; - public static final int kBackLeftDriveMotorId; - public static final int kBackLeftSteerMotorId; - public static final int kBackLeftEncoderId; - public static final String kBackLeftDriveCanbus; - public static final String kBackLeftSteerCanbus; - public static final String kBackLeftEncoderCanbus; - public static final String kBackLeftDriveType; - public static final String kBackLeftSteerType; - public static final String kBackLeftEncoderType; - public static final double kBackLeftEncoderOffset; // In Radians - public static final boolean kBackLeftDriveInvert; - public static final boolean kBackLeftSteerInvert; - public static final boolean kBackLeftEncoderInvert; - public static final double kBackLeftXPosMeters; - public static final double kBackLeftYPosMeters; - public static final int kBackRightDriveMotorId; - public static final int kBackRightSteerMotorId; - public static final int kBackRightEncoderId; - public static final String kBackRightDriveCanbus; - public static final String kBackRightSteerCanbus; - public static final String kBackRightEncoderCanbus; - public static final String kBackRightDriveType; - public static final String kBackRightSteerType; - public static final String kBackRightEncoderType; - public static final double kBackRightEncoderOffset; // In Radians - public static final boolean kBackRightDriveInvert; - public static final boolean kBackRightSteerInvert; - public static final boolean kBackRightEncoderInvert; - public static final double kBackRightXPosMeters; - public static final double kBackRightYPosMeters; - public static final double kDriveP; - public static final double kDriveI; - public static final double kDriveD; - public static final double kDriveF; - public static final double kDriveIZ; - public static final double kSteerP; - public static final double kSteerI; - public static final double kSteerD; - public static final double kSteerF; - public static final double kSteerIZ; + public static final int kFLDriveMotorId; + public static final int kFLSteerMotorId; + public static final int kFLEncoderId; + public static final String kFLDriveCanbus; + public static final String kFLSteerCanbus; + public static final String kFLEncoderCanbus; + public static final String kFLDriveType; + public static final String kFLSteerType; + public static final String kFLEncoderType; + public static final double kFLEncoderOffset; // In Radians + public static final boolean kFLDriveInvert; + public static final boolean kFLSteerInvert; + public static final boolean kFLEncoderInvert; + public static final double kFLXPosMeters; + public static final double kFLYPosMeters; + public static final int kFRDriveMotorId; + public static final int kFRSteerMotorId; + public static final int kFREncoderId; + public static final String kFRDriveCanbus; + public static final String kFRSteerCanbus; + public static final String kFREncoderCanbus; + public static final String kFRDriveType; + public static final String kFRSteerType; + public static final String kFREncoderType; + public static final double kFREncoderOffset; // In Radians + public static final boolean kFRDriveInvert; + public static final boolean kFRSteerInvert; + public static final boolean kFREncoderInvert; + public static final double kFRXPosMeters; + public static final double kFRYPosMeters; + public static final int kBLDriveMotorId; + public static final int kBLSteerMotorId; + public static final int kBLEncoderId; + public static final String kBLDriveCanbus; + public static final String kBLSteerCanbus; + public static final String kBLEncoderCanbus; + public static final String kBLDriveType; + public static final String kBLSteerType; + public static final String kBLEncoderType; + public static final double kBLEncoderOffset; // In Radians + public static final boolean kBLDriveInvert; + public static final boolean kBLSteerInvert; + public static final boolean kBLEncoderInvert; + public static final double kBLXPosMeters; + public static final double kBLYPosMeters; + public static final int kBRDriveMotorId; + public static final int kBRSteerMotorId; + public static final int kBREncoderId; + public static final String kBRDriveCanbus; + public static final String kBRSteerCanbus; + public static final String kBREncoderCanbus; + public static final String kBRDriveType; + public static final String kBRSteerType; + public static final String kBREncoderType; + public static final double kBREncoderOffset; // In Radians + public static final boolean kBRDriveInvert; + public static final boolean kBRSteerInvert; + public static final boolean kBREncoderInvert; + public static final double kBRXPosMeters; + public static final double kBRYPosMeters; // Fill in the values from the proper source static { @@ -138,82 +125,68 @@ public class DriveConstants { kDriveCurrentLimit = 120.0; // Example from CTRE documentation kDriveSlipCurrent = TunerConstants.FrontLeft.SlipCurrent; kOptimalVoltage = 12.0; // Assumed Ideal - kFrontLeftDriveMotorId = TunerConstants.FrontLeft.DriveMotorId; - kFrontLeftSteerMotorId = TunerConstants.FrontLeft.SteerMotorId; - kFrontLeftEncoderId = TunerConstants.FrontLeft.CANcoderId; - kFrontLeftDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFrontLeftSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFrontLeftEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFrontLeftDriveType = "kraken"; - kFrontLeftSteerType = "kraken"; - kFrontLeftEncoderType = "cancoder"; - kFrontLeftEncoderOffset = + kFLDriveMotorId = TunerConstants.FrontLeft.DriveMotorId; + kFLSteerMotorId = TunerConstants.FrontLeft.SteerMotorId; + kFLEncoderId = TunerConstants.FrontLeft.CANcoderId; + kFLDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFLSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFLEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFLDriveType = "kraken"; + kFLSteerType = "kraken"; + kFLEncoderType = "cancoder"; + kFLEncoderOffset = -Units.rotationsToRadians(TunerConstants.FrontLeft.CANcoderOffset) + Math.PI; - kFrontLeftDriveInvert = TunerConstants.FrontLeft.DriveMotorInverted; - kFrontLeftSteerInvert = TunerConstants.FrontLeft.SteerMotorInverted; - kFrontLeftEncoderInvert = false; - kFrontLeftXPosMeters = TunerConstants.FrontLeft.LocationX; - kFrontLeftYPosMeters = TunerConstants.FrontLeft.LocationY; - kFrontRightDriveMotorId = TunerConstants.FrontRight.DriveMotorId; - kFrontRightSteerMotorId = TunerConstants.FrontRight.SteerMotorId; - kFrontRightEncoderId = TunerConstants.FrontRight.CANcoderId; - kFrontRightDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFrontRightSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFrontRightEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFrontRightDriveType = "kraken"; - kFrontRightSteerType = "kraken"; - kFrontRightEncoderType = "cancoder"; - kFrontRightEncoderOffset = - -Units.rotationsToRadians(TunerConstants.FrontRight.CANcoderOffset); - kFrontRightDriveInvert = TunerConstants.FrontRight.DriveMotorInverted; - kFrontRightSteerInvert = TunerConstants.FrontRight.SteerMotorInverted; - kFrontRightEncoderInvert = false; - kFrontRightXPosMeters = TunerConstants.FrontRight.LocationX; - kFrontRightYPosMeters = TunerConstants.FrontRight.LocationY; - kBackLeftDriveMotorId = TunerConstants.BackLeft.DriveMotorId; - kBackLeftSteerMotorId = TunerConstants.BackLeft.SteerMotorId; - kBackLeftEncoderId = TunerConstants.BackLeft.CANcoderId; - kBackLeftDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBackLeftSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBackLeftEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBackLeftDriveType = "kraken"; - kBackLeftSteerType = "kraken"; - kBackLeftEncoderType = "cancoder"; - kBackLeftEncoderOffset = + kFLDriveInvert = TunerConstants.FrontLeft.DriveMotorInverted; + kFLSteerInvert = TunerConstants.FrontLeft.SteerMotorInverted; + kFLEncoderInvert = false; + kFLXPosMeters = TunerConstants.FrontLeft.LocationX; + kFLYPosMeters = TunerConstants.FrontLeft.LocationY; + kFRDriveMotorId = TunerConstants.FrontRight.DriveMotorId; + kFRSteerMotorId = TunerConstants.FrontRight.SteerMotorId; + kFREncoderId = TunerConstants.FrontRight.CANcoderId; + kFRDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFRSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFREncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFRDriveType = "kraken"; + kFRSteerType = "kraken"; + kFREncoderType = "cancoder"; + kFREncoderOffset = -Units.rotationsToRadians(TunerConstants.FrontRight.CANcoderOffset); + kFRDriveInvert = TunerConstants.FrontRight.DriveMotorInverted; + kFRSteerInvert = TunerConstants.FrontRight.SteerMotorInverted; + kFREncoderInvert = false; + kFRXPosMeters = TunerConstants.FrontRight.LocationX; + kFRYPosMeters = TunerConstants.FrontRight.LocationY; + kBLDriveMotorId = TunerConstants.BackLeft.DriveMotorId; + kBLSteerMotorId = TunerConstants.BackLeft.SteerMotorId; + kBLEncoderId = TunerConstants.BackLeft.CANcoderId; + kBLDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBLSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBLEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBLDriveType = "kraken"; + kBLSteerType = "kraken"; + kBLEncoderType = "cancoder"; + kBLEncoderOffset = -Units.rotationsToRadians(TunerConstants.BackLeft.CANcoderOffset) + Math.PI; - kBackLeftDriveInvert = TunerConstants.BackLeft.DriveMotorInverted; - kBackLeftSteerInvert = TunerConstants.BackLeft.SteerMotorInverted; - kBackLeftEncoderInvert = false; - kBackLeftXPosMeters = TunerConstants.BackLeft.LocationX; - kBackLeftYPosMeters = TunerConstants.BackLeft.LocationY; - kBackRightDriveMotorId = TunerConstants.BackRight.DriveMotorId; - kBackRightSteerMotorId = TunerConstants.BackRight.SteerMotorId; - kBackRightEncoderId = TunerConstants.BackRight.CANcoderId; - kBackRightDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBackRightSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBackRightEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBackRightDriveType = "kraken"; - kBackRightSteerType = "kraken"; - kBackRightEncoderType = "cancoder"; - kBackRightEncoderOffset = - -Units.rotationsToRadians(TunerConstants.BackRight.CANcoderOffset); - kBackRightDriveInvert = TunerConstants.BackRight.DriveMotorInverted; - kBackRightSteerInvert = TunerConstants.BackRight.SteerMotorInverted; - kBackRightEncoderInvert = false; - kBackRightXPosMeters = TunerConstants.BackRight.LocationX; - kBackRightYPosMeters = TunerConstants.BackRight.LocationY; - // NOTE: The PIDF values from TunerConstants.java make WPILib/AK implemention go crazy - // These values are from the AK example sketches - kDriveP = 0.05; - kDriveI = 0.0; - kDriveD = 0.0; - kDriveF = 0.13; - kDriveIZ = 0.0; - kSteerP = 7.0; - kSteerI = 0.0; - kSteerD = 0.0; - kSteerF = 0.0; - kSteerIZ = 0.0; + kBLDriveInvert = TunerConstants.BackLeft.DriveMotorInverted; + kBLSteerInvert = TunerConstants.BackLeft.SteerMotorInverted; + kBLEncoderInvert = false; + kBLXPosMeters = TunerConstants.BackLeft.LocationX; + kBLYPosMeters = TunerConstants.BackLeft.LocationY; + kBRDriveMotorId = TunerConstants.BackRight.DriveMotorId; + kBRSteerMotorId = TunerConstants.BackRight.SteerMotorId; + kBREncoderId = TunerConstants.BackRight.CANcoderId; + kBRDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBRSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBREncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBRDriveType = "kraken"; + kBRSteerType = "kraken"; + kBREncoderType = "cancoder"; + kBREncoderOffset = -Units.rotationsToRadians(TunerConstants.BackRight.CANcoderOffset); + kBRDriveInvert = TunerConstants.BackRight.DriveMotorInverted; + kBRSteerInvert = TunerConstants.BackRight.SteerMotorInverted; + kBREncoderInvert = false; + kBRXPosMeters = TunerConstants.BackRight.LocationX; + kBRYPosMeters = TunerConstants.BackRight.LocationY; break; case YAGSL: @@ -232,76 +205,66 @@ public class DriveConstants { kDriveCurrentLimit = YagslConstants.kDriveCurrentLimit; kDriveSlipCurrent = 120.0; kOptimalVoltage = YagslConstants.kOptimalVoltage; - kFrontLeftDriveMotorId = YagslConstants.kFrontLeftDriveMotorId; - kFrontLeftSteerMotorId = YagslConstants.kFrontLeftSteerMotorId; - kFrontLeftEncoderId = YagslConstants.kFrontLeftEncoderId; - kFrontLeftDriveCanbus = YagslConstants.kFrontLeftDriveCanbus; - kFrontLeftSteerCanbus = YagslConstants.kFrontLeftSteerCanbus; - kFrontLeftEncoderCanbus = YagslConstants.kFrontLeftEncoderCanbus; - kFrontLeftDriveType = YagslConstants.kFrontLeftDriveType.toLowerCase(); - kFrontLeftSteerType = YagslConstants.kFrontLeftSteerType.toLowerCase(); - kFrontLeftEncoderType = YagslConstants.kFrontLeftEncoderType.toLowerCase(); - kFrontLeftEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontLeftEncoderOffset); - kFrontLeftDriveInvert = YagslConstants.kFrontLeftDriveInvert; - kFrontLeftSteerInvert = YagslConstants.kFrontLeftSteerInvert; - kFrontLeftEncoderInvert = YagslConstants.kFrontLeftEncoderInvert; - kFrontLeftXPosMeters = Units.inchesToMeters(YagslConstants.kFrontLeftXPosInches); - kFrontLeftYPosMeters = Units.inchesToMeters(YagslConstants.kFrontLeftYPosInches); - kFrontRightDriveMotorId = YagslConstants.kFrontRightDriveMotorId; - kFrontRightSteerMotorId = YagslConstants.kFrontRightSteerMotorId; - kFrontRightEncoderId = YagslConstants.kFrontRightEncoderId; - kFrontRightDriveCanbus = YagslConstants.kFrontRightDriveCanbus; - kFrontRightSteerCanbus = YagslConstants.kFrontRightSteerCanbus; - kFrontRightEncoderCanbus = YagslConstants.kFrontRightEncoderCanbus; - kFrontRightDriveType = YagslConstants.kFrontRightDriveType.toLowerCase(); - kFrontRightSteerType = YagslConstants.kFrontRightSteerType.toLowerCase(); - kFrontRightEncoderType = YagslConstants.kFrontRightEncoderType.toLowerCase(); - kFrontRightEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontRightEncoderOffset); - kFrontRightDriveInvert = YagslConstants.kFrontRightDriveInvert; - kFrontRightSteerInvert = YagslConstants.kFrontRightSteerInvert; - kFrontRightEncoderInvert = YagslConstants.kFrontRightEncoderInvert; - kFrontRightXPosMeters = Units.inchesToMeters(YagslConstants.kFrontRightXPosInches); - kFrontRightYPosMeters = Units.inchesToMeters(YagslConstants.kFrontRightYPosInches); - kBackLeftDriveMotorId = YagslConstants.kBackLeftDriveMotorId; - kBackLeftSteerMotorId = YagslConstants.kBackLeftSteerMotorId; - kBackLeftEncoderId = YagslConstants.kBackLeftEncoderId; - kBackLeftDriveCanbus = YagslConstants.kBackLeftDriveCanbus; - kBackLeftSteerCanbus = YagslConstants.kBackLeftSteerCanbus; - kBackLeftEncoderCanbus = YagslConstants.kBackLeftEncoderCanbus; - kBackLeftDriveType = YagslConstants.kBackLeftDriveType.toLowerCase(); - kBackLeftSteerType = YagslConstants.kBackLeftSteerType.toLowerCase(); - kBackLeftEncoderType = YagslConstants.kBackLeftEncoderType.toLowerCase(); - kBackLeftEncoderOffset = Units.degreesToRadians(YagslConstants.kBackLeftEncoderOffset); - kBackLeftDriveInvert = YagslConstants.kBackLeftDriveInvert; - kBackLeftSteerInvert = YagslConstants.kBackLeftSteerInvert; - kBackLeftEncoderInvert = YagslConstants.kBackLeftEncoderInvert; - kBackLeftXPosMeters = Units.inchesToMeters(YagslConstants.kBackLeftXPosInches); - kBackLeftYPosMeters = Units.inchesToMeters(YagslConstants.kBackLeftYPosInches); - kBackRightDriveMotorId = YagslConstants.kBackRightDriveMotorId; - kBackRightSteerMotorId = YagslConstants.kBackRightSteerMotorId; - kBackRightEncoderId = YagslConstants.kBackRightEncoderId; - kBackRightDriveCanbus = YagslConstants.kBackRightDriveCanbus; - kBackRightSteerCanbus = YagslConstants.kBackRightSteerCanbus; - kBackRightEncoderCanbus = YagslConstants.kBackRightEncoderCanbus; - kBackRightDriveType = YagslConstants.kBackRightDriveType.toLowerCase(); - kBackRightSteerType = YagslConstants.kBackRightSteerType.toLowerCase(); - kBackRightEncoderType = YagslConstants.kBackRightEncoderType.toLowerCase(); - kBackRightEncoderOffset = Units.degreesToRadians(YagslConstants.kBackRightEncoderOffset); - kBackRightDriveInvert = YagslConstants.kBackRightDriveInvert; - kBackRightSteerInvert = YagslConstants.kBackRightSteerInvert; - kBackRightEncoderInvert = YagslConstants.kBackRightEncoderInvert; - kBackRightXPosMeters = Units.inchesToMeters(YagslConstants.kBackRightXPosInches); - kBackRightYPosMeters = Units.inchesToMeters(YagslConstants.kBackRightYPosInches); - kDriveP = YagslConstants.kDriveP; - kDriveI = YagslConstants.kDriveI; - kDriveD = YagslConstants.kDriveD; - kDriveF = YagslConstants.kDriveF; - kDriveIZ = YagslConstants.kDriveIZ; - kSteerP = YagslConstants.kSteerP; - kSteerI = YagslConstants.kSteerI; - kSteerD = YagslConstants.kSteerD; - kSteerF = YagslConstants.kSteerF; - kSteerIZ = YagslConstants.kSteerIZ; + kFLDriveMotorId = YagslConstants.kFrontLeftDriveMotorId; + kFLSteerMotorId = YagslConstants.kFrontLeftSteerMotorId; + kFLEncoderId = YagslConstants.kFrontLeftEncoderId; + kFLDriveCanbus = YagslConstants.kFrontLeftDriveCanbus; + kFLSteerCanbus = YagslConstants.kFrontLeftSteerCanbus; + kFLEncoderCanbus = YagslConstants.kFrontLeftEncoderCanbus; + kFLDriveType = YagslConstants.kFrontLeftDriveType.toLowerCase(); + kFLSteerType = YagslConstants.kFrontLeftSteerType.toLowerCase(); + kFLEncoderType = YagslConstants.kFrontLeftEncoderType.toLowerCase(); + kFLEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontLeftEncoderOffset); + kFLDriveInvert = YagslConstants.kFrontLeftDriveInvert; + kFLSteerInvert = YagslConstants.kFrontLeftSteerInvert; + kFLEncoderInvert = YagslConstants.kFrontLeftEncoderInvert; + kFLXPosMeters = Units.inchesToMeters(YagslConstants.kFrontLeftXPosInches); + kFLYPosMeters = Units.inchesToMeters(YagslConstants.kFrontLeftYPosInches); + kFRDriveMotorId = YagslConstants.kFrontRightDriveMotorId; + kFRSteerMotorId = YagslConstants.kFrontRightSteerMotorId; + kFREncoderId = YagslConstants.kFrontRightEncoderId; + kFRDriveCanbus = YagslConstants.kFrontRightDriveCanbus; + kFRSteerCanbus = YagslConstants.kFrontRightSteerCanbus; + kFREncoderCanbus = YagslConstants.kFrontRightEncoderCanbus; + kFRDriveType = YagslConstants.kFrontRightDriveType.toLowerCase(); + kFRSteerType = YagslConstants.kFrontRightSteerType.toLowerCase(); + kFREncoderType = YagslConstants.kFrontRightEncoderType.toLowerCase(); + kFREncoderOffset = Units.degreesToRadians(YagslConstants.kFrontRightEncoderOffset); + kFRDriveInvert = YagslConstants.kFrontRightDriveInvert; + kFRSteerInvert = YagslConstants.kFrontRightSteerInvert; + kFREncoderInvert = YagslConstants.kFrontRightEncoderInvert; + kFRXPosMeters = Units.inchesToMeters(YagslConstants.kFrontRightXPosInches); + kFRYPosMeters = Units.inchesToMeters(YagslConstants.kFrontRightYPosInches); + kBLDriveMotorId = YagslConstants.kBackLeftDriveMotorId; + kBLSteerMotorId = YagslConstants.kBackLeftSteerMotorId; + kBLEncoderId = YagslConstants.kBackLeftEncoderId; + kBLDriveCanbus = YagslConstants.kBackLeftDriveCanbus; + kBLSteerCanbus = YagslConstants.kBackLeftSteerCanbus; + kBLEncoderCanbus = YagslConstants.kBackLeftEncoderCanbus; + kBLDriveType = YagslConstants.kBackLeftDriveType.toLowerCase(); + kBLSteerType = YagslConstants.kBackLeftSteerType.toLowerCase(); + kBLEncoderType = YagslConstants.kBackLeftEncoderType.toLowerCase(); + kBLEncoderOffset = Units.degreesToRadians(YagslConstants.kBackLeftEncoderOffset); + kBLDriveInvert = YagslConstants.kBackLeftDriveInvert; + kBLSteerInvert = YagslConstants.kBackLeftSteerInvert; + kBLEncoderInvert = YagslConstants.kBackLeftEncoderInvert; + kBLXPosMeters = Units.inchesToMeters(YagslConstants.kBackLeftXPosInches); + kBLYPosMeters = Units.inchesToMeters(YagslConstants.kBackLeftYPosInches); + kBRDriveMotorId = YagslConstants.kBackRightDriveMotorId; + kBRSteerMotorId = YagslConstants.kBackRightSteerMotorId; + kBREncoderId = YagslConstants.kBackRightEncoderId; + kBRDriveCanbus = YagslConstants.kBackRightDriveCanbus; + kBRSteerCanbus = YagslConstants.kBackRightSteerCanbus; + kBREncoderCanbus = YagslConstants.kBackRightEncoderCanbus; + kBRDriveType = YagslConstants.kBackRightDriveType.toLowerCase(); + kBRSteerType = YagslConstants.kBackRightSteerType.toLowerCase(); + kBREncoderType = YagslConstants.kBackRightEncoderType.toLowerCase(); + kBREncoderOffset = Units.degreesToRadians(YagslConstants.kBackRightEncoderOffset); + kBRDriveInvert = YagslConstants.kBackRightDriveInvert; + kBRSteerInvert = YagslConstants.kBackRightSteerInvert; + kBREncoderInvert = YagslConstants.kBackRightEncoderInvert; + kBRXPosMeters = Units.inchesToMeters(YagslConstants.kBackRightXPosInches); + kBRYPosMeters = Units.inchesToMeters(YagslConstants.kBackRightYPosInches); break; default: @@ -313,84 +276,28 @@ public class DriveConstants { public static final double kDriveBaseRadiusMeters = Math.max( Math.max( - Math.hypot(kFrontLeftXPosMeters, kFrontLeftYPosMeters), - Math.hypot(kFrontRightXPosMeters, kFrontRightYPosMeters)), + Math.hypot(kFLXPosMeters, kFLYPosMeters), Math.hypot(kFRXPosMeters, kFRYPosMeters)), Math.max( - Math.hypot(kBackLeftXPosMeters, kBackLeftYPosMeters), - Math.hypot(kBackRightXPosMeters, kBackRightYPosMeters))); + Math.hypot(kBLXPosMeters, kBLYPosMeters), Math.hypot(kBRXPosMeters, kBRYPosMeters))); public static final double kDriveBaseRadiusInches = Units.metersToInches(kDriveBaseRadiusMeters); public static final double kWheelRadiusMeters = Units.inchesToMeters(kWheelRadiusInches); - // Stuff to deal with from AK25's Spark Swerve Template - public static final double maxSpeedMetersPerSec = 4.8; - public static final double odometryFrequency = 100.0; // Hz - public static final double trackWidth = Units.inchesToMeters(26.5); - public static final double wheelBase = Units.inchesToMeters(26.5); - public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0); - public static final Translation2d[] moduleTranslations = - new Translation2d[] { - new Translation2d(trackWidth / 2.0, wheelBase / 2.0), - new Translation2d(trackWidth / 2.0, -wheelBase / 2.0), - new Translation2d(-trackWidth / 2.0, wheelBase / 2.0), - new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0) - }; - - // Drive motor configuration - public static final int driveMotorCurrentLimit = 50; - public static final double wheelRadiusMeters = Units.inchesToMeters(1.5); - public static final double driveMotorReduction = - (45.0 * 22.0) / (14.0 * 15.0); // MAXSwerve with 14 pinion teeth and 22 spur teeth - public static final DCMotor driveGearbox = DCMotor.getNeoVortex(1); - - // Drive encoder configuration - public static final double driveEncoderPositionFactor = - 2 * Math.PI / driveMotorReduction; // Rotor Rotations -> Wheel Radians - public static final double driveEncoderVelocityFactor = - (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec + // Are we on the CANivore or not? + public static final double kOdometryFrequency = + new CANBus(kCANbusName).isNetworkFD() ? 250.0 : 100.0; + // SPARK Drive encoder configuration + // Rotor Rotations -> Wheel Radians + public static final double driveEncoderPositionFactor = 2 * Math.PI / kDriveGearRatio; + // Rotor RPM -> Wheel Rad/Sec + public static final double driveEncoderVelocityFactor = (2 * Math.PI) / 60.0 / kDriveGearRatio; + // Rotations -> Radians + public static final double turnEncoderPositionFactor = 2 * Math.PI; + // RPM -> Rad/Sec + public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // Drive PID configuration - public static final double driveKp = 0.0; - public static final double driveKd = 0.0; - public static final double driveKs = 0.0; public static final double driveKv = 0.1; - public static final double driveSimP = 0.05; - public static final double driveSimD = 0.0; - public static final double driveSimKs = 0.0; - public static final double driveSimKv = 0.0789; - - // Turn motor configuration - public static final boolean turnInverted = false; - public static final int turnMotorCurrentLimit = 20; - public static final double turnMotorReduction = 9424.0 / 203.0; - public static final DCMotor turnGearbox = DCMotor.getNeo550(1); - - // Turn encoder configuration - public static final boolean turnEncoderInverted = true; - public static final double turnEncoderPositionFactor = 2 * Math.PI; // Rotations -> Radians - public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // RPM -> Rad/Sec - - // Turn PID configuration - public static final double turnKp = 2.0; - public static final double turnKd = 0.0; - public static final double turnSimP = 8.0; - public static final double turnSimD = 0.0; + // Turn PID configuration0 public static final double turnPIDMinInput = 0; // Radians public static final double turnPIDMaxInput = 2 * Math.PI; // Radians - - // PathPlanner configuration - public static final double robotMassKg = 74.088; - public static final double robotMOI = 6.883; - public static final double wheelCOF = 1.2; - public static final RobotConfig ppConfig = - new RobotConfig( - robotMassKg, - robotMOI, - new ModuleConfig( - wheelRadiusMeters, - maxSpeedMetersPerSec, - wheelCOF, - driveGearbox.withReduction(driveMotorReduction), - driveMotorCurrentLimit, - 1), - moduleTranslations); } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 7f8dcb76..1f3ea00e 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -25,7 +25,7 @@ /** IO implementation for Pigeon2 */ public class GyroIONavX implements GyroIO { - private final AHRS navx = new AHRS(SPI.Port.kMXP, (byte) Drive.ODOMETRY_FREQUENCY); + private final AHRS navx = new AHRS(SPI.Port.kMXP, (byte) DriveConstants.kOdometryFrequency); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 79a5020d..31d18ad8 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -42,7 +42,7 @@ public class GyroIOPigeon2 implements GyroIO { public GyroIOPigeon2() { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(Drive.ODOMETRY_FREQUENCY); + yaw.setUpdateFrequency(DriveConstants.kOdometryFrequency); yawVelocity.setUpdateFrequency(50.0); pigeon.optimizeBusUtilization(); yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index 87befca0..669abb33 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -50,6 +50,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.PhoenixUtil; import frc.robot.util.SparkUtil; import java.util.Queue; @@ -115,48 +116,48 @@ public ModuleIOBlended(int module) { switch (module) { case 0 -> ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPosMeters, - kFrontLeftYPosMeters, - kFrontLeftDriveInvert, - kFrontLeftSteerInvert, - kFrontLeftEncoderInvert); + kFLSteerMotorId, + kFLDriveMotorId, + kFLEncoderId, + kFLEncoderOffset, + kFLXPosMeters, + kFLYPosMeters, + kFLDriveInvert, + kFLSteerInvert, + kFLEncoderInvert); case 1 -> ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPosMeters, - kFrontRightYPosMeters, - kFrontRightDriveInvert, - kFrontRightSteerInvert, - kFrontRightEncoderInvert); + kFRSteerMotorId, + kFRDriveMotorId, + kFREncoderId, + kFREncoderOffset, + kFRXPosMeters, + kFRYPosMeters, + kFRDriveInvert, + kFRSteerInvert, + kFREncoderInvert); case 2 -> ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPosMeters, - kBackLeftYPosMeters, - kBackLeftDriveInvert, - kBackLeftSteerInvert, - kBackLeftEncoderInvert); + kBLSteerMotorId, + kBLDriveMotorId, + kBLEncoderId, + kBLEncoderOffset, + kBLXPosMeters, + kBLYPosMeters, + kBLDriveInvert, + kBLSteerInvert, + kBLEncoderInvert); case 3 -> ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPosMeters, - kBackRightYPosMeters, - kBackRightDriveInvert, - kBackRightSteerInvert, - kBackRightEncoderInvert); + kBRSteerMotorId, + kBRDriveMotorId, + kBREncoderId, + kBREncoderOffset, + kBRXPosMeters, + kBRYPosMeters, + kBRDriveInvert, + kBRSteerInvert, + kBREncoderInvert); default -> throw new IllegalArgumentException("Invalid module index"); }; @@ -185,13 +186,13 @@ public ModuleIOBlended(int module) { // Configure turn motor var turnConfig = new SparkMaxConfig(); turnConfig - .inverted(turnInverted) + .inverted(constants.SteerMotorInverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(turnMotorCurrentLimit) + .smartCurrentLimit((int) DriveConstants.kSteerCurrentLimit) .voltageCompensation(12.0); turnConfig .absoluteEncoder - .inverted(turnEncoderInverted) + .inverted(constants.CANcoderInverted) .positionConversionFactor(turnEncoderPositionFactor) .velocityConversionFactor(turnEncoderVelocityFactor) .averageDepth(2); @@ -200,11 +201,15 @@ public ModuleIOBlended(int module) { .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) - .pidf(turnKp, 0.0, turnKd, 0.0); + .pidf( + DrivebaseConstants.steerPID.kP, + DrivebaseConstants.steerPID.kI, + DrivebaseConstants.steerPID.kD, + 0.0); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) .absoluteEncoderVelocityAlwaysOn(true) .absoluteEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -244,7 +249,7 @@ public ModuleIOBlended(int module) { turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(cancoder.getPosition()); // Configure periodic frames - BaseStatusSignal.setUpdateFrequencyForAll(Drive.ODOMETRY_FREQUENCY, drivePosition); + BaseStatusSignal.setUpdateFrequencyForAll(DriveConstants.kOdometryFrequency, drivePosition); BaseStatusSignal.setUpdateFrequencyForAll(50.0, driveVelocity, driveAppliedVolts, driveCurrent); ParentDevice.optimizeBusUtilizationForAll(driveTalon); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 20c5be92..f081ac48 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -36,6 +36,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 java.util.Queue; import java.util.function.DoubleSupplier; @@ -51,6 +52,8 @@ public class ModuleIOSpark implements ModuleIO { private final SparkBase turnSpark; private final RelativeEncoder driveEncoder; private final AbsoluteEncoder turnEncoder; + private final boolean turnInverted; + private final boolean turnEncoderInverted; // Closed loop controllers private final SparkClosedLoopController driveController; @@ -68,32 +71,48 @@ public class ModuleIOSpark implements ModuleIO { public ModuleIOSpark(int module) { zeroRotation = switch (module) { - case 0 -> new Rotation2d(kFrontLeftEncoderOffset); - case 1 -> new Rotation2d(kFrontRightEncoderOffset); - case 2 -> new Rotation2d(kBackLeftEncoderOffset); - case 3 -> new Rotation2d(kBackRightEncoderOffset); + case 0 -> new Rotation2d(kFLEncoderOffset); + case 1 -> new Rotation2d(kFREncoderOffset); + case 2 -> new Rotation2d(kBLEncoderOffset); + case 3 -> new Rotation2d(kBREncoderOffset); default -> new Rotation2d(); }; driveSpark = new SparkFlex( switch (module) { - case 0 -> kFrontLeftDriveMotorId; - case 1 -> kFrontRightDriveMotorId; - case 2 -> kBackLeftDriveMotorId; - case 3 -> kBackRightDriveMotorId; + case 0 -> kFLDriveMotorId; + case 1 -> kFRDriveMotorId; + case 2 -> kBLDriveMotorId; + case 3 -> kBRDriveMotorId; default -> 0; }, MotorType.kBrushless); turnSpark = new SparkMax( switch (module) { - case 0 -> kFrontLeftSteerMotorId; - case 1 -> kFrontRightSteerMotorId; - case 2 -> kBackLeftSteerMotorId; - case 3 -> kBackRightSteerMotorId; + case 0 -> kFLSteerMotorId; + case 1 -> kFRSteerMotorId; + case 2 -> kBLSteerMotorId; + case 3 -> kBRSteerMotorId; default -> 0; }, MotorType.kBrushless); + turnInverted = + switch (module) { + case 0 -> kFLSteerInvert; + case 1 -> kFRSteerInvert; + case 2 -> kBLSteerInvert; + case 3 -> kBRSteerInvert; + default -> false; + }; + turnEncoderInverted = + switch (module) { + case 0 -> kFLEncoderInvert; + case 1 -> kFREncoderInvert; + case 2 -> kBLEncoderInvert; + case 3 -> kBREncoderInvert; + default -> false; + }; driveEncoder = driveSpark.getEncoder(); turnEncoder = turnSpark.getAbsoluteEncoder(); driveController = driveSpark.getClosedLoopController(); @@ -103,7 +122,7 @@ public ModuleIOSpark(int module) { var driveConfig = new SparkFlexConfig(); driveConfig .idleMode(IdleMode.kBrake) - .smartCurrentLimit(driveMotorCurrentLimit) + .smartCurrentLimit((int) kDriveCurrentLimit) .voltageCompensation(12.0); driveConfig .encoder @@ -115,12 +134,14 @@ public ModuleIOSpark(int module) { .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pidf( - driveKp, 0.0, - driveKd, 0.0); + DrivebaseConstants.drivePID.kP, + DrivebaseConstants.drivePID.kI, + DrivebaseConstants.drivePID.kD, + 0.0); driveConfig .signals .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .primaryEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) .primaryEncoderVelocityAlwaysOn(true) .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -139,7 +160,7 @@ public ModuleIOSpark(int module) { turnConfig .inverted(turnInverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(turnMotorCurrentLimit) + .smartCurrentLimit((int) DriveConstants.kSteerCurrentLimit) .voltageCompensation(12.0); turnConfig .absoluteEncoder @@ -152,11 +173,15 @@ public ModuleIOSpark(int module) { .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) - .pidf(turnKp, 0.0, turnKd, 0.0); + .pidf( + DrivebaseConstants.steerPID.kP, + DrivebaseConstants.steerPID.kI, + DrivebaseConstants.steerPID.kD, + 0.0); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) .absoluteEncoderVelocityAlwaysOn(true) .absoluteEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -230,7 +255,9 @@ public void setTurnOpenLoop(double output) { @Override public void setDriveVelocity(double velocityRadPerSec) { - double ffVolts = driveKs * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; + double ffVolts = + DriveConstants.kDriveFrictionVoltage * Math.signum(velocityRadPerSec) + + driveKv * velocityRadPerSec; driveController.setReference( velocityRadPerSec, ControlType.kVelocity, 0, ffVolts, ArbFFUnits.kVoltage); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index ba9e1fb2..1a5d1f34 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -41,6 +41,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 java.util.Queue; import java.util.function.DoubleSupplier; @@ -56,6 +57,8 @@ public class ModuleIOSparkCANcoder implements ModuleIO { private final SparkBase turnSpark; private final RelativeEncoder driveEncoder; private final CANcoder cancoder; + private final boolean turnInverted; + private final boolean turnEncoderInverted; // Closed loop controllers private final SparkClosedLoopController driveController; @@ -79,40 +82,56 @@ public class ModuleIOSparkCANcoder implements ModuleIO { public ModuleIOSparkCANcoder(int module) { zeroRotation = switch (module) { - case 0 -> new Rotation2d(kFrontLeftEncoderOffset); - case 1 -> new Rotation2d(kFrontRightEncoderOffset); - case 2 -> new Rotation2d(kBackLeftEncoderOffset); - case 3 -> new Rotation2d(kBackRightEncoderOffset); + case 0 -> new Rotation2d(kFLEncoderOffset); + case 1 -> new Rotation2d(kFREncoderOffset); + case 2 -> new Rotation2d(kBLEncoderOffset); + case 3 -> new Rotation2d(kBREncoderOffset); default -> new Rotation2d(); }; driveSpark = new SparkFlex( switch (module) { - case 0 -> kFrontLeftDriveMotorId; - case 1 -> kFrontRightDriveMotorId; - case 2 -> kBackLeftDriveMotorId; - case 3 -> kBackRightDriveMotorId; + case 0 -> kFLDriveMotorId; + case 1 -> kFRDriveMotorId; + case 2 -> kBLDriveMotorId; + case 3 -> kBRDriveMotorId; default -> 0; }, MotorType.kBrushless); turnSpark = new SparkMax( switch (module) { - case 0 -> kFrontLeftSteerMotorId; - case 1 -> kFrontRightSteerMotorId; - case 2 -> kBackLeftSteerMotorId; - case 3 -> kBackRightSteerMotorId; + case 0 -> kFLSteerMotorId; + case 1 -> kFRSteerMotorId; + case 2 -> kBLSteerMotorId; + case 3 -> kBRSteerMotorId; default -> 0; }, MotorType.kBrushless); + turnInverted = + switch (module) { + case 0 -> kFLSteerInvert; + case 1 -> kFRSteerInvert; + case 2 -> kBLSteerInvert; + case 3 -> kBRSteerInvert; + default -> false; + }; + turnEncoderInverted = + switch (module) { + case 0 -> kFLEncoderInvert; + case 1 -> kFREncoderInvert; + case 2 -> kBLEncoderInvert; + case 3 -> kBREncoderInvert; + default -> false; + }; driveEncoder = driveSpark.getEncoder(); cancoder = new CANcoder( switch (module) { - case 0 -> kFrontLeftEncoderId; - case 1 -> kFrontRightEncoderId; - case 2 -> kBackLeftEncoderId; - case 3 -> kBackRightEncoderId; + case 0 -> kFLEncoderId; + case 1 -> kFREncoderId; + case 2 -> kBLEncoderId; + case 3 -> kBREncoderId; default -> 0; }, kCANbusName); @@ -123,7 +142,7 @@ public ModuleIOSparkCANcoder(int module) { var driveConfig = new SparkFlexConfig(); driveConfig .idleMode(IdleMode.kBrake) - .smartCurrentLimit(driveMotorCurrentLimit) + .smartCurrentLimit((int) kDriveCurrentLimit) .voltageCompensation(12.0); driveConfig .encoder @@ -135,12 +154,14 @@ public ModuleIOSparkCANcoder(int module) { .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pidf( - driveKp, 0.0, - driveKd, 0.0); + DrivebaseConstants.drivePID.kP, + DrivebaseConstants.drivePID.kI, + DrivebaseConstants.drivePID.kD, + 0.0); driveConfig .signals .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .primaryEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) .primaryEncoderVelocityAlwaysOn(true) .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -159,7 +180,7 @@ public ModuleIOSparkCANcoder(int module) { turnConfig .inverted(turnInverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(turnMotorCurrentLimit) + .smartCurrentLimit((int) DriveConstants.kSteerCurrentLimit) .voltageCompensation(12.0); turnConfig .absoluteEncoder @@ -172,11 +193,15 @@ public ModuleIOSparkCANcoder(int module) { .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) - .pidf(turnKp, 0.0, turnKd, 0.0); + .pidf( + DrivebaseConstants.steerPID.kP, + DrivebaseConstants.steerPID.kI, + DrivebaseConstants.steerPID.kD, + 0.0); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) .absoluteEncoderVelocityAlwaysOn(true) .absoluteEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -257,7 +282,9 @@ public void setTurnOpenLoop(double output) { @Override public void setDriveVelocity(double velocityRadPerSec) { - double ffVolts = driveKs * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; + double ffVolts = + DriveConstants.kDriveFrictionVoltage * Math.signum(velocityRadPerSec) + + driveKv * velocityRadPerSec; driveController.setReference( velocityRadPerSec, ControlType.kVelocity, 0, ffVolts, ArbFFUnits.kVoltage); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index e6f82e31..ddae737d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -181,7 +181,7 @@ public ModuleIOTalonFX(int module) { // Configure periodic frames BaseStatusSignal.setUpdateFrequencyForAll( - Drive.ODOMETRY_FREQUENCY, drivePosition, turnPosition); + DriveConstants.kOdometryFrequency, drivePosition, turnPosition); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, driveVelocity, diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index a42b4964..3c01c1ee 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -121,12 +121,12 @@ public void run() { signalsLock.lock(); try { if (isCANFD && phoenixSignals.length > 0) { - BaseStatusSignal.waitForAll(2.0 / Drive.ODOMETRY_FREQUENCY, phoenixSignals); + BaseStatusSignal.waitForAll(2.0 / DriveConstants.kOdometryFrequency, phoenixSignals); } else { // "waitForAll" does not support blocking on multiple signals with a bus // that is not CAN FD, regardless of Pro licensing. No reasoning for this // behavior is provided by the documentation. - Thread.sleep((long) (1000.0 / Drive.ODOMETRY_FREQUENCY)); + Thread.sleep((long) (1000.0 / DriveConstants.kOdometryFrequency)); if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals); } } catch (InterruptedException e) { diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java index 1f6e63e0..4ef20791 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java @@ -55,7 +55,7 @@ private SparkOdometryThread() { public void start() { if (timestampQueues.size() > 0) { - notifier.startPeriodic(1.0 / DriveConstants.odometryFrequency); + notifier.startPeriodic(1.0 / DriveConstants.kOdometryFrequency); } } diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java index 6db39c5c..ee332e8f 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -43,11 +43,11 @@ public Flywheel(FlywheelIO io) { case REAL: case REPLAY: ffModel = new SimpleMotorFeedforward(kStaticGainReal, kVelocityGainReal); - io.configurePID(kPReal, kIReal, kDReal); + io.configurePID(pidReal.kP, pidReal.kI, pidReal.kD); break; case SIM: ffModel = new SimpleMotorFeedforward(kStaticGainSim, kVelocityGainSim); - io.configurePID(kPSim, kISim, kDSim); + io.configurePID(pidSim.kP, pidSim.kI, pidSim.kD); break; default: ffModel = new SimpleMotorFeedforward(0.0, 0.0); diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java index 725413de..ad8a9058 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java @@ -58,7 +58,7 @@ public FlywheelIOSpark() { var leaderConfig = new SparkFlexConfig(); leaderConfig .idleMode(IdleMode.kBrake) - .smartCurrentLimit(DriveConstants.driveMotorCurrentLimit) + .smartCurrentLimit((int) DriveConstants.kDriveCurrentLimit) .voltageCompensation(12.0); leaderConfig.encoder.uvwMeasurementPeriod(10).uvwAverageDepth(2); leaderConfig @@ -70,7 +70,7 @@ public FlywheelIOSpark() { leaderConfig .signals .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / DriveConstants.odometryFrequency)) + .primaryEncoderPositionPeriodMs((int) (1000.0 / DriveConstants.kOdometryFrequency)) .primaryEncoderVelocityAlwaysOn(true) .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) From e2473f1dbe638848f67bab35d944cb76918a1c84 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 20 Dec 2024 16:00:47 -0700 Subject: [PATCH 09/30] Consolidation of vision constants Also, renamed DriveConstants module -> SwerveConstants modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.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/GyroIONavX.java modified: src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java modified: src/main/java/frc/robot/subsystems/drive/Module.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/ModuleIOTalonFX.java modified: src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java modified: src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java renamed: src/main/java/frc/robot/subsystems/drive/DriveConstants.java -> src/main/java/frc/robot/subsystems/drive/SwerveConstants.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java modified: src/main/java/frc/robot/subsystems/vision/Vision.java deleted: src/main/java/frc/robot/subsystems/vision/VisionConstants.java modified: src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java --- src/main/java/frc/robot/Constants.java | 41 +++++++++++++ src/main/java/frc/robot/RobotContainer.java | 37 ++++++------ .../frc/robot/commands/DriveCommands.java | 4 +- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../robot/subsystems/drive/GyroIONavX.java | 2 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 4 +- .../frc/robot/subsystems/drive/Module.java | 2 +- .../subsystems/drive/ModuleIOBlended.java | 10 ++-- .../robot/subsystems/drive/ModuleIOSpark.java | 6 +- .../drive/ModuleIOSparkCANcoder.java | 6 +- .../subsystems/drive/ModuleIOTalonFX.java | 8 +-- .../drive/PhoenixOdometryThread.java | 4 +- .../subsystems/drive/SparkOdometryThread.java | 2 +- ...iveConstants.java => SwerveConstants.java} | 2 +- .../flywheel_example/FlywheelIOSpark.java | 6 +- .../frc/robot/subsystems/vision/Vision.java | 2 +- .../subsystems/vision/VisionConstants.java | 60 ------------------- .../vision/VisionIOPhotonVisionSim.java | 2 +- 18 files changed, 91 insertions(+), 109 deletions(-) rename src/main/java/frc/robot/subsystems/drive/{DriveConstants.java => SwerveConstants.java} (99%) delete mode 100644 src/main/java/frc/robot/subsystems/vision/VisionConstants.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 312bc1f3..6d7a5796 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -23,6 +23,8 @@ 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.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; @@ -245,6 +247,45 @@ public static class VisionConstants { public static final double kThetaStdDevCoefficient = 0.01; } + public class VisionConstants2 { + // AprilTag layout + public static AprilTagFieldLayout aprilTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + // Camera names, must match names configured on coprocessor + public static String camera0Name = "camera_0"; + public static String camera1Name = "camera_1"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + public static Transform3d robotToCamera0 = + new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d robotToCamera1 = + new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + // Basic filtering thresholds + public static double maxAmbiguity = 0.3; + public static double maxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double linearStdDevBaseline = 0.02; // Meters + public static double angularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] cameraStdDevFactors = + new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 + }; + + // Multipliers to apply for MegaTag 2 observations + public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static double angularStdDevMegatag2Factor = + Double.POSITIVE_INFINITY; // No rotation data available + } + /** AprilTag Field Layout ************************************************ */ /* SEASON SPECIFIC! -- This section is for 2024 (Crescendo) */ // NOTE: This section will be updated to 2025 "Reefscape" following kickoff diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 06130c5a..dfcb62b3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,7 @@ package frc.robot; -import static frc.robot.subsystems.vision.VisionConstants.*; +import static frc.robot.Constants.VisionConstants2.*; import choreo.auto.AutoChooser; import choreo.auto.AutoFactory; @@ -45,7 +45,7 @@ import frc.robot.commands.DriveCommands; import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.subsystems.drive.SwerveConstants; import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; @@ -351,44 +351,45 @@ public static class Ports { /* DRIVETRAIN CAN DEVICE IDS */ // Input the correct Power Distribution Module port for each motor!!!! + // NOTE: The CAN ID and bus are set in the Swerve Generator (Phoenix Tuner or YAGSL) // Front Left public static final RobotDeviceId FL_DRIVE = - new RobotDeviceId(DriveConstants.kFLDriveMotorId, DriveConstants.kFLDriveCanbus, 18); + new RobotDeviceId(SwerveConstants.kFLDriveMotorId, SwerveConstants.kFLDriveCanbus, 18); public static final RobotDeviceId FL_ROTATION = - new RobotDeviceId(DriveConstants.kFLSteerMotorId, DriveConstants.kFLSteerCanbus, 19); + new RobotDeviceId(SwerveConstants.kFLSteerMotorId, SwerveConstants.kFLSteerCanbus, 19); public static final RobotDeviceId FL_CANCODER = - new RobotDeviceId(DriveConstants.kFLEncoderId, DriveConstants.kFLEncoderCanbus, null); + new RobotDeviceId(SwerveConstants.kFLEncoderId, SwerveConstants.kFLEncoderCanbus, null); // Front Right public static final RobotDeviceId FR_DRIVE = - new RobotDeviceId(DriveConstants.kFRDriveMotorId, DriveConstants.kFRDriveCanbus, 17); + new RobotDeviceId(SwerveConstants.kFRDriveMotorId, SwerveConstants.kFRDriveCanbus, 17); public static final RobotDeviceId FR_ROTATION = - new RobotDeviceId(DriveConstants.kFRSteerMotorId, DriveConstants.kFRSteerCanbus, 16); + new RobotDeviceId(SwerveConstants.kFRSteerMotorId, SwerveConstants.kFRSteerCanbus, 16); public static final RobotDeviceId FR_CANCODER = - new RobotDeviceId(DriveConstants.kFREncoderId, DriveConstants.kFREncoderCanbus, null); + new RobotDeviceId(SwerveConstants.kFREncoderId, SwerveConstants.kFREncoderCanbus, null); // Back Left public static final RobotDeviceId BL_DRIVE = - new RobotDeviceId(DriveConstants.kBLDriveMotorId, DriveConstants.kBLDriveCanbus, 1); + new RobotDeviceId(SwerveConstants.kBLDriveMotorId, SwerveConstants.kBLDriveCanbus, 1); public static final RobotDeviceId BL_ROTATION = - new RobotDeviceId(DriveConstants.kBLSteerMotorId, DriveConstants.kBLSteerCanbus, 0); + new RobotDeviceId(SwerveConstants.kBLSteerMotorId, SwerveConstants.kBLSteerCanbus, 0); public static final RobotDeviceId BL_CANCODER = - new RobotDeviceId(DriveConstants.kBLEncoderId, DriveConstants.kBLEncoderCanbus, null); + new RobotDeviceId(SwerveConstants.kBLEncoderId, SwerveConstants.kBLEncoderCanbus, null); // Back Right public static final RobotDeviceId BR_DRIVE = - new RobotDeviceId(DriveConstants.kBRDriveMotorId, DriveConstants.kBRSteerCanbus, 2); + new RobotDeviceId(SwerveConstants.kBRDriveMotorId, SwerveConstants.kBRSteerCanbus, 2); public static final RobotDeviceId BR_ROTATION = - new RobotDeviceId(DriveConstants.kBRSteerMotorId, DriveConstants.kBRSteerCanbus, 3); + new RobotDeviceId(SwerveConstants.kBRSteerMotorId, SwerveConstants.kBRSteerCanbus, 3); public static final RobotDeviceId BR_CANCODER = - new RobotDeviceId(DriveConstants.kBREncoderId, DriveConstants.kBREncoderCanbus, null); + new RobotDeviceId(SwerveConstants.kBREncoderId, SwerveConstants.kBREncoderCanbus, null); // Pigeon public static final RobotDeviceId PIGEON = - new RobotDeviceId(DriveConstants.kPigeonId, DriveConstants.kCANbusName, null); + new RobotDeviceId(SwerveConstants.kPigeonId, SwerveConstants.kCANbusName, null); /* SUBSYSTEM CAN DEVICE IDS */ - // This is where mechanism subsystem devices are defined + // This is where mechanism subsystem devices are defined (Including ID, bus, and power port) // Example: - public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, 8); - public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, 9); + public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, "", 8); + public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, "", 9); /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ // This is where digital I/O feedback devices are defined diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index ba208961..6fe259f3 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -33,7 +33,7 @@ import frc.robot.Constants.DrivebaseConstants; import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.subsystems.drive.SwerveConstants; import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; @@ -364,7 +364,7 @@ public static Command wheelRadiusCharacterization(Drive drive) { wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; } double wheelRadius = - (state.gyroDelta * DriveConstants.kDriveBaseRadiusMeters) / wheelDelta; + (state.gyroDelta * SwerveConstants.kDriveBaseRadiusMeters) / wheelDelta; NumberFormat formatter = new DecimalFormat("#0.000"); System.out.println( diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9631dd16..d4bc70d3 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -16,7 +16,7 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.subsystems.drive.SwerveConstants.*; import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.swerve.SwerveRequest; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 1f3ea00e..4d31f036 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -25,7 +25,7 @@ /** IO implementation for Pigeon2 */ public class GyroIONavX implements GyroIO { - private final AHRS navx = new AHRS(SPI.Port.kMXP, (byte) DriveConstants.kOdometryFrequency); + private final AHRS navx = new AHRS(SPI.Port.kMXP, (byte) SwerveConstants.kOdometryFrequency); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 31d18ad8..56130ab5 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -15,7 +15,7 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.subsystems.drive.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; @@ -42,7 +42,7 @@ public class GyroIOPigeon2 implements GyroIO { public GyroIOPigeon2() { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(DriveConstants.kOdometryFrequency); + yaw.setUpdateFrequency(SwerveConstants.kOdometryFrequency); yawVelocity.setUpdateFrequency(50.0); pigeon.optimizeBusUtilization(); yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 97a485f2..0b2ccdb1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -15,7 +15,7 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.subsystems.drive.SwerveConstants.*; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index 669abb33..2a375a98 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -15,7 +15,7 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.subsystems.drive.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; @@ -161,9 +161,9 @@ public ModuleIOBlended(int module) { default -> throw new IllegalArgumentException("Invalid module index"); }; - driveTalon = new TalonFX(constants.DriveMotorId, DriveConstants.kCANbusName); + driveTalon = new TalonFX(constants.DriveMotorId, SwerveConstants.kCANbusName); turnSpark = new SparkMax(constants.SteerMotorId, MotorType.kBrushless); - cancoder = new CANcoder(constants.CANcoderId, DriveConstants.kCANbusName); + cancoder = new CANcoder(constants.CANcoderId, SwerveConstants.kCANbusName); turnController = turnSpark.getClosedLoopController(); @@ -188,7 +188,7 @@ public ModuleIOBlended(int module) { turnConfig .inverted(constants.SteerMotorInverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit((int) DriveConstants.kSteerCurrentLimit) + .smartCurrentLimit((int) SwerveConstants.kSteerCurrentLimit) .voltageCompensation(12.0); turnConfig .absoluteEncoder @@ -249,7 +249,7 @@ public ModuleIOBlended(int module) { turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(cancoder.getPosition()); // Configure periodic frames - BaseStatusSignal.setUpdateFrequencyForAll(DriveConstants.kOdometryFrequency, drivePosition); + BaseStatusSignal.setUpdateFrequencyForAll(SwerveConstants.kOdometryFrequency, drivePosition); BaseStatusSignal.setUpdateFrequencyForAll(50.0, driveVelocity, driveAppliedVolts, driveCurrent); ParentDevice.optimizeBusUtilizationForAll(driveTalon); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index f081ac48..a20abfe6 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -15,7 +15,7 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.subsystems.drive.SwerveConstants.*; import static frc.robot.util.SparkUtil.*; import com.revrobotics.AbsoluteEncoder; @@ -160,7 +160,7 @@ public ModuleIOSpark(int module) { turnConfig .inverted(turnInverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit((int) DriveConstants.kSteerCurrentLimit) + .smartCurrentLimit((int) SwerveConstants.kSteerCurrentLimit) .voltageCompensation(12.0); turnConfig .absoluteEncoder @@ -256,7 +256,7 @@ public void setTurnOpenLoop(double output) { @Override public void setDriveVelocity(double velocityRadPerSec) { double ffVolts = - DriveConstants.kDriveFrictionVoltage * Math.signum(velocityRadPerSec) + SwerveConstants.kDriveFrictionVoltage * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; driveController.setReference( velocityRadPerSec, ControlType.kVelocity, 0, ffVolts, ArbFFUnits.kVoltage); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index 1a5d1f34..aeb152a1 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -15,7 +15,7 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.subsystems.drive.SwerveConstants.*; import static frc.robot.util.SparkUtil.*; import com.ctre.phoenix6.BaseStatusSignal; @@ -180,7 +180,7 @@ public ModuleIOSparkCANcoder(int module) { turnConfig .inverted(turnInverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit((int) DriveConstants.kSteerCurrentLimit) + .smartCurrentLimit((int) SwerveConstants.kSteerCurrentLimit) .voltageCompensation(12.0); turnConfig .absoluteEncoder @@ -283,7 +283,7 @@ public void setTurnOpenLoop(double output) { @Override public void setDriveVelocity(double velocityRadPerSec) { double ffVolts = - DriveConstants.kDriveFrictionVoltage * Math.signum(velocityRadPerSec) + SwerveConstants.kDriveFrictionVoltage * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; driveController.setReference( velocityRadPerSec, ControlType.kVelocity, 0, ffVolts, ArbFFUnits.kVoltage); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index ddae737d..16f05567 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -107,9 +107,9 @@ public ModuleIOTalonFX(int module) { default -> throw new IllegalArgumentException("Invalid module index"); }; - driveTalon = new TalonFX(constants.DriveMotorId, DriveConstants.kCANbusName); - turnTalon = new TalonFX(constants.SteerMotorId, DriveConstants.kCANbusName); - cancoder = new CANcoder(constants.CANcoderId, DriveConstants.kCANbusName); + driveTalon = new TalonFX(constants.DriveMotorId, SwerveConstants.kCANbusName); + turnTalon = new TalonFX(constants.SteerMotorId, SwerveConstants.kCANbusName); + cancoder = new CANcoder(constants.CANcoderId, SwerveConstants.kCANbusName); // Configure drive motor var driveConfig = constants.DriveMotorInitialConfigs; @@ -181,7 +181,7 @@ public ModuleIOTalonFX(int module) { // Configure periodic frames BaseStatusSignal.setUpdateFrequencyForAll( - DriveConstants.kOdometryFrequency, drivePosition, turnPosition); + SwerveConstants.kOdometryFrequency, drivePosition, turnPosition); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, driveVelocity, diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 3c01c1ee..90f0f6f9 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -121,12 +121,12 @@ public void run() { signalsLock.lock(); try { if (isCANFD && phoenixSignals.length > 0) { - BaseStatusSignal.waitForAll(2.0 / DriveConstants.kOdometryFrequency, phoenixSignals); + BaseStatusSignal.waitForAll(2.0 / SwerveConstants.kOdometryFrequency, phoenixSignals); } else { // "waitForAll" does not support blocking on multiple signals with a bus // that is not CAN FD, regardless of Pro licensing. No reasoning for this // behavior is provided by the documentation. - Thread.sleep((long) (1000.0 / DriveConstants.kOdometryFrequency)); + Thread.sleep((long) (1000.0 / SwerveConstants.kOdometryFrequency)); if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals); } } catch (InterruptedException e) { diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java index 4ef20791..6df6929c 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java @@ -55,7 +55,7 @@ private SparkOdometryThread() { public void start() { if (timestampQueues.size() > 0) { - notifier.startPeriodic(1.0 / DriveConstants.kOdometryFrequency); + notifier.startPeriodic(1.0 / SwerveConstants.kOdometryFrequency); } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java similarity index 99% rename from src/main/java/frc/robot/subsystems/drive/DriveConstants.java rename to src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index 07e078e4..bfe2d652 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -27,7 +27,7 @@ *

Does some translation of how the two keep values to return a completely unified API. This file * should not be modified. */ -public class DriveConstants { +public class SwerveConstants { // Declare all the constants public static final String kImuType; diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java index ad8a9058..718487fa 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java @@ -32,7 +32,7 @@ import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.math.util.Units; import frc.robot.RobotContainer.Ports; -import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.subsystems.drive.SwerveConstants; /** * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with @@ -58,7 +58,7 @@ public FlywheelIOSpark() { var leaderConfig = new SparkFlexConfig(); leaderConfig .idleMode(IdleMode.kBrake) - .smartCurrentLimit((int) DriveConstants.kDriveCurrentLimit) + .smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit) .voltageCompensation(12.0); leaderConfig.encoder.uvwMeasurementPeriod(10).uvwAverageDepth(2); leaderConfig @@ -70,7 +70,7 @@ public FlywheelIOSpark() { leaderConfig .signals .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / DriveConstants.kOdometryFrequency)) + .primaryEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) .primaryEncoderVelocityAlwaysOn(true) .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 3038741e..c1b769fc 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -17,7 +17,7 @@ package frc.robot.subsystems.vision; -import static frc.robot.subsystems.vision.VisionConstants.*; +import static frc.robot.Constants.VisionConstants2.*; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java deleted file mode 100644 index ba5b1218..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// 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.subsystems.vision; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; - -public class VisionConstants { - // AprilTag layout - public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - - // Camera names, must match names configured on coprocessor - public static String camera0Name = "camera_0"; - public static String camera1Name = "camera_1"; - - // Robot to camera transforms - // (Not used by Limelight, configure in web UI instead) - public static Transform3d robotToCamera0 = - new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); - public static Transform3d robotToCamera1 = - new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); - - // Basic filtering thresholds - public static double maxAmbiguity = 0.3; - public static double maxZError = 0.75; - - // Standard deviation baselines, for 1 meter distance and 1 tag - // (Adjusted automatically based on distance and # of tags) - public static double linearStdDevBaseline = 0.02; // Meters - public static double angularStdDevBaseline = 0.06; // Radians - - // Standard deviation multipliers for each camera - // (Adjust to trust some cameras more than others) - public static double[] cameraStdDevFactors = - new double[] { - 1.0, // Camera 0 - 1.0 // Camera 1 - }; - - // Multipliers to apply for MegaTag 2 observations - public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve - public static double angularStdDevMegatag2Factor = - Double.POSITIVE_INFINITY; // No rotation data available -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index 610c1f3f..51c6e60b 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -15,7 +15,7 @@ package frc.robot.subsystems.vision; -import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; +import static frc.robot.Constants.VisionConstants2.aprilTagLayout; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; From 702a55b98d70b10aba840adb3036aee660949eb6 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 20 Dec 2024 17:06:17 -0700 Subject: [PATCH 10/30] Combining and reorganizing constants modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/commands/ChoreoAutoController.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java modified: src/main/java/frc/robot/subsystems/vision/Vision.java modified: src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java modified: src/main/java/frc/robot/util/PowerMonitoring.java --- src/main/java/frc/robot/Constants.java | 224 ++++++++++++------ src/main/java/frc/robot/RobotContainer.java | 220 +++++------------ .../robot/commands/ChoreoAutoController.java | 8 +- .../frc/robot/subsystems/drive/Drive.java | 12 +- .../flywheel_example/FlywheelIOSpark.java | 9 +- .../flywheel_example/FlywheelIOTalonFX.java | 13 +- .../frc/robot/subsystems/vision/Vision.java | 9 +- .../vision/VisionIOPhotonVisionSim.java | 5 +- .../java/frc/robot/util/PowerMonitoring.java | 20 +- 9 files changed, 249 insertions(+), 271 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6d7a5796..2dcfa543 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,16 +22,16 @@ import com.pathplanner.lib.config.PIDConstants; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; 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.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; -import frc.robot.generated.TunerConstants; +import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; +import frc.robot.subsystems.drive.SwerveConstants; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; import frc.robot.util.RBSIEnum.AutoType; @@ -39,6 +39,7 @@ import frc.robot.util.RBSIEnum.RobotType; import frc.robot.util.RBSIEnum.SwerveType; import frc.robot.util.RBSIEnum.VisionType; +import frc.robot.util.RobotDeviceId; import java.io.IOException; import java.nio.file.Path; import lombok.Getter; @@ -97,14 +98,15 @@ public static void disableHAL() { /** Physical Constants for Robot Operation ******************************* */ public static final class PhysicalConstants { - public static final double kRobotMass = (148 - 20.3) * 0.453592; // 32lbs * kg per pound + public static final double kRobotMassKg = Units.lbsToKilograms(100.); public static final Matter kChassis = - new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), kRobotMass); - public static final double kLoopTime = 0.13; // s, 20ms + 110ms sprk max velocity lag + new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), kRobotMassKg); + // Robot moment of intertial; this can be obtained from a CAD model of your drivetrain. Usually, + // this is between 3 and 8 kg*m^2. + public static final double kRobotMOI = 6.8; - public static final double robotMassKg = 74.088; - public static final double robotMOI = 6.883; - public static final double wheelCOF = 1.2; + // Wheel coefficient of friction + public static final double kWheelCOF = 1.2; } /** Power Distribution Constants ********************************** */ @@ -116,44 +118,25 @@ public static final class PowerConstants { public static final double kSmallPortMaxCurrent = 20.; } - /** Accelerometer Constants ********************************************** */ - public static class AccelerometerConstants { - - // Insert here the orientation (CCW == +) of the Rio and IMU from the robot - // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference - // frame. - // NOTE: It is assumed that both the Rio and the IMU are mounted such that +Z is UP - public static final Rotation2d kRioOrientation = - switch (getRobot()) { - case COMPBOT -> Rotation2d.fromDegrees(0.); - case DEVBOT -> Rotation2d.fromDegrees(0.); - default -> Rotation2d.fromDegrees(0.); - }; - // IMU can be one of Pigeon2 or NavX - public static final Rotation2d kIMUOrientation = - switch (getRobot()) { - case COMPBOT -> Rotation2d.fromDegrees(0.); - case DEVBOT -> Rotation2d.fromDegrees(0.); - default -> Rotation2d.fromDegrees(0.); - }; - } - /** Drive Base Constants ************************************************* */ public static final class DrivebaseConstants { // Theoretical free speed (m/s) at 12v applied output; - // This needs to be tuned to your individual robot - // NOTE: If using SwerveType.PHOENIX6, adjust this in the Phoenix X Tuner Swerve Generator - public static final LinearVelocity kMaxLinearSpeed = TunerConstants.kSpeedAt12Volts; - // Otherwise, set the maximum linear speed here - // public static final double kMaxLinearSpeed = 5.21; + // IMPORTANT: Follow the AdvantageKit instructions for measuring the ACTUAL maximum linear speed + // of YOUR ROBOT, and replace the estimate here with your measured value! + public static final double kMaxLinearSpeed = Units.feetToMeters(18); // Set 3/4 of a rotation per second as the max angular velocity public static final double kMaxAngularSpeed = 1.5 * Math.PI; // Maximum chassis accelerations desired for robot motion -- metric / radians + // TODO: Compute the maximum linear acceleration given the PHYSICS of the ROBOT! public static final double kMaxLinearAccel = 4.0; // m/s/s - public static final double kMaxAngularAccel = Units.degreesToRadians(720); // deg/s/s + public static final double kMaxAngularAccel = Units.degreesToRadians(720); + + // Drive and Turn PID constants + public static final PIDConstants drivePID = new PIDConstants(0.05, 0.0, 0.0); + public static final PIDConstants steerPID = new PIDConstants(5.0, 0.0, 0.4); // Hold time on motor brakes when disabled public static final double kWheelLockTime = 10; // seconds @@ -164,12 +147,9 @@ public static final class DrivebaseConstants { public static final double kQuasiTimeout = 5.0; // seconds public static final double kDynamicTimeout = 3.0; // seconds - public static final PIDConstants drivePID = new PIDConstants(0.05, 0.0, 0.0); - public static final PIDConstants steerPID = new PIDConstants(5.0, 0.0, 0.4); - + // Not sure what to do with these, yet... // kDriveF = 0.13; // kDriveIZ = 0.0; - // kSteerF = 0.0; // kSteerIZ = 0.0; } @@ -195,44 +175,60 @@ public static final class FlywheelConstants { public static final PIDConstants pidSim = new PIDConstants(1.0, 0.0, 0.0); } + /** Accelerometer Constants ********************************************** */ + public static class AccelerometerConstants { + + // Insert here the orientation (CCW == +) of the Rio and IMU from the robot + // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference + // frame. + // NOTE: It is assumed that both the Rio and the IMU are mounted such that +Z is UP + public static final Rotation2d kRioOrientation = + switch (getRobot()) { + case COMPBOT -> Rotation2d.fromDegrees(0.); + case DEVBOT -> Rotation2d.fromDegrees(0.); + default -> Rotation2d.fromDegrees(0.); + }; + // IMU can be one of Pigeon2 or NavX + public static final Rotation2d kIMUOrientation = + switch (getRobot()) { + case COMPBOT -> Rotation2d.fromDegrees(0.); + case DEVBOT -> Rotation2d.fromDegrees(0.); + default -> Rotation2d.fromDegrees(0.); + }; + } + /** Operator Constants *************************************************** */ public static class OperatorConstants { - // Joystick Deadband + // Joystick Deadbands public static final double kLeftDeadband = 0.1; public static final double kRightDeadband = 0.1; public static final double kTurnConstant = 6; - } - - /** Autonomous Action Constants ****************************************** */ - public static final class AutoConstantsPathPlanner { - // Translation PID constants - public static final PIDConstants kAutoTranslatePID = new PIDConstants(0.7, 0, 0); - // Rotation PID constants - public static final PIDConstants kAutoAnglePID = new PIDConstants(0.4, 0, 0.01); - - public static final double ROBOT_MASS_KG = 74.088; - public static final double ROBOT_MOI = 6.883; - public static final double WHEEL_COF = 1.2; + // Override and Console Toggle Switches + // Assumes this controller: https://www.amazon.com/gp/product/B00UUROWWK + // Example from: + // https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2024-build-thread/442736/72 + public static final int DRIVER_SWITCH_0 = 1; + public static final int DRIVER_SWITCH_1 = 2; + public static final int DRIVER_SWITCH_2 = 3; + + public static final int OPERATOR_SWITCH_0 = 8; + public static final int OPERATOR_SWITCH_1 = 9; + public static final int OPERATOR_SWITCH_2 = 10; + public static final int OPERATOR_SWITCH_3 = 11; + public static final int OPERATOR_SWITCH_4 = 12; + + public static final int[] MULTI_TOGGLE = {4, 5}; } - /** Choreo Autonomous Action Constants *********************************** */ - public static final class AutoConstantsChoreo { - - public static final double kMaxSpeedMetersPerSecond = 3; - public static final double kMaxAccelerationMetersPerSecondSquared = 3; - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - - public static final double kPXController = 1; - public static final double kPYController = 1; - public static final double kPThetaController = 1; + /** Autonomous Action Constants ****************************************** */ + public static final class AutoConstants { - // Constraint for the motion profiled robot angle controller - public static final TrapezoidProfile.Constraints kThetaControllerConstraints = - new TrapezoidProfile.Constraints( - kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); + // 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); } /** Vision Constants (Assuming PhotonVision) ***************************** */ @@ -247,10 +243,27 @@ public static class VisionConstants { public static final double kThetaStdDevCoefficient = 0.01; } - public class VisionConstants2 { - // AprilTag layout - public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + /** Vision Camera Posses ************************************************* */ + public static class Cameras { + + public static final Pose3d[] cameraPoses = + switch (Constants.getRobot()) { + case COMPBOT -> + new Pose3d[] { + // Camera #1 + new Pose3d( + Units.inchesToMeters(-1.0), + Units.inchesToMeters(0), + Units.inchesToMeters(23.5), + new Rotation3d(0.0, Units.degreesToRadians(-20), 0.0)), + }; + case DEVBOT -> new Pose3d[] {}; + default -> new Pose3d[] {}; + }; + } + + /** More Vision Constants ********************************************** */ + public class MoreVisionConstants { // Camera names, must match names configured on coprocessor public static String camera0Name = "camera_0"; @@ -286,15 +299,73 @@ public class VisionConstants2 { Double.POSITIVE_INFINITY; // No rotation data available } + /** List of Device CAN and Power Distribution Circuit IDs **************** */ + public static class CANandPowerPorts { + + /* DRIVETRAIN CAN DEVICE IDS */ + // Input the correct Power Distribution Module port for each motor!!!! + // NOTE: The CAN ID and bus are set in the Swerve Generator (Phoenix Tuner or YAGSL) + + // Front Left + public static final RobotDeviceId FL_DRIVE = + new RobotDeviceId(SwerveConstants.kFLDriveMotorId, SwerveConstants.kFLDriveCanbus, 18); + public static final RobotDeviceId FL_ROTATION = + new RobotDeviceId(SwerveConstants.kFLSteerMotorId, SwerveConstants.kFLSteerCanbus, 19); + public static final RobotDeviceId FL_CANCODER = + new RobotDeviceId(SwerveConstants.kFLEncoderId, SwerveConstants.kFLEncoderCanbus, null); + // Front Right + public static final RobotDeviceId FR_DRIVE = + new RobotDeviceId(SwerveConstants.kFRDriveMotorId, SwerveConstants.kFRDriveCanbus, 17); + public static final RobotDeviceId FR_ROTATION = + new RobotDeviceId(SwerveConstants.kFRSteerMotorId, SwerveConstants.kFRSteerCanbus, 16); + public static final RobotDeviceId FR_CANCODER = + new RobotDeviceId(SwerveConstants.kFREncoderId, SwerveConstants.kFREncoderCanbus, null); + // Back Left + public static final RobotDeviceId BL_DRIVE = + new RobotDeviceId(SwerveConstants.kBLDriveMotorId, SwerveConstants.kBLDriveCanbus, 1); + public static final RobotDeviceId BL_ROTATION = + new RobotDeviceId(SwerveConstants.kBLSteerMotorId, SwerveConstants.kBLSteerCanbus, 0); + public static final RobotDeviceId BL_CANCODER = + new RobotDeviceId(SwerveConstants.kBLEncoderId, SwerveConstants.kBLEncoderCanbus, null); + // Back Right + public static final RobotDeviceId BR_DRIVE = + new RobotDeviceId(SwerveConstants.kBRDriveMotorId, SwerveConstants.kBRSteerCanbus, 2); + public static final RobotDeviceId BR_ROTATION = + new RobotDeviceId(SwerveConstants.kBRSteerMotorId, SwerveConstants.kBRSteerCanbus, 3); + public static final RobotDeviceId BR_CANCODER = + new RobotDeviceId(SwerveConstants.kBREncoderId, SwerveConstants.kBREncoderCanbus, null); + // Pigeon + public static final RobotDeviceId PIGEON = + new RobotDeviceId(SwerveConstants.kPigeonId, SwerveConstants.kCANbusName, null); + + /* SUBSYSTEM CAN DEVICE IDS */ + // This is where mechanism subsystem devices are defined (Including ID, bus, and power port) + // Example: + public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, "", 8); + public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, "", 9); + + /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ + // This is where digital I/O feedback devices are defined + // Example: + // public static final int ELEVATOR_BOTTOM_LIMIT = 3; + + /* LINEAR SERVO PWM CHANNELS */ + // This is where PWM-controlled devices (actuators, servos, pneumatics, etc.) + // are defined + // Example: + // public static final int INTAKE_SERVO = 0; + } + /** AprilTag Field Layout ************************************************ */ /* SEASON SPECIFIC! -- This section is for 2024 (Crescendo) */ // NOTE: This section will be updated to 2025 "Reefscape" following kickoff public static class AprilTagConstants { public static final double aprilTagWidth = Units.inchesToMeters(6.50); + public static final String aprilTagFamily = "36h11"; public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; - public static final AprilTagFieldLayout aprilTagFieldLayout = + public static final AprilTagFieldLayout aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); @Getter @@ -381,4 +452,9 @@ public static VisionType getVisionType() { public static boolean getPhoenixPro() { return phoenixPro; } + + /** Get the current AprilTag layout type. */ + public static AprilTagLayoutType getAprilTagLayoutType() { + return AprilTagConstants.defaultAprilTagType; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dfcb62b3..ea8ca0e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,7 @@ package frc.robot; -import static frc.robot.Constants.VisionConstants2.*; +import static frc.robot.Constants.MoreVisionConstants.*; import choreo.auto.AutoChooser; import choreo.auto.AutoFactory; @@ -28,24 +28,18 @@ import choreo.auto.AutoTrajectory; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.drive.SwerveConstants; import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; @@ -60,33 +54,32 @@ import frc.robot.util.OverrideSwitches; import frc.robot.util.PowerMonitoring; import frc.robot.util.RBSIEnum; -import frc.robot.util.RobotDeviceId; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** This is the location for defining robot hardware, commands, and controller button bindings. */ public class RobotContainer { - // Define the Driver and, optionally, the Operator/Co-Driver Controllers + /** Define the Driver and, optionally, the Operator/Co-Driver Controllers */ // Replace with ``CommandPS4Controller`` or ``CommandJoystick`` if needed - final CommandXboxController driverXbox = new CommandXboxController(0); - final CommandXboxController operatorXbox = new CommandXboxController(1); - final OverrideSwitches overrides = new OverrideSwitches(2); + final CommandXboxController driverXbox = new CommandXboxController(0); // Main Driver - // Autonomous Things - Field2d m_field = new Field2d(); + final CommandXboxController operatorXbox = new CommandXboxController(1); // Second Operator + final OverrideSwitches overrides = new OverrideSwitches(2); // Console toggle switches - // Declare the robot subsystems here + /** Declare the robot subsystems here ************************************ */ // These are the "Active Subsystems" that the robot controlls private final Drive m_drivebase; + private final Flywheel m_flywheel; // These are "Virtual Subsystems" that report information but have no motors private final Accelerometer m_accel; private final Vision m_vision; private final PowerMonitoring m_power; - // Dashboard inputs + /** Dashboard inputs ***************************************************** */ // AutoChoosers for both supported path planning types private final LoggedDashboardChooser autoChooserPathPlanner; + private final AutoChooser autoChooserChoreo; private final AutoFactory autoFactoryChoreo; // Input estimated battery capacity (if full, use printed value) @@ -99,16 +92,10 @@ public class RobotContainer { // Alerts private final Alert aprilTagLayoutAlert = new Alert("", AlertType.INFO); - /** Returns the current AprilTag layout type. */ - public AprilTagLayoutType getAprilTagLayoutType() { - return AprilTagConstants.defaultAprilTagType; - } - - public static AprilTagLayoutType staticGetAprilTagLayoutType() { - return AprilTagConstants.defaultAprilTagType; - } - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ + /** + * Constructor for the Robot Container. This container holds subsystems, opertator interface + * devices, and commands. + */ public RobotContainer() { // Instantiate Robot Subsystems based on RobotType @@ -210,49 +197,6 @@ private void defineAutoCommands() { NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.zero())); } - /** - * Set up the SysID routines from AdvantageKit - * - *

NOTE: These are currently only accessible with Constants.AutoType.PATHPLANNER - */ - private void definesysIdRoutines() { - if (Constants.getAutoType() == RBSIEnum.AutoType.PATHPLANNER) { - // Drivebase characterization - autoChooserPathPlanner.addOption( - "Drive Wheel Radius Characterization", - DriveCommands.wheelRadiusCharacterization(m_drivebase)); - autoChooserPathPlanner.addOption( - "Drive Simple FF Characterization", - DriveCommands.feedforwardCharacterization(m_drivebase)); - autoChooserPathPlanner.addOption( - "Drive SysId (Quasistatic Forward)", - m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Drive SysId (Quasistatic Reverse)", - m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( - "Drive SysId (Dynamic Forward)", - m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Drive SysId (Dynamic Reverse)", - m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - - // Example Flywheel SysId Characterization - autoChooserPathPlanner.addOption( - "Flywheel SysId (Quasistatic Forward)", - m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Flywheel SysId (Quasistatic Reverse)", - m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( - "Flywheel SysId (Dynamic Forward)", - m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Flywheel SysId (Dynamic Reverse)", - m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - } - } - /** * Use this method to define your button->command mappings. Buttons can be created by * instantiating a {@link GenericHID} or one of its subclasses ({@link @@ -338,107 +282,57 @@ public void setMotorBrake(boolean brake) { /** Updates the alerts. */ public void updateAlerts() { // AprilTag layout alert - boolean aprilTagAlertActive = getAprilTagLayoutType() != AprilTagLayoutType.OFFICIAL; + boolean aprilTagAlertActive = Constants.getAprilTagLayoutType() != AprilTagLayoutType.OFFICIAL; aprilTagLayoutAlert.set(aprilTagAlertActive); if (aprilTagAlertActive) { aprilTagLayoutAlert.setText( - "Non-official AprilTag layout in use (" + getAprilTagLayoutType().toString() + ")."); + "Non-official AprilTag layout in use (" + + Constants.getAprilTagLayoutType().toString() + + ")."); } } - /** List of Device CAN and Power Distribution Circuit IDs **************** */ - public static class Ports { - - /* DRIVETRAIN CAN DEVICE IDS */ - // Input the correct Power Distribution Module port for each motor!!!! - // NOTE: The CAN ID and bus are set in the Swerve Generator (Phoenix Tuner or YAGSL) - - // Front Left - public static final RobotDeviceId FL_DRIVE = - new RobotDeviceId(SwerveConstants.kFLDriveMotorId, SwerveConstants.kFLDriveCanbus, 18); - public static final RobotDeviceId FL_ROTATION = - new RobotDeviceId(SwerveConstants.kFLSteerMotorId, SwerveConstants.kFLSteerCanbus, 19); - public static final RobotDeviceId FL_CANCODER = - new RobotDeviceId(SwerveConstants.kFLEncoderId, SwerveConstants.kFLEncoderCanbus, null); - // Front Right - public static final RobotDeviceId FR_DRIVE = - new RobotDeviceId(SwerveConstants.kFRDriveMotorId, SwerveConstants.kFRDriveCanbus, 17); - public static final RobotDeviceId FR_ROTATION = - new RobotDeviceId(SwerveConstants.kFRSteerMotorId, SwerveConstants.kFRSteerCanbus, 16); - public static final RobotDeviceId FR_CANCODER = - new RobotDeviceId(SwerveConstants.kFREncoderId, SwerveConstants.kFREncoderCanbus, null); - // Back Left - public static final RobotDeviceId BL_DRIVE = - new RobotDeviceId(SwerveConstants.kBLDriveMotorId, SwerveConstants.kBLDriveCanbus, 1); - public static final RobotDeviceId BL_ROTATION = - new RobotDeviceId(SwerveConstants.kBLSteerMotorId, SwerveConstants.kBLSteerCanbus, 0); - public static final RobotDeviceId BL_CANCODER = - new RobotDeviceId(SwerveConstants.kBLEncoderId, SwerveConstants.kBLEncoderCanbus, null); - // Back Right - public static final RobotDeviceId BR_DRIVE = - new RobotDeviceId(SwerveConstants.kBRDriveMotorId, SwerveConstants.kBRSteerCanbus, 2); - public static final RobotDeviceId BR_ROTATION = - new RobotDeviceId(SwerveConstants.kBRSteerMotorId, SwerveConstants.kBRSteerCanbus, 3); - public static final RobotDeviceId BR_CANCODER = - new RobotDeviceId(SwerveConstants.kBREncoderId, SwerveConstants.kBREncoderCanbus, null); - // Pigeon - public static final RobotDeviceId PIGEON = - new RobotDeviceId(SwerveConstants.kPigeonId, SwerveConstants.kCANbusName, null); - - /* SUBSYSTEM CAN DEVICE IDS */ - // This is where mechanism subsystem devices are defined (Including ID, bus, and power port) - // Example: - public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, "", 8); - public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, "", 9); - - /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ - // This is where digital I/O feedback devices are defined - // Example: - // public static final int ELEVATOR_BOTTOM_LIMIT = 3; - - /* LINEAR SERVO PWM CHANNELS */ - // This is where PWM-controlled devices (actuators, servos, pneumatics, etc.) - // are defined - // Example: - // public static final int INTAKE_SERVO = 0; - } - - /** Override and Console Toggle Switches ********************************* */ - public static class Overrides { - - // Assumes this controller: https://www.amazon.com/gp/product/B00UUROWWK - // Example from: - // https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2024-build-thread/442736/72 - public static final int DRIVER_SWITCH_0 = 1; - public static final int DRIVER_SWITCH_1 = 2; - public static final int DRIVER_SWITCH_2 = 3; - - public static final int OPERATOR_SWITCH_0 = 8; - public static final int OPERATOR_SWITCH_1 = 9; - public static final int OPERATOR_SWITCH_2 = 10; - public static final int OPERATOR_SWITCH_3 = 11; - public static final int OPERATOR_SWITCH_4 = 12; - - public static final int[] MULTI_TOGGLE = {4, 5}; - } + /** + * Set up the SysID routines from AdvantageKit + * + *

NOTE: These are currently only accessible with Constants.AutoType.PATHPLANNER + */ + private void definesysIdRoutines() { + if (Constants.getAutoType() == RBSIEnum.AutoType.PATHPLANNER) { + // Drivebase characterization + autoChooserPathPlanner.addOption( + "Drive Wheel Radius Characterization", + DriveCommands.wheelRadiusCharacterization(m_drivebase)); + autoChooserPathPlanner.addOption( + "Drive Simple FF Characterization", + DriveCommands.feedforwardCharacterization(m_drivebase)); + autoChooserPathPlanner.addOption( + "Drive SysId (Quasistatic Forward)", + m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooserPathPlanner.addOption( + "Drive SysId (Quasistatic Reverse)", + m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooserPathPlanner.addOption( + "Drive SysId (Dynamic Forward)", + m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooserPathPlanner.addOption( + "Drive SysId (Dynamic Reverse)", + m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - /** Vision Camera Posses ************************************************* */ - public static class Cameras { - - public static final Pose3d[] cameraPoses = - switch (Constants.getRobot()) { - case COMPBOT -> - new Pose3d[] { - // Camera #1 - new Pose3d( - Units.inchesToMeters(-1.0), - Units.inchesToMeters(0), - Units.inchesToMeters(23.5), - new Rotation3d(0.0, Units.degreesToRadians(-20), 0.0)), - }; - case DEVBOT -> new Pose3d[] {}; - default -> new Pose3d[] {}; - }; + // Example Flywheel SysId Characterization + autoChooserPathPlanner.addOption( + "Flywheel SysId (Quasistatic Forward)", + m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooserPathPlanner.addOption( + "Flywheel SysId (Quasistatic Reverse)", + m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooserPathPlanner.addOption( + "Flywheel SysId (Dynamic Forward)", + m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooserPathPlanner.addOption( + "Flywheel SysId (Dynamic Reverse)", + m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + } } /** diff --git a/src/main/java/frc/robot/commands/ChoreoAutoController.java b/src/main/java/frc/robot/commands/ChoreoAutoController.java index 44810ba8..20fced4d 100644 --- a/src/main/java/frc/robot/commands/ChoreoAutoController.java +++ b/src/main/java/frc/robot/commands/ChoreoAutoController.java @@ -15,7 +15,7 @@ package frc.robot.commands; -import static frc.robot.Constants.AutoConstantsPathPlanner.*; +import static frc.robot.Constants.AutoConstants.*; import choreo.trajectory.SwerveSample; import edu.wpi.first.math.controller.PIDController; @@ -27,11 +27,11 @@ public class ChoreoAutoController implements Consumer { private final Drive drive; // drive subsystem private final PIDController xController = - new PIDController(kAutoTranslatePID.kP, 0.0, kAutoTranslatePID.kD); + new PIDController(kAutoDrivePID.kP, 0.0, kAutoDrivePID.kD); private final PIDController yController = - new PIDController(kAutoTranslatePID.kP, 0.0, kAutoTranslatePID.kD); + new PIDController(kAutoDrivePID.kP, 0.0, kAutoDrivePID.kD); private final PIDController headingController = - new PIDController(kAutoAnglePID.kP, 0.0, kAutoAnglePID.kD); + new PIDController(kAutoSteerPID.kP, 0.0, kAutoSteerPID.kD); public ChoreoAutoController(Drive drive) { this.drive = drive; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index d4bc70d3..3affb922 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -52,8 +52,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; -import frc.robot.Constants.AutoConstantsPathPlanner; import frc.robot.Constants.DrivebaseConstants; +import frc.robot.Constants.PhysicalConstants; import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; import java.util.concurrent.locks.Lock; @@ -66,12 +66,12 @@ public class Drive extends SubsystemBase { // PathPlanner config constants private static final RobotConfig PP_CONFIG = new RobotConfig( - AutoConstantsPathPlanner.ROBOT_MASS_KG, - AutoConstantsPathPlanner.ROBOT_MOI, + PhysicalConstants.kRobotMassKg, + PhysicalConstants.kRobotMOI, new ModuleConfig( kWheelRadiusMeters, - DrivebaseConstants.kMaxLinearSpeed.magnitude(), - AutoConstantsPathPlanner.WHEEL_COF, + DrivebaseConstants.kMaxLinearSpeed, + PhysicalConstants.kWheelCOF, DCMotor.getKrakenX60Foc(1).withReduction(kDriveGearRatio), kDriveSlipCurrent, 1), @@ -406,7 +406,7 @@ public void addVisionMeasurement( /** Returns the maximum linear speed in meters per sec. */ public double getMaxLinearSpeedMetersPerSec() { - return DrivebaseConstants.kMaxLinearSpeed.in(MetersPerSecond); + return DrivebaseConstants.kMaxLinearSpeed; } /** Returns the maximum angular speed in radians per sec. */ diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java index 718487fa..9a916bfc 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java @@ -31,7 +31,7 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.math.util.Units; -import frc.robot.RobotContainer.Ports; +import frc.robot.Constants.CANandPowerPorts; import frc.robot.subsystems.drive.SwerveConstants; /** @@ -42,14 +42,15 @@ public class FlywheelIOSpark implements FlywheelIO { // Define the leader / follower motors from the Ports section of RobotContainer private final SparkBase leader = - new SparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); + new SparkMax(CANandPowerPorts.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); private final SparkBase follower = - new SparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); + new SparkMax(CANandPowerPorts.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); private final RelativeEncoder encoder = leader.getEncoder(); private final SparkClosedLoopController pid = leader.getClosedLoopController(); // IMPORTANT: Include here all devices listed above that are part of this mechanism! public final int[] powerPorts = { - Ports.FLYWHEEL_LEADER.getPowerPort(), Ports.FLYWHEEL_FOLLOWER.getPowerPort() + CANandPowerPorts.FLYWHEEL_LEADER.getPowerPort(), + CANandPowerPorts.FLYWHEEL_FOLLOWER.getPowerPort() }; public FlywheelIOSpark() { diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java index 66c645ab..e294d1e6 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -31,18 +31,23 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import frc.robot.RobotContainer.Ports; +import frc.robot.Constants.CANandPowerPorts; public class FlywheelIOTalonFX implements FlywheelIO { // Define the leader / follower motors from the Ports section of RobotContainer private final TalonFX leader = - new TalonFX(Ports.FLYWHEEL_LEADER.getDeviceNumber(), Ports.FLYWHEEL_LEADER.getBus()); + new TalonFX( + CANandPowerPorts.FLYWHEEL_LEADER.getDeviceNumber(), + CANandPowerPorts.FLYWHEEL_LEADER.getBus()); private final TalonFX follower = - new TalonFX(Ports.FLYWHEEL_FOLLOWER.getDeviceNumber(), Ports.FLYWHEEL_FOLLOWER.getBus()); + new TalonFX( + CANandPowerPorts.FLYWHEEL_FOLLOWER.getDeviceNumber(), + CANandPowerPorts.FLYWHEEL_FOLLOWER.getBus()); // IMPORTANT: Include here all devices listed above that are part of this mechanism! public final int[] powerPorts = { - Ports.FLYWHEEL_LEADER.getPowerPort(), Ports.FLYWHEEL_FOLLOWER.getPowerPort() + CANandPowerPorts.FLYWHEEL_LEADER.getPowerPort(), + CANandPowerPorts.FLYWHEEL_FOLLOWER.getPowerPort() }; private final StatusSignal leaderPosition = leader.getPosition(); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index c1b769fc..5f985b68 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -17,7 +17,7 @@ package frc.robot.subsystems.vision; -import static frc.robot.Constants.VisionConstants2.*; +import static frc.robot.Constants.MoreVisionConstants.*; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.AprilTagConstants; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import java.util.LinkedList; import java.util.List; @@ -94,7 +95,7 @@ public void periodic() { // Add tag poses for (int tagId : inputs[cameraIndex].tagIds) { - var tagPose = aprilTagLayout.getTagPose(tagId); + var tagPose = AprilTagConstants.aprilTagLayout.getTagPose(tagId); if (tagPose.isPresent()) { tagPoses.add(tagPose.get()); } @@ -112,9 +113,9 @@ public void periodic() { // Must be within the field boundaries || observation.pose().getX() < 0.0 - || observation.pose().getX() > aprilTagLayout.getFieldLength() + || observation.pose().getX() > AprilTagConstants.aprilTagLayout.getFieldLength() || observation.pose().getY() < 0.0 - || observation.pose().getY() > aprilTagLayout.getFieldWidth(); + || observation.pose().getY() > AprilTagConstants.aprilTagLayout.getFieldWidth(); // Add pose to log robotPoses.add(observation.pose()); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index 51c6e60b..a71991c4 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -15,10 +15,9 @@ package frc.robot.subsystems.vision; -import static frc.robot.Constants.VisionConstants2.aprilTagLayout; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; +import frc.robot.Constants.AprilTagConstants; import java.util.function.Supplier; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; @@ -45,7 +44,7 @@ public VisionIOPhotonVisionSim( // Initialize vision sim if (visionSim == null) { visionSim = new VisionSystemSim("main"); - visionSim.addAprilTags(aprilTagLayout); + visionSim.addAprilTags(AprilTagConstants.aprilTagLayout); } // Add sim camera diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java index 0b7f12c9..31dc7e81 100644 --- a/src/main/java/frc/robot/util/PowerMonitoring.java +++ b/src/main/java/frc/robot/util/PowerMonitoring.java @@ -15,8 +15,8 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants.CANandPowerPorts; import frc.robot.Constants.PowerConstants; -import frc.robot.RobotContainer.Ports; import frc.robot.util.Alert.AlertType; import org.littletonrobotics.conduit.ConduitApi; import org.littletonrobotics.junction.Logger; @@ -33,7 +33,9 @@ public class PowerMonitoring extends VirtualSubsystem { private final RBSISubsystem[] subsystems; // Get the AdvantageKit conduit for pulling PDM information + @SuppressWarnings("unused") private LoggedPowerDistribution loggedPowerDistribution = LoggedPowerDistribution.getInstance(); + private ConduitApi conduit = ConduitApi.getInstance(); // Define local variables @@ -52,16 +54,16 @@ public class PowerMonitoring extends VirtualSubsystem { // DRIVE and STEER motor power ports private final int[] m_drivePowerPorts = { - Ports.FL_DRIVE.getPowerPort(), - Ports.FR_DRIVE.getPowerPort(), - Ports.BL_DRIVE.getPowerPort(), - Ports.BR_DRIVE.getPowerPort() + CANandPowerPorts.FL_DRIVE.getPowerPort(), + CANandPowerPorts.FR_DRIVE.getPowerPort(), + CANandPowerPorts.BL_DRIVE.getPowerPort(), + CANandPowerPorts.BR_DRIVE.getPowerPort() }; private final int[] m_steerPowerPorts = { - Ports.FL_ROTATION.getPowerPort(), - Ports.FR_ROTATION.getPowerPort(), - Ports.BL_ROTATION.getPowerPort(), - Ports.BR_ROTATION.getPowerPort() + CANandPowerPorts.FL_ROTATION.getPowerPort(), + CANandPowerPorts.FR_ROTATION.getPowerPort(), + CANandPowerPorts.BL_ROTATION.getPowerPort(), + CANandPowerPorts.BR_ROTATION.getPowerPort() }; // Class method definition, including inputs of optional subsystems From 0e9df59cbced2d1653ebb26af9f3c8157aa454da Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 20 Dec 2024 17:31:00 -0700 Subject: [PATCH 11/30] Late-breaking beta update modified: src/main/java/frc/robot/subsystems/drive/SwerveConstants.java modified: vendordeps/PathplannerLib-beta.json --- .../java/frc/robot/subsystems/drive/SwerveConstants.java | 2 -- vendordeps/PathplannerLib-beta.json | 6 +++--- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index bfe2d652..8f4d2e25 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -1,7 +1,5 @@ // Copyright (c) 2024 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License diff --git a/vendordeps/PathplannerLib-beta.json b/vendordeps/PathplannerLib-beta.json index 35959504..0185feb4 100644 --- a/vendordeps/PathplannerLib-beta.json +++ b/vendordeps/PathplannerLib-beta.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib-beta.json", "name": "PathplannerLib", - "version": "2025.0.0-beta-6", + "version": "2025.0.0-beta-6.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.0.0-beta-6" + "version": "2025.0.0-beta-6.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.0.0-beta-6", + "version": "2025.0.0-beta-6.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 2b60d355a712f4870dbaaacfe4717d3aef3843c4 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 29 Dec 2024 14:26:24 -0700 Subject: [PATCH 12/30] Updates from AdvantageKit since 19-Dec-2024 modified: src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java modified: src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java --- .../subsystems/vision/VisionIOLimelight.java | 3 +- .../vision/VisionIOPhotonVision.java | 28 ++++++++++++++++++- 2 files changed, 29 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index fdf07b79..21316dad 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -62,7 +62,8 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) { @Override public void updateInputs(VisionIOInputs inputs) { // Update connection status based on whether an update has been seen in the last 250ms - inputs.connected = (RobotController.getFPGATime() - latencySubscriber.getLastChange()) < 250; + inputs.connected = + ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250; // Update target observation inputs.latestTargetObservation = diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 390f03a9..b9c375ac 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -15,6 +15,8 @@ package frc.robot.subsystems.vision; +import static frc.robot.Constants.AprilTagConstants.*; + import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; @@ -59,7 +61,7 @@ public void updateInputs(VisionIOInputs inputs) { } // Add pose observation - if (result.multitagResult.isPresent()) { + if (result.multitagResult.isPresent()) { // Multitag result var multitagResult = result.multitagResult.get(); // Calculate robot pose @@ -85,6 +87,30 @@ public void updateInputs(VisionIOInputs inputs) { multitagResult.fiducialIDsUsed.size(), // Tag count totalTagDistance / result.targets.size(), // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type + + } 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()) { + Transform3d fieldToTarget = + new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); + Transform3d cameraToTarget = target.bestCameraToTarget; + 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( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + target.poseAmbiguity, // Ambiguity + 1, // Tag count + cameraToTarget.getTranslation().getNorm(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + } } } From 2d7c2bea339655f9be71c129d0fda704a43972be Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 29 Dec 2024 14:36:35 -0700 Subject: [PATCH 13/30] Update vendordeps modified: src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java modified: vendordeps/maple-sim.json --- .../frc/robot/subsystems/vision/VisionIOPhotonVision.java | 2 +- vendordeps/maple-sim.json | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index b9c375ac..7f374e65 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -87,7 +87,7 @@ public void updateInputs(VisionIOInputs inputs) { multitagResult.fiducialIDsUsed.size(), // Tag count totalTagDistance / result.targets.size(), // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type - + } else if (!result.targets.isEmpty()) { // Single tag result var target = result.targets.get(0); // Calculate robot pose diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index 2dbf1d32..4b4a9c57 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.2.5", + "version": "0.2.7", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.2.5" + "version": "0.2.7" }, { "groupId": "org.dyn4j", From 12ba3588ac21b56fd6741893658d3a2c614a49cf Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 29 Dec 2024 16:08:34 -0700 Subject: [PATCH 14/30] Clean up constants more modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/vision/Vision.java new file: src/main/java/frc/robot/util/GetJoystickValue.java --- src/main/java/frc/robot/Constants.java | 56 +++++++------------ src/main/java/frc/robot/RobotContainer.java | 47 +++++++++++----- .../frc/robot/subsystems/vision/Vision.java | 3 +- .../java/frc/robot/util/GetJoystickValue.java | 24 ++++++++ 4 files changed, 78 insertions(+), 52 deletions(-) create mode 100644 src/main/java/frc/robot/util/GetJoystickValue.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2dcfa543..630a15d3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,7 +22,6 @@ import com.pathplanner.lib.config.PIDConstants; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -200,6 +199,10 @@ public static class AccelerometerConstants { /** Operator Constants *************************************************** */ public static class OperatorConstants { + // Joystick Functions + // Set to TRUE for Drive = Left, Turn = Right; else FALSE + public static final boolean kDriveLeftTurnRight = true; + // Joystick Deadbands public static final double kLeftDeadband = 0.1; public static final double kRightDeadband = 0.1; @@ -241,50 +244,36 @@ public static class VisionConstants { public static final double kZMargin = 0.75; public static final double kXYZStdDevCoefficient = 0.005; public static final double kThetaStdDevCoefficient = 0.01; - } - /** Vision Camera Posses ************************************************* */ - public static class Cameras { + // Basic filtering thresholds + public static final double maxAmbiguity = 0.3; + public static final double maxZError = 0.75; - public static final Pose3d[] cameraPoses = - switch (Constants.getRobot()) { - case COMPBOT -> - new Pose3d[] { - // Camera #1 - new Pose3d( - Units.inchesToMeters(-1.0), - Units.inchesToMeters(0), - Units.inchesToMeters(23.5), - new Rotation3d(0.0, Units.degreesToRadians(-20), 0.0)), - }; - case DEVBOT -> new Pose3d[] {}; - default -> new Pose3d[] {}; - }; - } + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static final double linearStdDevBaseline = 0.02; // Meters + public static final double angularStdDevBaseline = 0.06; // Radians - /** More Vision Constants ********************************************** */ - public class MoreVisionConstants { + // Multipliers to apply for MegaTag 2 observations + public static final double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static final double angularStdDevMegatag2Factor = + Double.POSITIVE_INFINITY; // No rotation data available + } + /** Vision Camera Posses ************************************************* */ + public static class Cameras { // Camera names, must match names configured on coprocessor public static String camera0Name = "camera_0"; public static String camera1Name = "camera_1"; + // ... And more, if needed // Robot to camera transforms - // (Not used by Limelight, configure in web UI instead) + // (ONLY USED FOR PHOTONVISION -- Limelight: configure in web UI instead) public static Transform3d robotToCamera0 = new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); public static Transform3d robotToCamera1 = new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); - // Basic filtering thresholds - public static double maxAmbiguity = 0.3; - public static double maxZError = 0.75; - - // Standard deviation baselines, for 1 meter distance and 1 tag - // (Adjusted automatically based on distance and # of tags) - public static double linearStdDevBaseline = 0.02; // Meters - public static double angularStdDevBaseline = 0.06; // Radians - // Standard deviation multipliers for each camera // (Adjust to trust some cameras more than others) public static double[] cameraStdDevFactors = @@ -292,11 +281,6 @@ public class MoreVisionConstants { 1.0, // Camera 0 1.0 // Camera 1 }; - - // Multipliers to apply for MegaTag 2 observations - public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve - public static double angularStdDevMegatag2Factor = - Double.POSITIVE_INFINITY; // No rotation data available } /** List of Device CAN and Power Distribution Circuit IDs **************** */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ea8ca0e4..012fbd00 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,7 @@ package frc.robot; -import static frc.robot.Constants.MoreVisionConstants.*; +import static frc.robot.Constants.Cameras.*; import choreo.auto.AutoChooser; import choreo.auto.AutoFactory; @@ -37,6 +37,7 @@ import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; +import frc.robot.Constants.OperatorConstants; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.drive.Drive; @@ -50,6 +51,7 @@ import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; +import frc.robot.util.GetJoystickValue; import frc.robot.util.LoggedTunableNumber; import frc.robot.util.OverrideSwitches; import frc.robot.util.PowerMonitoring; @@ -61,9 +63,9 @@ public class RobotContainer { /** Define the Driver and, optionally, the Operator/Co-Driver Controllers */ // Replace with ``CommandPS4Controller`` or ``CommandJoystick`` if needed - final CommandXboxController driverXbox = new CommandXboxController(0); // Main Driver + final CommandXboxController driverController = new CommandXboxController(0); // Main Driver - final CommandXboxController operatorXbox = new CommandXboxController(1); // Second Operator + final CommandXboxController operatorController = new CommandXboxController(1); // Second Operator final OverrideSwitches overrides = new OverrideSwitches(2); // Console toggle switches /** Declare the robot subsystems here ************************************ */ @@ -205,40 +207,55 @@ private void defineAutoCommands() { */ private void configureBindings() { + // Send the proper joystick input based on driver preference -- Set this in `Constants.java` + GetJoystickValue driveStickY; + GetJoystickValue driveStickX; + GetJoystickValue turnStickX; + if (OperatorConstants.kDriveLeftTurnRight) { + driveStickY = driverController::getLeftY; + driveStickX = driverController::getLeftX; + turnStickX = driverController::getRightX; + } else { + driveStickY = driverController::getRightY; + driveStickX = driverController::getRightX; + turnStickX = driverController::getLeftX; + } + // SET STANDARD DRIVING AS DEFAULT COMMAND FOR THE DRIVEBASE - // NOTE: Left joystick controls lateral translation, right joystick (X) controls rotation m_drivebase.setDefaultCommand( DriveCommands.fieldRelativeDrive( m_drivebase, - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX(), - () -> driverXbox.getRightX())); + () -> -driveStickY.value(), + () -> -driveStickX.value(), + () -> turnStickX.value())); // Example Commands // Press B button while driving --> ROBOT-CENTRIC - driverXbox + driverController .b() .onTrue( Commands.runOnce( () -> DriveCommands.robotRelativeDrive( m_drivebase, - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX(), - () -> driverXbox.getRightX()), + () -> -driveStickY.value(), + () -> -driveStickX.value(), + () -> turnStickX.value()), m_drivebase)); // Press A button -> BRAKE - driverXbox.a().whileTrue(Commands.runOnce(() -> m_drivebase.setMotorBrake(true), m_drivebase)); + driverController + .a() + .whileTrue(Commands.runOnce(() -> m_drivebase.setMotorBrake(true), m_drivebase)); // Press X button --> Stop with wheels in X-Lock position - driverXbox.x().onTrue(Commands.runOnce(m_drivebase::stopWithX, m_drivebase)); + driverController.x().onTrue(Commands.runOnce(m_drivebase::stopWithX, m_drivebase)); // Press Y button --> Manually Re-Zero the Gyro - driverXbox.y().onTrue(Commands.runOnce(() -> m_drivebase.zero())); + driverController.y().onTrue(Commands.runOnce(() -> m_drivebase.zero())); // Press RIGHT BUMPER --> Run the example flywheel - driverXbox + driverController .rightBumper() .whileTrue( Commands.startEnd( diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 5f985b68..d285f469 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -17,7 +17,8 @@ package frc.robot.subsystems.vision; -import static frc.robot.Constants.MoreVisionConstants.*; +import static frc.robot.Constants.Cameras.*; +import static frc.robot.Constants.VisionConstants.*; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; diff --git a/src/main/java/frc/robot/util/GetJoystickValue.java b/src/main/java/frc/robot/util/GetJoystickValue.java new file mode 100644 index 00000000..e2188313 --- /dev/null +++ b/src/main/java/frc/robot/util/GetJoystickValue.java @@ -0,0 +1,24 @@ +// Copyright (c) 2024 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; + +/** + * Interface needed to abstraxct away which joystick is used for driving and which for steering with + * a swerve base. Teams may specify to use the left joystick for either driving or steering in the + * `Constants.java` file under OperatorConstants. + */ +@FunctionalInterface +public interface GetJoystickValue { + double value(); +} From 17efa75248b130d1ac6e89671be0c0f89fb2a1b1 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 29 Dec 2024 16:54:56 -0700 Subject: [PATCH 15/30] Remove duplicative drive command modified: src/main/java/frc/robot/commands/DriveCommands.java --- .../frc/robot/commands/DriveCommands.java | 40 ------------------- 1 file changed, 40 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 6fe259f3..71829e25 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -154,46 +154,6 @@ private static Translation2d getLinearVelocityFromJoysticks(double x, double y) .getTranslation(); } - /** - * Field relative drive command using two joysticks (controlling linear and angular velocities). - */ - public static Command joystickDrive( - Drive drive, - DoubleSupplier xSupplier, - DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier) { - return Commands.run( - () -> { - // Get linear velocity - Translation2d linearVelocity = - getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - - // Apply rotation deadband - double omega = - MathUtil.applyDeadband(omegaSupplier.getAsDouble(), OperatorConstants.kRightDeadband); - - // Square rotation value for more precise control - omega = Math.copySign(omega * omega, omega); - - // Convert to field relative speeds & send command - ChassisSpeeds speeds = - new ChassisSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec()); - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - speeds, - isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation())); - }, - drive); - } - /** * Field relative drive command using joystick for linear control and PID for angular control. * Possible use cases include snapping to an angle, aiming at a vision target, or controlling From 6ec382f2cf493e037428a2963f0ce48ccdebcbcf Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 2 Jan 2025 08:34:13 -0700 Subject: [PATCH 16/30] Update to 2025, WPILib 2025.1.1 modified: .github/dependabot.yml modified: AdvantageScope Swerve Calibration.json modified: WPILib-License.md modified: build.gradle modified: settings.gradle modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/Main.java modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.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/accelerometer/Accelerometer.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 modified: src/main/java/frc/robot/subsystems/drive/Module.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIO.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOSim.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/ModuleIOTalonFX.java modified: src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java modified: src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java modified: src/main/java/frc/robot/subsystems/drive/SwerveConstants.java modified: src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java modified: src/main/java/frc/robot/subsystems/vision/Vision.java modified: src/main/java/frc/robot/subsystems/vision/VisionIO.java modified: src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java modified: src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java modified: src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java modified: src/main/java/frc/robot/util/Alert.java modified: src/main/java/frc/robot/util/GeomUtil.java modified: src/main/java/frc/robot/util/GetJoystickValue.java modified: src/main/java/frc/robot/util/LocalADStarAK.java modified: src/main/java/frc/robot/util/LoggedTunableNumber.java modified: src/main/java/frc/robot/util/OverrideSwitches.java modified: src/main/java/frc/robot/util/PhoenixUtil.java modified: src/main/java/frc/robot/util/PowerMonitoring.java modified: src/main/java/frc/robot/util/RBSIEnum.java modified: src/main/java/frc/robot/util/RBSISubsystem.java modified: src/main/java/frc/robot/util/RobotDeviceId.java modified: src/main/java/frc/robot/util/SparkUtil.java modified: src/main/java/frc/robot/util/VirtualSubsystem.java modified: src/main/java/frc/robot/util/YagslConstants.java modified: src/test/CurrentLimitTests.java modified: src/test/FusedCANcoderTests.java modified: src/test/LatencyCompensationTests.java modified: src/test/RobotContainerTest.java --- .github/dependabot.yml | 6 ++++-- AdvantageScope Swerve Calibration.json | 2 +- WPILib-License.md | 2 +- build.gradle | 2 +- settings.gradle | 2 +- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Main.java | 2 +- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/commands/ChoreoAutoController.java | 2 +- src/main/java/frc/robot/commands/DriveCommands.java | 4 ++-- .../frc/robot/subsystems/accelerometer/Accelerometer.java | 2 +- src/main/java/frc/robot/subsystems/drive/Drive.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/GyroIO.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/GyroIONavX.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/Module.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/ModuleIO.java | 4 ++-- .../java/frc/robot/subsystems/drive/ModuleIOBlended.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java | 4 ++-- .../frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java | 4 ++-- .../java/frc/robot/subsystems/drive/ModuleIOTalonFX.java | 4 ++-- .../frc/robot/subsystems/drive/PhoenixOdometryThread.java | 4 ++-- .../frc/robot/subsystems/drive/SparkOdometryThread.java | 4 ++-- .../java/frc/robot/subsystems/drive/SwerveConstants.java | 2 +- .../frc/robot/subsystems/flywheel_example/Flywheel.java | 4 ++-- .../frc/robot/subsystems/flywheel_example/FlywheelIO.java | 4 ++-- .../robot/subsystems/flywheel_example/FlywheelIOSim.java | 4 ++-- .../robot/subsystems/flywheel_example/FlywheelIOSpark.java | 4 ++-- .../subsystems/flywheel_example/FlywheelIOTalonFX.java | 4 ++-- src/main/java/frc/robot/subsystems/vision/Vision.java | 6 +++--- src/main/java/frc/robot/subsystems/vision/VisionIO.java | 4 ++-- .../java/frc/robot/subsystems/vision/VisionIOLimelight.java | 4 ++-- .../frc/robot/subsystems/vision/VisionIOPhotonVision.java | 4 ++-- .../robot/subsystems/vision/VisionIOPhotonVisionSim.java | 4 ++-- src/main/java/frc/robot/util/Alert.java | 4 ++-- src/main/java/frc/robot/util/GeomUtil.java | 2 +- src/main/java/frc/robot/util/GetJoystickValue.java | 2 +- src/main/java/frc/robot/util/LocalADStarAK.java | 2 +- src/main/java/frc/robot/util/LoggedTunableNumber.java | 2 +- src/main/java/frc/robot/util/OverrideSwitches.java | 2 +- src/main/java/frc/robot/util/PhoenixUtil.java | 4 ++-- src/main/java/frc/robot/util/PowerMonitoring.java | 2 +- src/main/java/frc/robot/util/RBSIEnum.java | 2 +- src/main/java/frc/robot/util/RBSISubsystem.java | 2 +- src/main/java/frc/robot/util/RobotDeviceId.java | 2 +- src/main/java/frc/robot/util/SparkUtil.java | 4 ++-- src/main/java/frc/robot/util/VirtualSubsystem.java | 2 +- src/main/java/frc/robot/util/YagslConstants.java | 2 +- src/test/CurrentLimitTests.java | 4 ++-- src/test/FusedCANcoderTests.java | 4 ++-- src/test/LatencyCompensationTests.java | 4 ++-- src/test/RobotContainerTest.java | 2 +- 54 files changed, 89 insertions(+), 87 deletions(-) diff --git a/.github/dependabot.yml b/.github/dependabot.yml index ffd8ba16..436a2a8b 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -10,8 +10,10 @@ updates: - package-ecosystem: "github-actions" # Workflow files stored in the default location of `.github/workflows`. (You don't need to specify `/.github/workflows` for `directory`. You can use `directory: "/"`.) directory: "/" + labels: + - "github_actions" schedule: - interval: "weekly" + interval: "monthly" # Maintain dependencies for gradle - package-ecosystem: "gradle" @@ -20,4 +22,4 @@ updates: labels: - "gradle dependencies" schedule: - interval: "monthly" + interval: "weekly" diff --git a/AdvantageScope Swerve Calibration.json b/AdvantageScope Swerve Calibration.json index e3c2d54e..f412ed27 100644 --- a/AdvantageScope Swerve Calibration.json +++ b/AdvantageScope Swerve Calibration.json @@ -122,7 +122,7 @@ } } ], - "game": "2024 Field", + "game": "2025 Field", "origin": "blue" }, "controllerUUID": "psf0y633oclnjyocus23hcnq1d4tpyte", diff --git a/WPILib-License.md b/WPILib-License.md index 645e5425..d744196f 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2024 FIRST and other WPILib contributors +Copyright (c) 2009-2025 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/build.gradle b/build.gradle index 9c88d910..8de0975c 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3" + id "edu.wpi.first.GradleRIO" version "2025.1.1" id "com.peterabeles.gversion" version "1.10.3" id "com.diffplug.spotless" version "6.25.0" id "io.freefair.lombok" version "8.11" diff --git a/settings.gradle b/settings.gradle index d94f73c6..969c7b09 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2024' + String frcYear = '2025' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 630a15d3..9b1e4702 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 750b769a..3526e0ab 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d5ba409e..84af8888 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 012fbd00..8be3dc93 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/commands/ChoreoAutoController.java b/src/main/java/frc/robot/commands/ChoreoAutoController.java index 20fced4d..852cd6b6 100644 --- a/src/main/java/frc/robot/commands/ChoreoAutoController.java +++ b/src/main/java/frc/robot/commands/ChoreoAutoController.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright 2024 SleipnirGroup // https://choreo.autos/ diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 71829e25..89d2571a 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 91519de3..1a17d901 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3affb922..11d2e920 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 89467c24..c8c4d16f 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 4d31f036..8d5929d7 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 56130ab5..a4ae3d30 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 0b2ccdb1..c51680ef 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 23b2f26b..bcd3bb3c 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index 2a375a98..b019359b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2024 FRC 2486 +// Copyright 2024-2025 FRC 2486 // https://github.com/Coconuts2486-FRC // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 7d060c8b..671a4ca5 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index a20abfe6..68f6cdb1 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index aeb152a1..175654ff 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 16f05567..e07cb99c 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 90f0f6f9..3b3a2e56 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java index 6df6929c..8ccb2edc 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index 8f4d2e25..870aedc0 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java index ee332e8f..53e3b0d3 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java index 8c7d0139..5ffbf398 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java index fe3046d4..308a88ce 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java index 9a916bfc..b07eebe8 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java index e294d1e6..614b40e4 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index d285f469..34c676a2 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,8 +1,8 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2024 FRC 2486 +// Copyright (c) 2024-2025 FRC 2486 // http://github.com/Coconuts2486-FRC -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index f2b85b8f..6755c3a7 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index 21316dad..14e06a87 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 7f374e65..e61fc331 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index a71991c4..e6574596 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/Alert.java b/src/main/java/frc/robot/util/Alert.java index 5608c3b7..b29d4ee7 100644 --- a/src/main/java/frc/robot/util/Alert.java +++ b/src/main/java/frc/robot/util/Alert.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2024 FRC 6328 +// Copyright (c) 2024-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java index 3a59a7fd..3bc1578f 100644 --- a/src/main/java/frc/robot/util/GeomUtil.java +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/GetJoystickValue.java b/src/main/java/frc/robot/util/GetJoystickValue.java index e2188313..d83b9018 100644 --- a/src/main/java/frc/robot/util/GetJoystickValue.java +++ b/src/main/java/frc/robot/util/GetJoystickValue.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index 08b61727..e7828e50 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 Michael Jansen // http://gist.github.com/mjansen4857 diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 20c6b561..9db5d1cd 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/OverrideSwitches.java b/src/main/java/frc/robot/util/OverrideSwitches.java index a04b00e3..798b9bb8 100644 --- a/src/main/java/frc/robot/util/OverrideSwitches.java +++ b/src/main/java/frc/robot/util/OverrideSwitches.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java index cbe2d3ea..28951184 100644 --- a/src/main/java/frc/robot/util/PhoenixUtil.java +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java index 31dc7e81..52850d31 100644 --- a/src/main/java/frc/robot/util/PowerMonitoring.java +++ b/src/main/java/frc/robot/util/PowerMonitoring.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/RBSIEnum.java b/src/main/java/frc/robot/util/RBSIEnum.java index 8725c45a..b4de7e72 100644 --- a/src/main/java/frc/robot/util/RBSIEnum.java +++ b/src/main/java/frc/robot/util/RBSIEnum.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java index 7a5c8e46..829300ff 100644 --- a/src/main/java/frc/robot/util/RBSISubsystem.java +++ b/src/main/java/frc/robot/util/RBSISubsystem.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/RobotDeviceId.java b/src/main/java/frc/robot/util/RobotDeviceId.java index 055491bc..4f1c2141 100644 --- a/src/main/java/frc/robot/util/RobotDeviceId.java +++ b/src/main/java/frc/robot/util/RobotDeviceId.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 254 // https://github.com/team254 diff --git a/src/main/java/frc/robot/util/SparkUtil.java b/src/main/java/frc/robot/util/SparkUtil.java index 9c86fd2e..df627df4 100644 --- a/src/main/java/frc/robot/util/SparkUtil.java +++ b/src/main/java/frc/robot/util/SparkUtil.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java index 2943eb06..59117db7 100644 --- a/src/main/java/frc/robot/util/VirtualSubsystem.java +++ b/src/main/java/frc/robot/util/VirtualSubsystem.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/YagslConstants.java b/src/main/java/frc/robot/util/YagslConstants.java index 9630b932..2b3302fe 100644 --- a/src/main/java/frc/robot/util/YagslConstants.java +++ b/src/main/java/frc/robot/util/YagslConstants.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/test/CurrentLimitTests.java b/src/test/CurrentLimitTests.java index dcf25f9c..02459aa3 100644 --- a/src/test/CurrentLimitTests.java +++ b/src/test/CurrentLimitTests.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2024 Cross The Road Electronics +// Copyright (c) 2024-2025 Cross The Road Electronics // https://github.com/CrossTheRoadElec/Phoenix6-Examples // // This program is free software; you can redistribute it and/or diff --git a/src/test/FusedCANcoderTests.java b/src/test/FusedCANcoderTests.java index 077eb269..bda6925f 100644 --- a/src/test/FusedCANcoderTests.java +++ b/src/test/FusedCANcoderTests.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2024 Cross The Road Electronics +// Copyright (c) 2024-2025 Cross The Road Electronics // https://github.com/CrossTheRoadElec/Phoenix6-Examples // // This program is free software; you can redistribute it and/or diff --git a/src/test/LatencyCompensationTests.java b/src/test/LatencyCompensationTests.java index 27c696a2..4fb6b653 100644 --- a/src/test/LatencyCompensationTests.java +++ b/src/test/LatencyCompensationTests.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2024 Cross The Road Electronics +// Copyright (c) -2025 Cross The Road Electronics // https://github.com/CrossTheRoadElec/Phoenix6-Examples // // This program is free software; you can redistribute it and/or diff --git a/src/test/RobotContainerTest.java b/src/test/RobotContainerTest.java index a7912d60..ad125e03 100644 --- a/src/test/RobotContainerTest.java +++ b/src/test/RobotContainerTest.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage From 0651ebce2996d1f796e25aab337b8ba515424709 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 2 Jan 2025 10:03:07 -0700 Subject: [PATCH 17/30] Tie in CTRE Pro to all relevant places modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java modified: src/main/java/frc/robot/util/RBSIEnum.java --- src/main/java/frc/robot/Constants.java | 11 ++++-- src/main/java/frc/robot/RobotContainer.java | 15 ++++++-- .../subsystems/drive/ModuleIOBlended.java | 17 +++++++--- .../subsystems/drive/ModuleIOTalonFX.java | 34 ++++++++++++------- .../flywheel_example/FlywheelIOSpark.java | 6 +++- .../flywheel_example/FlywheelIOTalonFX.java | 7 +++- src/main/java/frc/robot/util/RBSIEnum.java | 15 ++++++++ 7 files changed, 81 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9b1e4702..e22e1069 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -34,7 +34,9 @@ import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; import frc.robot.util.RBSIEnum.AutoType; +import frc.robot.util.RBSIEnum.CTREPro; import frc.robot.util.RBSIEnum.Mode; +import frc.robot.util.RBSIEnum.MotorIdleMode; import frc.robot.util.RBSIEnum.RobotType; import frc.robot.util.RBSIEnum.SwerveType; import frc.robot.util.RBSIEnum.VisionType; @@ -67,7 +69,7 @@ public final class Constants { // under strict caveat emptor -- and submit any error and bugfixes // via GitHub issues. private static SwerveType swerveType = SwerveType.PHOENIX6; // PHOENIX6, YAGSL - private static boolean phoenixPro = false; // CTRE Pro License? true, false + private static CTREPro phoenixPro = CTREPro.UNLICENSED; // LICENSED, UNLICENSED private static AutoType autoType = AutoType.PATHPLANNER; // PATHPLANNER, CHOREO private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE @@ -156,6 +158,9 @@ public static final class DrivebaseConstants { /** Example Flywheel Mechanism Constants ********************************* */ public static final class FlywheelConstants { + // Mechanism idle mode + public static final MotorIdleMode kFlywheelIdleMode = MotorIdleMode.COAST; // BRAKE, COAST + // Mechanism motor gear ratio public static final double kFlywheelGearRatio = 1.5; @@ -200,7 +205,7 @@ public static class AccelerometerConstants { public static class OperatorConstants { // Joystick Functions - // Set to TRUE for Drive = Left, Turn = Right; else FALSE + // Set to TRUE for Drive = Left Stick, Turn = Right Stick; else FALSE public static final boolean kDriveLeftTurnRight = true; // Joystick Deadbands @@ -433,7 +438,7 @@ public static VisionType getVisionType() { } /** Get the current CTRE/Phoenix Pro License state */ - public static boolean getPhoenixPro() { + public static CTREPro getPhoenixPro() { return phoenixPro; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8be3dc93..64c3da0d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,8 @@ 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; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -222,6 +224,7 @@ private void configureBindings() { } // SET STANDARD DRIVING AS DEFAULT COMMAND FOR THE DRIVEBASE + // TODO: With a re-do of Phoenix Tuner X on George, ensure the signs are all correct!!!!! m_drivebase.setDefaultCommand( DriveCommands.fieldRelativeDrive( m_drivebase, @@ -229,7 +232,7 @@ private void configureBindings() { () -> -driveStickX.value(), () -> turnStickX.value())); - // Example Commands + // ** Example Commands -- Remap, remove, or change as desired ** // Press B button while driving --> ROBOT-CENTRIC driverController .b() @@ -252,7 +255,15 @@ private void configureBindings() { driverController.x().onTrue(Commands.runOnce(m_drivebase::stopWithX, m_drivebase)); // Press Y button --> Manually Re-Zero the Gyro - driverController.y().onTrue(Commands.runOnce(() -> m_drivebase.zero())); + driverController + .y() + .onTrue( + Commands.runOnce( + () -> + m_drivebase.setPose( + new Pose2d(m_drivebase.getPose().getTranslation(), new Rotation2d())), + m_drivebase) + .ignoringDisable(true)); // Press RIGHT BUMPER --> Run the example flywheel driverController diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index b019359b..d4d7eb73 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -31,6 +31,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.ControlType; @@ -50,6 +51,7 @@ import edu.wpi.first.units.measure.AngularVelocity; 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.util.PhoenixUtil; import frc.robot.util.SparkUtil; @@ -171,10 +173,10 @@ public ModuleIOBlended(int module) { var driveConfig = constants.DriveMotorInitialConfigs; driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveConfig.Slot0 = constants.DriveMotorGains; - driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent; - driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; + driveConfig.Feedback.SensorToMechanismRatio = SwerveConstants.kDriveGearRatio; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = SwerveConstants.kDriveSlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -SwerveConstants.kDriveSlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimit = SwerveConstants.kDriveCurrentLimit; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; driveConfig.MotorOutput.Inverted = constants.DriveMotorInverted @@ -231,6 +233,13 @@ public ModuleIOBlended(int module) { : SensorDirectionValue.CounterClockwise_Positive; cancoder.getConfigurator().apply(cancoderConfig); + // Set motor Closed Loop Output type based on Phoenix Pro status + constants.DriveMotorClosedLoopOutput = + switch (Constants.getPhoenixPro()) { + case LICENSED -> ClosedLoopOutputType.TorqueCurrentFOC; + case UNLICENSED -> ClosedLoopOutputType.Voltage; + }; + // Create timestamp queue timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index e07cb99c..4f5ed18e 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -35,6 +35,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -42,6 +43,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; import frc.robot.generated.TunerConstants; import java.util.Queue; @@ -115,10 +117,10 @@ public ModuleIOTalonFX(int module) { var driveConfig = constants.DriveMotorInitialConfigs; driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveConfig.Slot0 = constants.DriveMotorGains; - driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent; - driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; + driveConfig.Feedback.SensorToMechanismRatio = SwerveConstants.kDriveGearRatio; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = SwerveConstants.kDriveSlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -SwerveConstants.kDriveSlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimit = SwerveConstants.kDriveCurrentLimit; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; driveConfig.MotorOutput.Inverted = constants.DriveMotorInverted @@ -132,17 +134,11 @@ public ModuleIOTalonFX(int module) { turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; turnConfig.Slot0 = constants.SteerMotorGains; turnConfig.Feedback.FeedbackRemoteSensorID = constants.CANcoderId; - turnConfig.Feedback.FeedbackSensorSource = - switch (constants.FeedbackSource) { - case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; - case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; - case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder; - }; - turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; + turnConfig.Feedback.RotorToSensorRatio = SwerveConstants.kSteerGearRatio; + turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / SwerveConstants.kSteerGearRatio; turnConfig.MotionMagic.MotionMagicAcceleration = turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; - turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * SwerveConstants.kSteerGearRatio; turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1; turnConfig.ClosedLoopGeneral.ContinuousWrap = true; turnConfig.MotorOutput.Inverted = @@ -160,6 +156,18 @@ public ModuleIOTalonFX(int module) { : SensorDirectionValue.CounterClockwise_Positive; cancoder.getConfigurator().apply(cancoderConfig); + // Set motor Closed Loop Output type and CANCoder feedback type based on Phoenix Pro status + switch (Constants.getPhoenixPro()) { + case LICENSED: + turnConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + constants.DriveMotorClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; + constants.SteerMotorClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; + case UNLICENSED: + turnConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + constants.DriveMotorClosedLoopOutput = ClosedLoopOutputType.Voltage; + constants.SteerMotorClosedLoopOutput = ClosedLoopOutputType.Voltage; + } + // Create timestamp queue timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java index b07eebe8..3034bd00 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java @@ -58,7 +58,11 @@ public FlywheelIOSpark() { // Configure leader motor var leaderConfig = new SparkFlexConfig(); leaderConfig - .idleMode(IdleMode.kBrake) + .idleMode( + switch (kFlywheelIdleMode) { + case COAST -> IdleMode.kCoast; + case BRAKE -> IdleMode.kBrake; + }) .smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit) .voltageCompensation(12.0); leaderConfig.encoder.uvwMeasurementPeriod(10).uvwAverageDepth(2); diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java index 614b40e4..9364d321 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -60,9 +60,14 @@ public FlywheelIOTalonFX() { var config = new TalonFXConfiguration(); config.CurrentLimits.SupplyCurrentLimit = 30.0; config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.NeutralMode = + switch (kFlywheelIdleMode) { + case COAST -> NeutralModeValue.Coast; + case BRAKE -> NeutralModeValue.Brake; + }; leader.getConfigurator().apply(config); follower.getConfigurator().apply(config); + // If follower rotates in the opposite direction, set "OpposeMasterDirection" to true follower.setControl(new Follower(leader.getDeviceID(), false)); BaseStatusSignal.setUpdateFrequencyForAll( diff --git a/src/main/java/frc/robot/util/RBSIEnum.java b/src/main/java/frc/robot/util/RBSIEnum.java index b4de7e72..ddb31d86 100644 --- a/src/main/java/frc/robot/util/RBSIEnum.java +++ b/src/main/java/frc/robot/util/RBSIEnum.java @@ -51,4 +51,19 @@ public static enum VisionType { LIMELIGHT, // Limelight (https://docs.limelightvision.io/docs/docs-limelight/) NONE // No cameras } + + /** + * Enumerate CTRE Phoenix Pro Status + * https://v6.docs.ctr-electronics.com/en/latest/docs/licensing/licensing.html + */ + public static enum CTREPro { + LICENSED, // Have a valid 2025 CTRE Phoenix Pro License + UNLICENSED // Do not have a valid 2025 CTRE Phoenix Pro License + } + + /** Enumerate the supported motor idle modes */ + public static enum MotorIdleMode { + COAST, // Allow the motor to coast when idle + BRAKE // Hold motor position when idle + } } From 5d3078d332727be778c7349fdcc147a45caf470a Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 2 Jan 2025 10:58:24 -0700 Subject: [PATCH 18/30] Align drive commands with AK2025 templates modified: src/main/java/frc/robot/Constants.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/README --- src/main/java/frc/robot/Constants.java | 3 +- .../frc/robot/commands/DriveCommands.java | 98 ++++++++----------- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../java/frc/robot/subsystems/drive/README | 4 +- 4 files changed, 46 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e22e1069..4b513876 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -209,8 +209,7 @@ public static class OperatorConstants { public static final boolean kDriveLeftTurnRight = true; // Joystick Deadbands - public static final double kLeftDeadband = 0.1; - public static final double kRightDeadband = 0.1; + public static final double kDeadband = 0.1; public static final double kTurnConstant = 6; // Override and Console Toggle Switches diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 89d2571a..0fe4e417 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -63,18 +63,22 @@ public static Command fieldRelativeDrive( return Commands.run( () -> { // Get the Linear Velocity & Omega from inputs - Translation2d linearVelocity = getLinearVelocity(xSupplier, ySupplier); - double omega = getOmega(omegaSupplier); + Translation2d linearVelocity = + getLinearVelocity(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = getOmega(omegaSupplier.getAsDouble()); // Convert to field relative speeds & send command + ChassisSpeeds speeds = + new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec()); boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec(), + speeds, isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation())); @@ -92,10 +96,10 @@ public static Command robotRelativeDrive( DoubleSupplier omegaSupplier) { return Commands.run( () -> { - // Get the Linear Velocity & Omega from inputs - Translation2d linearVelocity = getLinearVelocity(xSupplier, ySupplier); - double omega = getOmega(omegaSupplier); + Translation2d linearVelocity = + getLinearVelocity(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = getOmega(omegaSupplier.getAsDouble()); // Run with straight-up velocities w.r.t. the robot! drive.runVelocity( @@ -107,53 +111,6 @@ public static Command robotRelativeDrive( drive); } - /** - * Compute the new linear velocity from inputs, including applying deadbands and squaring for - * smoothness - */ - private static Translation2d getLinearVelocity( - DoubleSupplier xSupplier, DoubleSupplier ySupplier) { - - // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband( - Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), - OperatorConstants.kLeftDeadband); - Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - - // Square values - linearMagnitude = linearMagnitude * linearMagnitude; - - return new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); - } - - /** - * Compute the new angular velocity from inputs, including applying deadbands and squaring for - * smoothness - */ - private static double getOmega(DoubleSupplier omegaSupplier) { - double omega = - MathUtil.applyDeadband(omegaSupplier.getAsDouble(), OperatorConstants.kRightDeadband); - return Math.copySign(omega * omega, omega); - } - - private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { - // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband(Math.hypot(x, y), OperatorConstants.kLeftDeadband); - Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); - - // Square magnitude for more precise control - linearMagnitude = linearMagnitude * linearMagnitude; - - // Return new linear velocity - return new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); - } - /** * Field relative drive command using joystick for linear control and PID for angular control. * Possible use cases include snapping to an angle, aiming at a vision target, or controlling @@ -180,7 +137,7 @@ public static Command joystickDriveAtAngle( () -> { // Get linear velocity Translation2d linearVelocity = - getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + getLinearVelocity(xSupplier.getAsDouble(), ySupplier.getAsDouble()); // Calculate angular speed double omega = @@ -209,6 +166,35 @@ public static Command joystickDriveAtAngle( .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } + /** + * Compute the new linear velocity from inputs, including applying deadbands and squaring for + * smoothness + */ + private static Translation2d getLinearVelocity(double x, double y) { + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), OperatorConstants.kDeadband); + Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); + + // Square magnitude for more precise control + linearMagnitude = linearMagnitude * linearMagnitude; + + // Return new linear velocity + return new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + } + + /** + * Compute the new angular velocity from inputs, including applying deadbands and squaring for + * smoothness + */ + private static double getOmega(double omega) { + omega = MathUtil.applyDeadband(omega, OperatorConstants.kDeadband); + return Math.copySign(omega * omega, omega); + } + + /***************************************************************************/ + /** DRIVEBASE CHARACTERIZATION ROUTINES ********************************** */ /** * Measures the velocity feedforward constants for the drive motors. * diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 11d2e920..870087d1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -277,7 +277,7 @@ public void setMotorBrake(boolean brake) { */ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, Constants.loopPeriodSecs); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DrivebaseConstants.kMaxLinearSpeed); diff --git a/src/main/java/frc/robot/subsystems/drive/README b/src/main/java/frc/robot/subsystems/drive/README index 1e84dc77..2b457143 100644 --- a/src/main/java/frc/robot/subsystems/drive/README +++ b/src/main/java/frc/robot/subsystems/drive/README @@ -12,10 +12,10 @@ Existing IO files: - ``GyroIONavX.java``: NavX as the swerve IMU, connected via SPI - ``ModuleIOTalonFX.java``: 2x TalonFX motors + CANcoder - - ``ModuleIOSparkMax.java``: 2x Rev NEO motors + analog absolute encoder connected to the RIO + - ``ModuleIOSpark.java``: 2x Rev NEO motors + analog absolute encoder connected to the RIO - ``ModuleIOSparkCANcoder.java``: 2x Rv NEO motors + CANcoder - ``ModuleIOBlended.java``: TalonFX drive, Rev NEO steer motors + CANcoder -------------------- -Last Modified: 22 Nov 2024, TPEB +Last Modified: 2 Jan 2025, TPEB From a3fba5ef7680718e32460130d636fe849b6d1c5d Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 2 Jan 2025 11:22:19 -0700 Subject: [PATCH 19/30] Cleaning up drive commands modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/commands/DriveCommands.java --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/commands/DriveCommands.java | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4b513876..6a197959 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -127,7 +127,7 @@ public static final class DrivebaseConstants { // of YOUR ROBOT, and replace the estimate here with your measured value! public static final double kMaxLinearSpeed = Units.feetToMeters(18); - // Set 3/4 of a rotation per second as the max angular velocity + // Set 3/4 of a rotation per second as the max angular velocity (radians/sec) public static final double kMaxAngularSpeed = 1.5 * Math.PI; // Maximum chassis accelerations desired for robot motion -- metric / radians diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 0fe4e417..2a8220a8 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -116,7 +116,7 @@ public static Command robotRelativeDrive( * Possible use cases include snapping to an angle, aiming at a vision target, or controlling * absolute rotation with a joystick. */ - public static Command joystickDriveAtAngle( + public static Command fieldRelativeDriveAtAngle( Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier, @@ -173,9 +173,10 @@ public static Command joystickDriveAtAngle( private static Translation2d getLinearVelocity(double x, double y) { // Apply deadband double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), OperatorConstants.kDeadband); - Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); + Rotation2d linearDirection = new Rotation2d(x, y); // Square magnitude for more precise control + // NOTE: The x & y values range from -1 to +1, so their squares are as well linearMagnitude = linearMagnitude * linearMagnitude; // Return new linear velocity @@ -194,7 +195,7 @@ private static double getOmega(double omega) { } /***************************************************************************/ - /** DRIVEBASE CHARACTERIZATION ROUTINES ********************************** */ + /** DRIVEBASE CHARACTERIZATION COMMANDS ********************************** */ /** * Measures the velocity feedforward constants for the drive motors. * From fe8c3f684f64d6e0f540d871160bb96d3b917383 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 2 Jan 2025 13:13:04 -0700 Subject: [PATCH 20/30] Add joystick slew rate limiter modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/commands/DriveCommands.java --- src/main/java/frc/robot/Constants.java | 3 +++ .../java/frc/robot/commands/DriveCommands.java | 15 +++++++++++---- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6a197959..35d6aec0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -212,6 +212,9 @@ public static class OperatorConstants { public static final double kDeadband = 0.1; public static final double kTurnConstant = 6; + // Joystick slew rate limiters to smooth erratic joystick motions, measured in units per second + public static final double kJoystickSlewLimit = 0.5; + // Override and Console Toggle Switches // Assumes this controller: https://www.amazon.com/gp/product/B00UUROWWK // Example from: diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 2a8220a8..9e284802 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -50,6 +50,12 @@ public class DriveCommands { private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + // Create slew rate limiters for smoothing erratic joystick motions + private static final SlewRateLimiter linearVelocityFilter = + new SlewRateLimiter(OperatorConstants.kJoystickSlewLimit); + private static final SlewRateLimiter omegaFilter = + new SlewRateLimiter(OperatorConstants.kJoystickSlewLimit); + private DriveCommands() {} /** @@ -168,7 +174,7 @@ public static Command fieldRelativeDriveAtAngle( /** * Compute the new linear velocity from inputs, including applying deadbands and squaring for - * smoothness + * smoothness. Also apply the linear velocity Slew Rate Limiter. */ private static Translation2d getLinearVelocity(double x, double y) { // Apply deadband @@ -181,17 +187,18 @@ private static Translation2d getLinearVelocity(double x, double y) { // Return new linear velocity return new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .transformBy( + new Transform2d(linearVelocityFilter.calculate(linearMagnitude), 0.0, new Rotation2d())) .getTranslation(); } /** * Compute the new angular velocity from inputs, including applying deadbands and squaring for - * smoothness + * smoothness. Also apply the angular Slew Rate Limiter. */ private static double getOmega(double omega) { omega = MathUtil.applyDeadband(omega, OperatorConstants.kDeadband); - return Math.copySign(omega * omega, omega); + return omegaFilter.calculate(Math.copySign(omega * omega, omega)); } /***************************************************************************/ From 38f6391cb70c4caa90502710f591682815064677 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 2 Jan 2025 14:03:23 -0700 Subject: [PATCH 21/30] Update FRC year for WPILib kickoff release modified: .wpilib/wpilib_preferences.json --- .wpilib/wpilib_preferences.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index a1591102..51005bf5 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2025beta", + "projectYear": "2025", "teamNumber": 0 } From 1ad24e0747bff029ae4037f141920a7d3c53c220 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 2 Jan 2025 15:42:24 -0700 Subject: [PATCH 22/30] Updated install instructions Also regenerated `TunerConstants.java` with latest Tuner X Preview version. modified: INSTALL.md modified: src/main/java/frc/robot/generated/TunerConstants.java --- INSTALL.md | 49 ++++++++++++++----- .../frc/robot/generated/TunerConstants.java | 30 ++++++------ 2 files changed, 51 insertions(+), 28 deletions(-) mode change 100644 => 100755 src/main/java/frc/robot/generated/TunerConstants.java diff --git a/INSTALL.md b/INSTALL.md index ce8cb1c8..97a07847 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -24,13 +24,26 @@ recommended to **not** select the "Include all branches" checkbox. -------- -### Setting up your new project +### Software Requirements + +The Az-RBSI requires the [2025 WPILib Installer]( +https://github.com/wpilibsuite/allwpilib/releases) (VSCode and associated +tools), 2025 firmware installed on all hardware (motors, encoders, power +distribution, etc.), the [2025 NI FRC Game Tools]( +https://www.ni.com/en/support/downloads/drivers/download.frc-game-tools.html) +(Driver Station and associated tools), and the [2025 CTRE Phoenix Tuner X]( +https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/index.html). Take a +moment to update all software and firmware before attempting to load your new +robot project. + +Please note that you need these _minimum_ versions of the following components: + +* WPILib `2025.1.1` +* RoboRIO image `FRC_roboRIO_2025_v2.0` -**The Az-RBSI requires the 2025 WPILib Installer (VSCode and associated tools), -2025 firmware installed on all hardware (motors, encoders, power distribution, -etc.), the 2025 NI FRC Game Tools (Driver Station and associated tools), and -the 2025 CTRE Phoenix Tuner X. Take a moment to update all software and -firmware before attempting to load your new robot project.** +-------- + +### Setting up your new project When your new robot code respository is created, it will have a single commit that contains the entire Az-RBSI template for the current release. (See the @@ -42,8 +55,8 @@ steps you need to complete: 1. Add your team number to the `.wpilib/wpilib_preferences.json` file. The generic Az-RBSI template contains a team number "0", and your code will not - deploy properly if this variable is not set (*e.g.*, VSCode looks for the - RoboRIO on IP address `10.TE.AM.2`, and it will not find anything if it + deploy properly if this variable is not set (*i.e.*, since VSCode looks for + the RoboRIO on IP address `10.TE.AM.2`, it will not find anything if it tries to contact `10.0.0.2`.) If you forget to change this value, you will get an error message when deploying code to your robot like: @@ -86,14 +99,24 @@ method are encouraged to submit bug reports and code fixes to the [Az-RBSI repository](https://github.com/AZ-First/Az-RBSI). 6. The Az-RBSI expects an Xbox-style controller -- if you have a PS4 or other, - change this at the top of the `RobotContainer.java` file in the - `src/main/java/frc/robot` directory. - -7. Power ports... + substitute the proper command-based controller class for + `CommandXboxController` near the top of the `RobotContainer.java` file in + the `src/main/java/frc/robot` directory. + +7. Power monitoring by subsystem is included in the Az-RBSI. In order to + properly match subsystems to ports on your Power Distribution Module, + carefully edit the `CANandPowerPorts` of `Constants.java` to include the + proper power ports for each motor in your drivetrain, and include any + motors from additional subsystems you add to your robot. To include + additional subsystems in the monitoring, add them to the [`m_power` + instantiation]( + https://github.com/AZ-First/Az-RBSI/blob/38f6391cb70c4caa90502710f591682815064677/src/main/java/frc/robot/RobotContainer.java#L154-L157) in the `RobotContainer.java` file. 8. All of the constants for needed for tuning your robot should be in the `Constants.java` file in the `src/main/java/frc/robot` directory. This file - should be thoroughly edited to match the particulars of your robot. + should be thoroughly edited to match the particulars of your robot. Be sure + to work through each section of this file and include the proper values for + your robot. -------- diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java old mode 100644 new mode 100755 index d1f48432..72a2488c --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -29,10 +29,10 @@ public class TunerConstants { new Slot0Configs() .withKP(100) .withKI(0) - .withKD(1.0) - .withKS(0.2) - .withKV(0.0) - .withKA(0.0) + .withKD(0.5) + .withKS(0.1) + .withKV(2.66) + .withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput @@ -76,14 +76,14 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.41); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 0; + private static final double kCoupleRatio = 3.125; - private static final double kDriveGearRatio = 6.746031746031747; - private static final double kSteerGearRatio = 15.42857142857143; + private static final double kDriveGearRatio = 5.902777777777778; + private static final double kSteerGearRatio = 21.428571428571427; private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; @@ -92,11 +92,11 @@ public class TunerConstants { private static final int kPigeonId = 13; // These are only used for simulation - private static final double kSteerInertia = 0.004; - private static final double kDriveInertia = 0.025; + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.25); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.25); + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() @@ -129,7 +129,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.472900390625); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.47265625); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftCANcoderInverted = false; @@ -151,7 +151,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.4091796875); + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.411376953125); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftCANcoderInverted = false; @@ -162,7 +162,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.086181640625); + private static final Angle kBackRightEncoderOffset = Rotations.of(0.086669921875); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightCANcoderInverted = false; From 22eea3b469fa513a491c449bb28841bee5428b54 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 2 Jan 2025 23:15:21 -0700 Subject: [PATCH 23/30] Revert to WPILib 2025.1.1-beta-3 Just trying to get the robot to run! modified: .wpilib/wpilib_preferences.json modified: build.gradle modified: settings.gradle modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java modified: src/main/java/frc/robot/subsystems/flywheel_example/README new file: src/main/java/frc/robot/subsystems/vision/README --- .wpilib/wpilib_preferences.json | 2 +- build.gradle | 2 +- settings.gradle | 2 +- src/main/java/frc/robot/Robot.java | 2 ++ .../frc/robot/subsystems/drive/Drive.java | 1 + .../robot/subsystems/flywheel_example/README | 2 +- .../java/frc/robot/subsystems/vision/README | 29 +++++++++++++++++++ 7 files changed, 36 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/vision/README diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 51005bf5..a1591102 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2025", + "projectYear": "2025beta", "teamNumber": 0 } diff --git a/build.gradle b/build.gradle index 8de0975c..9c88d910 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3" id "com.peterabeles.gversion" version "1.10.3" id "com.diffplug.spotless" version "6.25.0" id "io.freefair.lombok" version "8.11" diff --git a/settings.gradle b/settings.gradle index 969c7b09..d94f73c6 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2025' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 84af8888..f5bf2c26 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -143,6 +143,8 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { + + // TODO: Make sure Gyro inits here with whatever is in the path planning thingie m_robotContainer.setMotorBrake(true); switch (Constants.getAutoType()) { case PATHPLANNER: diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 870087d1..052e2b3b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -129,6 +129,7 @@ public Drive() { for (int i = 0; i < 4; i++) { switch (modType) { case 0b00000000: // ALL-CTRE + // TODO: ADD CASE FOR USING NAVX!!! throw new RuntimeException( "For an all-CTRE drive base, use Phoenix Tuner X Swerve Generator instead of YAGSL!"); case 0b00010000: // Blended Talon Drive / NEO Steer diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/README b/src/main/java/frc/robot/subsystems/flywheel_example/README index 7cad088f..b796c05d 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/README +++ b/src/main/java/frc/robot/subsystems/flywheel_example/README @@ -10,7 +10,7 @@ with AdvantageKit. The structure of the subsystem directory is: * FlywheelIO.java The base subsystem I/O (input/output) interface that contains the - structure and class varibales needed by the library-specific modules. + structure and class variables needed by the library-specific modules. * FlywheelIOSim.java Simulated flywheel module, for use with robot simulations, does not diff --git a/src/main/java/frc/robot/subsystems/vision/README b/src/main/java/frc/robot/subsystems/vision/README new file mode 100644 index 00000000..a8c8ad27 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/README @@ -0,0 +1,29 @@ +This directory contains an the basic vision template, as distributed with +AdvantageKit. The structure of the subsystem directory is: + + * Vision.java + The base subsystem definitions and outward-facing (public) hardware- + agnostic functions for interacting with the subsystem. The idea is + that any of the hardware-specific (i.e. camera system API calls) + functionality is insulated from the rest of the robot code within + the library-specific modules in this directory. + + * VisionIO.java + The base subsystem I/O (input/output) interface that contains the + structure and class variables needed by the library-specific modules. + + * VisionIOLimelight.java + The vision implementation for the Limelight camera system, which maps + Limelight-specific API calls back onto the generic vision API defined + above. + + * VisionIOPhotonVision.java + The vision implementation for the Photon Vision camera system, which + maps PhotonLib-specific API calls back onto the generic vision API + defined above. + + * VisionIOPhotonVisionSim.java + Simulated vision module, for use with robot simulations, does not + control actual hardware. + + \ No newline at end of file From 4a0f6ec55c0111fb8e640a90bae3b9f254113ee4 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 4 Jan 2025 17:50:52 -0700 Subject: [PATCH 24/30] Updating more vendordeps; WPILib 2025.1.1 Gold Still waiting on Photon Vision 2025 Gold Standard. Also need to fix issues related to Phoenix 6 Gold (vs beta). Update the PathPlanner navgrid. modified: .wpilib/wpilib_preferences.json modified: build.gradle modified: settings.gradle modified: src/main/deploy/pathplanner/navgrid.json modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java modified: src/main/java/frc/robot/subsystems/drive/GyroIONavX.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/flywheel_example/FlywheelIOSim.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java modified: src/main/java/frc/robot/util/OverrideSwitches.java modified: vendordeps/AdvantageKit.json renamed: vendordeps/ChoreoLib2025Beta.json -> vendordeps/ChoreoLib2025.json deleted: vendordeps/NavX.json renamed: vendordeps/PathplannerLib-beta.json -> vendordeps/PathplannerLib.json renamed: vendordeps/Phoenix5-frc2025-beta-latest.json -> vendordeps/Phoenix5-frc2025-latest.json renamed: vendordeps/Phoenix6-frc2025-beta-latest.json -> vendordeps/Phoenix6-frc2025-latest.json modified: vendordeps/REVLib.json renamed: vendordeps/ReduxLib_2025.json -> vendordeps/ReduxLib-2025.0.0.json new file: vendordeps/Studica-2025.0.0.json new file: vendordeps/ThriftyLib.json modified: vendordeps/URCL.json modified: vendordeps/maple-sim.json renamed: vendordeps/yagsl.json -> vendordeps/yagsl-2025.1.3.json --- .wpilib/wpilib_preferences.json | 2 +- build.gradle | 2 +- settings.gradle | 2 +- src/main/deploy/pathplanner/navgrid.json | 447 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 6 +- .../accelerometer/Accelerometer.java | 2 +- .../robot/subsystems/drive/GyroIONavX.java | 7 +- .../robot/subsystems/drive/ModuleIOSpark.java | 7 +- .../drive/ModuleIOSparkCANcoder.java | 7 +- .../flywheel_example/FlywheelIOSim.java | 2 +- .../flywheel_example/FlywheelIOSpark.java | 3 +- .../java/frc/robot/util/OverrideSwitches.java | 2 +- vendordeps/AdvantageKit.json | 10 +- ...reoLib2025Beta.json => ChoreoLib2025.json} | 10 +- vendordeps/NavX.json | 39 -- ...annerLib-beta.json => PathplannerLib.json} | 12 +- ...test.json => Phoenix5-frc2025-latest.json} | 34 +- ...test.json => Phoenix6-frc2025-latest.json} | 58 +-- vendordeps/REVLib.json | 10 +- ...uxLib_2025.json => ReduxLib-2025.0.0.json} | 14 +- vendordeps/Studica-2025.0.0.json | 71 +++ vendordeps/ThriftyLib.json | 20 + vendordeps/URCL.json | 14 +- vendordeps/maple-sim.json | 4 +- .../{yagsl.json => yagsl-2025.1.3.json} | 28 +- 25 files changed, 453 insertions(+), 360 deletions(-) rename vendordeps/{ChoreoLib2025Beta.json => ChoreoLib2025.json} (79%) delete mode 100644 vendordeps/NavX.json rename vendordeps/{PathplannerLib-beta.json => PathplannerLib.json} (82%) rename vendordeps/{Phoenix5-frc2025-beta-latest.json => Phoenix5-frc2025-latest.json} (84%) rename vendordeps/{Phoenix6-frc2025-beta-latest.json => Phoenix6-frc2025-latest.json} (89%) rename vendordeps/{ReduxLib_2025.json => ReduxLib-2025.0.0.json} (87%) create mode 100644 vendordeps/Studica-2025.0.0.json create mode 100644 vendordeps/ThriftyLib.json rename vendordeps/{yagsl.json => yagsl-2025.1.3.json} (70%) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index a1591102..51005bf5 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2025beta", + "projectYear": "2025", "teamNumber": 0 } diff --git a/build.gradle b/build.gradle index 9c88d910..8de0975c 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3" + id "edu.wpi.first.GradleRIO" version "2025.1.1" id "com.peterabeles.gversion" version "1.10.3" id "com.diffplug.spotless" version "6.25.0" id "io.freefair.lombok" version "8.11" diff --git a/settings.gradle b/settings.gradle index d94f73c6..969c7b09 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2024' + String frcYear = '2025' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index 690f5db2..bbd91940 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1,7 +1,7 @@ { "field_size": { - "x": 16.54, - "y": 8.21 + "x": 17.548, + "y": 8.052 }, "nodeSizeMeters": 0.3, "grid": [ @@ -61,6 +61,9 @@ true, true, true, + true, + true, + true, true ], [ @@ -119,6 +122,9 @@ true, true, true, + true, + true, + true, true ], [ @@ -127,7 +133,6 @@ true, true, true, - true, false, false, false, @@ -171,8 +176,12 @@ false, false, false, - true, - true, + false, + false, + false, + false, + false, + false, true, true, true, @@ -184,7 +193,6 @@ true, true, true, - true, false, false, false, @@ -231,7 +239,11 @@ false, false, false, - true, + false, + false, + false, + false, + false, true, true, true, @@ -291,6 +303,9 @@ false, false, false, + false, + false, + false, true, true, true @@ -350,6 +365,9 @@ false, false, false, + false, + false, + false, true, true ], @@ -408,6 +426,9 @@ false, false, false, + false, + false, + false, true, true ], @@ -429,14 +450,6 @@ false, false, false, - true, - true, - true, - false, - false, - false, - false, - false, false, false, false, @@ -447,9 +460,6 @@ false, false, false, - true, - true, - true, false, false, false, @@ -466,12 +476,6 @@ false, false, false, - true, - true - ], - [ - true, - true, false, false, false, @@ -487,8 +491,9 @@ false, false, true, - true, - true, + true + ], + [ true, true, false, @@ -503,15 +508,8 @@ false, false, false, - false, - true, - true, true, true, - true, - false, - false, - false, false, false, false, @@ -524,12 +522,6 @@ false, false, false, - true, - true - ], - [ - true, - true, false, false, false, @@ -547,9 +539,6 @@ true, true, true, - true, - true, - false, false, false, false, @@ -563,15 +552,11 @@ false, false, true, + true + ], + [ true, true, - true, - true, - false, - false, - false, - false, - false, false, false, false, @@ -583,9 +568,9 @@ false, false, true, - true - ], - [ + true, + true, + true, true, true, false, @@ -602,15 +587,6 @@ false, false, false, - true, - true, - true, - true, - true, - false, - false, - false, - false, false, false, false, @@ -636,10 +612,6 @@ false, false, false, - false, - false, - false, - false, true, true ], @@ -654,13 +626,12 @@ false, false, false, + false, + true, + true, true, true, true, - false, - false, - false, - false, true, true, true, @@ -679,16 +650,20 @@ false, false, false, - true, - true, - true, false, false, false, false, + false, + false, + true, true, true, true, + true, + true, + true, + false, false, false, false, @@ -711,6 +686,12 @@ false, false, false, + false, + true, + true, + true, + true, + true, true, true, true, @@ -724,6 +705,9 @@ false, false, false, + true, + true, + true, false, false, false, @@ -732,6 +716,15 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -741,11 +734,10 @@ false, false, false, - false, - false, - true, - true, true, + true + ], + [ true, true, false, @@ -757,9 +749,13 @@ false, false, true, - true - ], - [ + true, + true, + true, + true, + true, + true, + true, true, true, false, @@ -781,6 +777,15 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -790,6 +795,12 @@ false, false, false, + true, + true + ], + [ + true, + true, false, false, false, @@ -798,9 +809,11 @@ false, false, false, - false, - false, - false, + true, + true, + true, + true, + true, true, true, true, @@ -815,9 +828,6 @@ false, true, true, - true - ], - [ true, true, true, @@ -827,6 +837,11 @@ false, false, false, + false, + true, + true, + true, + true, true, true, true, @@ -841,6 +856,12 @@ false, false, false, + true, + true + ], + [ + true, + true, false, false, false, @@ -849,9 +870,16 @@ false, false, false, - false, - false, - false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -870,12 +898,12 @@ false, false, false, + false, + true, + true, true, true, true, - true - ], - [ true, true, true, @@ -886,14 +914,13 @@ false, false, false, - true, - true, - true, - false, false, false, false, true, + true + ], + [ true, true, false, @@ -904,6 +931,17 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, false, false, false, @@ -918,11 +956,6 @@ false, false, false, - true, - true, - true, - false, - false, false, false, false, @@ -931,9 +964,6 @@ true, true, true, - true - ], - [ true, true, true, @@ -948,13 +978,37 @@ false, false, false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, false, false, + false, + false, + true, + true, true, true, true, true, true, + true, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -973,6 +1027,8 @@ true, true, true, + true, + true, false, false, false, @@ -983,18 +1039,10 @@ false, false, false, - false, - true, - true, - true, - true, true, true ], [ - true, - true, - true, true, true, false, @@ -1007,12 +1055,22 @@ false, false, false, - false, true, true, true, true, true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -1043,16 +1101,9 @@ false, false, true, - true, - true, - true, - true, true ], [ - true, - true, - true, true, true, false, @@ -1066,9 +1117,7 @@ false, false, false, - true, - true, - true, + false, true, true, false, @@ -1084,11 +1133,8 @@ false, false, false, - true, - true, - true, - true, - true, + false, + false, false, false, false, @@ -1103,14 +1149,22 @@ true, true, true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, true, true ], [ - true, - true, - true, true, true, false, @@ -1125,9 +1179,6 @@ false, false, false, - true, - true, - true, false, false, false, @@ -1143,9 +1194,6 @@ false, false, false, - true, - true, - true, false, false, false, @@ -1158,17 +1206,26 @@ false, false, false, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, true, true ], [ - true, - true, - true, true, true, false, @@ -1216,16 +1273,20 @@ false, false, false, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, true, true ], [ - true, - true, true, true, false, @@ -1275,9 +1336,14 @@ false, false, false, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, true, true ], @@ -1334,12 +1400,17 @@ false, false, false, - true, + false, + false, + false, + false, true, true, true ], [ + true, + true, true, true, false, @@ -1395,9 +1466,13 @@ false, true, true, + true, true ], [ + true, + true, + true, true, true, false, @@ -1449,64 +1524,6 @@ false, false, false, - false, - false, - false, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, true, true, true, @@ -1569,6 +1586,9 @@ true, true, true, + true, + true, + true, true ], [ @@ -1627,6 +1647,9 @@ true, true, true, + true, + true, + true, true ] ] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 64c3da0d..8274e393 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,7 +23,6 @@ import choreo.auto.AutoChooser; import choreo.auto.AutoFactory; -import choreo.auto.AutoFactory.AutoBindings; import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; import com.pathplanner.lib.auto.AutoBuilder; @@ -173,8 +172,7 @@ public RobotContainer() { // provided Pose2d m_drivebase::followTrajectory, // The drive subsystem trajectory follower true, // If alliance flipping should be enabled - m_drivebase, // The drive subsystem - new AutoBindings() // An empty AutoBindings object + m_drivebase // The drive subsystem ); autoChooserChoreo = new AutoChooser(); autoChooserChoreo.addRoutine("twoPieceAuto", this::twoPieceAuto); @@ -376,7 +374,7 @@ private AutoRoutine twoPieceAuto() { AutoTrajectory scoreTraj = routine.trajectory("scoreGamepiece"); // When the routine begins, reset odometry and start the first trajectory - routine.active().onTrue(Commands.sequence(routine.resetOdometry(pickupTraj), pickupTraj.cmd())); + routine.active().onTrue(Commands.sequence(pickupTraj.resetOdometry(), pickupTraj.cmd())); // Starting at the event marker named "intake", run the intake // pickupTraj.atTime("intake").onTrue(intakeSubsystem.intake()); diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 1a17d901..b63d4ba8 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -16,7 +16,7 @@ import static frc.robot.Constants.AccelerometerConstants.*; import com.ctre.phoenix6.hardware.Pigeon2; -import com.kauailabs.navx.frc.AHRS; +import com.studica.frc.AHRS; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.BuiltInAccelerometer; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 8d5929d7..6de5b723 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -15,17 +15,18 @@ package frc.robot.subsystems.drive; -import com.kauailabs.navx.frc.AHRS; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.SPI; import java.util.Queue; /** IO implementation for Pigeon2 */ public class GyroIONavX implements GyroIO { - private final AHRS navx = new AHRS(SPI.Port.kMXP, (byte) SwerveConstants.kOdometryFrequency); + private final AHRS navx = + new AHRS(NavXComType.kMXP_SPI, (byte) SwerveConstants.kOdometryFrequency); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 68f6cdb1..841ac551 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -20,6 +20,7 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; @@ -259,7 +260,11 @@ public void setDriveVelocity(double velocityRadPerSec) { SwerveConstants.kDriveFrictionVoltage * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; driveController.setReference( - velocityRadPerSec, ControlType.kVelocity, 0, ffVolts, ArbFFUnits.kVoltage); + velocityRadPerSec, + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + ffVolts, + ArbFFUnits.kVoltage); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index 175654ff..ce7744c3 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -22,6 +22,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; @@ -286,7 +287,11 @@ public void setDriveVelocity(double velocityRadPerSec) { SwerveConstants.kDriveFrictionVoltage * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; driveController.setReference( - velocityRadPerSec, ControlType.kVelocity, 0, ffVolts, ArbFFUnits.kVoltage); + velocityRadPerSec, + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + ffVolts, + ArbFFUnits.kVoltage); } @Override diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java index 308a88ce..a4a82cd8 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java @@ -29,7 +29,7 @@ public class FlywheelIOSim implements FlywheelIO { // spins slower than the motors, this number should be greater than one. private static final double kFlywheelGearing = 1.0; - // 1/2 MR² + // 1/2 MR^2 private static final double kFlywheelMomentOfInertia = 0.5 * Units.lbsToKilograms(1.5) * Math.pow(Units.inchesToMeters(4), 2); diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java index 3034bd00..871a9dea 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java @@ -19,6 +19,7 @@ import static frc.robot.util.SparkUtil.*; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; @@ -109,7 +110,7 @@ public void setVelocity(double velocityRadPerSec, double ffVolts) { pid.setReference( Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * kFlywheelGearRatio, ControlType.kVelocity, - 0, + ClosedLoopSlot.kSlot0, ffVolts, ArbFFUnits.kVoltage); } diff --git a/src/main/java/frc/robot/util/OverrideSwitches.java b/src/main/java/frc/robot/util/OverrideSwitches.java index 798b9bb8..9301bab4 100644 --- a/src/main/java/frc/robot/util/OverrideSwitches.java +++ b/src/main/java/frc/robot/util/OverrideSwitches.java @@ -18,7 +18,7 @@ // https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2024-build-thread/442736/72 // // The "Arcade Controller" used by 6328 (https://www.amazon.com/gp/product/B00UUROWWK) -// enumerates as a “Generic USB Controller”, mapped in this modules as a GenericHID. +// enumerates as a "Generic USB Controller", mapped in this modules as a GenericHID. // // If another type of controller is used, swap in the proper class. diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 69538ea8..c7f5262f 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,23 +1,25 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "4.0.0-beta-1", + "version": "4.0.0", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2025", - "mavenUrls": [], + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", "javaDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "4.0.0-beta-1" + "version": "4.0.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "4.0.0-beta-1", + "version": "4.0.0", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/ChoreoLib2025Beta.json b/vendordeps/ChoreoLib2025.json similarity index 79% rename from vendordeps/ChoreoLib2025Beta.json rename to vendordeps/ChoreoLib2025.json index 914a8593..16de3a67 100644 --- a/vendordeps/ChoreoLib2025Beta.json +++ b/vendordeps/ChoreoLib2025.json @@ -1,19 +1,19 @@ { - "fileName": "ChoreoLib2025Beta.json", + "fileName": "ChoreoLib2025.json", "name": "ChoreoLib", - "version": "2025.0.0-beta-9", + "version": "2025.0.0", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", "frcYear": "2025", "mavenUrls": [ "https://lib.choreo.autos/dep", "https://repo1.maven.org/maven2" ], - "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025Beta.json", + "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025.json", "javaDependencies": [ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2025.0.0-beta-9" + "version": "2025.0.0" }, { "groupId": "com.google.code.gson", @@ -26,7 +26,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2025.0.0-beta-9", + "version": "2025.0.0", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json deleted file mode 100644 index 6e535663..00000000 --- a/vendordeps/NavX.json +++ /dev/null @@ -1,39 +0,0 @@ -{ - "fileName": "NavX-2025.1.1-beta-1.json", - "name": "navx_frc", - "version": "2025.1.1-beta-1", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2025", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2025/" - ], - "jsonUrl": "https://dev.studica.com/releases/2025/NavX-2025.1.1-beta-1.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx_frc-java", - "version": "2025.1.1-beta-1" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx_frc-cpp", - "version": "2025.1.1-beta-1", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} diff --git a/vendordeps/PathplannerLib-beta.json b/vendordeps/PathplannerLib.json similarity index 82% rename from vendordeps/PathplannerLib-beta.json rename to vendordeps/PathplannerLib.json index 0185feb4..c7d2e948 100644 --- a/vendordeps/PathplannerLib-beta.json +++ b/vendordeps/PathplannerLib.json @@ -1,18 +1,18 @@ { - "fileName": "PathplannerLib-beta.json", + "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2025.0.0-beta-6.1", + "version": "2025.1.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib-beta.json", + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", "javaDependencies": [ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.0.0-beta-6.1" + "version": "2025.1.1" } ], "jniDependencies": [], @@ -20,15 +20,15 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.0.0-beta-6.1", + "version": "2025.1.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "osxuniversal", "linuxx86-64", + "osxuniversal", "linuxathena", "linuxarm32", "linuxarm64" diff --git a/vendordeps/Phoenix5-frc2025-beta-latest.json b/vendordeps/Phoenix5-frc2025-latest.json similarity index 84% rename from vendordeps/Phoenix5-frc2025-beta-latest.json rename to vendordeps/Phoenix5-frc2025-latest.json index ff142cf6..23e9208a 100644 --- a/vendordeps/Phoenix5-frc2025-beta-latest.json +++ b/vendordeps/Phoenix5-frc2025-latest.json @@ -1,50 +1,50 @@ { - "fileName": "Phoenix5-frc2025-beta-latest.json", + "fileName": "Phoenix5-frc2025-latest.json", "name": "CTRE-Phoenix (v5)", - "version": "5.34.0-beta-4", + "version": "5.35.0", "frcYear": "2025", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-beta-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json", "requires": [ { "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6-frc2025-beta-latest.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json" + "offlineFileName": "Phoenix6-frc2025-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" } ], "conflictsWith": [ { "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", - "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" }, { "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", - "offlineFileName": "Phoenix5-replay-frc2025-beta-latest.json" + "offlineFileName": "Phoenix5-replay-frc2025-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.34.0-beta-4" + "version": "5.35.0" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.34.0-beta-4" + "version": "5.35.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.34.0-beta-4", + "version": "5.35.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.34.0-beta-4", + "version": "5.35.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -74,7 +74,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.34.0-beta-4", + "version": "5.35.0", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.34.0-beta-4", + "version": "5.35.0", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -106,7 +106,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.34.0-beta-4", + "version": "5.35.0", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -122,7 +122,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.34.0-beta-4", + "version": "5.35.0", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.34.0-beta-4", + "version": "5.35.0", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -154,7 +154,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.34.0-beta-4", + "version": "5.35.0", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/Phoenix6-frc2025-beta-latest.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 89% rename from vendordeps/Phoenix6-frc2025-beta-latest.json rename to vendordeps/Phoenix6-frc2025-latest.json index 140c7704..b1f42e3e 100644 --- a/vendordeps/Phoenix6-frc2025-beta-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,32 +1,32 @@ { - "fileName": "Phoenix6-frc2025-beta-latest.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.0.0-beta-4", + "version": "25.1.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", "conflictsWith": [ { "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.0.0-beta-4" + "version": "25.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -196,7 +196,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +212,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +228,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +244,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +260,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +276,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +292,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +324,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -340,7 +340,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +356,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +372,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 6b3a1574..6e719675 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.0-beta-4", + "version": "2025.0.0", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.0-beta-4" + "version": "2025.0.0" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0-beta-4", + "version": "2025.0.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.0-beta-4", + "version": "2025.0.0", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0-beta-4", + "version": "2025.0.0", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/ReduxLib_2025.json b/vendordeps/ReduxLib-2025.0.0.json similarity index 87% rename from vendordeps/ReduxLib_2025.json rename to vendordeps/ReduxLib-2025.0.0.json index 5305e85a..18ff5a11 100644 --- a/vendordeps/ReduxLib_2025.json +++ b/vendordeps/ReduxLib-2025.0.0.json @@ -1,8 +1,8 @@ { - "fileName": "ReduxLib_2025.json", + "fileName": "ReduxLib-2025.0.0.json", "name": "ReduxLib", - "version": "2025.0.0-beta2", - "frcYear": 2025, + "version": "2025.0.0", + "frcYear": "2025", "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", "mavenUrls": [ "https://maven.reduxrobotics.com/" @@ -12,14 +12,14 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-java", - "version": "2025.0.0-beta2" + "version": "2025.0.0" } ], "jniDependencies": [ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-driver", - "version": "2025.0.0-beta2", + "version": "2025.0.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-cpp", - "version": "2025.0.0-beta2", + "version": "2025.0.0", "libName": "ReduxLib", "headerClassifier": "headers", "sourcesClassifier": "sources", @@ -54,7 +54,7 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-driver", - "version": "2025.0.0-beta2", + "version": "2025.0.0", "libName": "ReduxCore", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/Studica-2025.0.0.json b/vendordeps/Studica-2025.0.0.json new file mode 100644 index 00000000..8cba5305 --- /dev/null +++ b/vendordeps/Studica-2025.0.0.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.0.0.json", + "name": "Studica", + "version": "2025.0.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json", + "cppDependencies": [ + { + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "Studica", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.0" + }, + { + "artifactId": "Studica-driver", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.0" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.0" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.0" + } + ] +} diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json new file mode 100644 index 00000000..23948625 --- /dev/null +++ b/vendordeps/ThriftyLib.json @@ -0,0 +1,20 @@ +{ + "fileName": "ThriftyLib.json", + "name": "ThriftyLib", + "version": "2025.0.1", + "frcYear": "2025", + "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", + "mavenUrls": [ + "https://docs.home.thethriftybot.com" + ], + "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json", + "javaDependencies": [ + { + "groupId": "com.thethriftybot.frc", + "artifactId": "ThriftyLib-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json index 1099ee6c..c991b6af 100644 --- a/vendordeps/URCL.json +++ b/vendordeps/URCL.json @@ -1,25 +1,25 @@ { "fileName": "URCL.json", "name": "URCL", - "version": "2025.0.0-beta-1", + "version": "2025.0.0", "frcYear": "2025", "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", "mavenUrls": [ - "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2025.0.0-beta-1" + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" ], - "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", + "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/main/URCL.json", "javaDependencies": [ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-java", - "version": "2025.0.0-beta-1" + "version": "2025.0.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-driver", - "version": "2025.0.0-beta-1", + "version": "2025.0.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-cpp", - "version": "2025.0.0-beta-1", + "version": "2025.0.0", "libName": "URCL", "headerClassifier": "headers", "sharedLibrary": false, @@ -49,7 +49,7 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-driver", - "version": "2025.0.0-beta-1", + "version": "2025.0.0", "libName": "URCLDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index 4b4a9c57..b0669799 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.2.7", + "version": "0.2.8", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.2.7" + "version": "0.2.8" }, { "groupId": "org.dyn4j", diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl-2025.1.3.json similarity index 70% rename from vendordeps/yagsl.json rename to vendordeps/yagsl-2025.1.3.json index cb05dc8d..b3be624b 100644 --- a/vendordeps/yagsl.json +++ b/vendordeps/yagsl-2025.1.3.json @@ -1,7 +1,7 @@ { - "fileName": "yagsl.json", + "fileName": "yagsl-2025.1.3.json", "name": "YAGSL", - "version": "2025.1.0.4", + "version": "2025.1.3", "frcYear": "2025", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -12,39 +12,45 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2025.1.0.4" + "version": "2025.1.3" } ], "requires": [ { "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "errorMessage": "REVLib is required!", - "offlineFileName": "REVLib.json", + "offlineFileName": "REVLib-2025.0.0.json", "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json" }, { "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", "errorMessage": "ReduxLib is required!", - "offlineFileName": "ReduxLib_2025.json", + "offlineFileName": "ReduxLib-2025.0.0.json", "onlineUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json" }, { "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "errorMessage": "Phoenix6 is required!", - "offlineFileName": "Phoenix6-frc2025-beta-latest.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json" + "offlineFileName": "Phoenix6-25.1.0.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" }, { "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "errorMessage": "Phoenix5 is required!", - "offlineFileName": "Phoenix5.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-beta-latest.json" + "offlineFileName": "Phoenix5-5.35.0.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json" }, { "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "errorMessage": "NavX is required!", - "offlineFileName": "Studica-2025.1.1-beta-3.json", - "onlineUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-3.json" + "offlineFileName": "Studica-2025.0.0.json", + "onlineUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json" + }, + { + "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", + "errorMessage": "ThriftyLib is required!", + "offlineFileName": "ThriftyLib.json", + "onlineUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json" }, { "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", From 2a95b2b124d46327e3b79301908415efa50c746f Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 6 Jan 2025 17:18:47 -0700 Subject: [PATCH 25/30] Conformal changes for Phoenix6 kickoff version modified: src/main/java/frc/robot/generated/TunerConstants.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java modified: src/main/java/frc/robot/subsystems/drive/SwerveConstants.java --- .../frc/robot/generated/TunerConstants.java | 284 ++++++++++++------ .../subsystems/drive/ModuleIOBlended.java | 40 +-- .../robot/subsystems/drive/ModuleIOSim.java | 6 +- .../subsystems/drive/ModuleIOTalonFX.java | 14 +- .../subsystems/drive/SwerveConstants.java | 24 +- 5 files changed, 239 insertions(+), 129 deletions(-) diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 72a2488c..5ab0a8a7 100755 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -3,17 +3,14 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; -import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; // import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -46,6 +43,13 @@ public class TunerConstants { // This affects the PID/FF gains for the drive motors private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = + SteerMotorArrangement.TalonFX_Integrated; + // The remote sensor feedback type to use for the steer motors; // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; @@ -54,7 +58,7 @@ public class TunerConstants { // This needs to be tuned to your individual robot private static final Current kSlipCurrent = Amps.of(120.0); - // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); private static final TalonFXConfiguration steerInitialConfigs = @@ -66,7 +70,7 @@ public class TunerConstants { // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(Amps.of(60)) .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration cancoderInitialConfigs = new CANcoderConfiguration(); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; @@ -76,13 +80,13 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.41); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.125; + private static final double kCoupleRatio = 3.5714285714285716; - private static final double kDriveGearRatio = 5.902777777777778; + private static final double kDriveGearRatio = 6.746031746031747; private static final double kSteerGearRatio = 21.428571428571427; private static final Distance kWheelRadius = Inches.of(2); @@ -104,34 +108,39 @@ public class TunerConstants { .withPigeon2Id(kPigeonId) .withPigeon2Configs(pigeonConfigs); - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withCANcoderInitialConfigs(cancoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + private static final SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + ConstantCreator = + new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); // Front Left 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.47265625); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.472412109375); private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftCANcoderInverted = false; + private static final boolean kFrontLeftEncoderInverted = false; private static final Distance kFrontLeftXPos = Inches.of(10.375); private static final Distance kFrontLeftYPos = Inches.of(10.375); @@ -140,9 +149,9 @@ 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.11474609375); + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.11376953125); private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightCANcoderInverted = false; + private static final boolean kFrontRightEncoderInverted = false; private static final Distance kFrontRightXPos = Inches.of(10.375); private static final Distance kFrontRightYPos = Inches.of(-10.375); @@ -151,9 +160,9 @@ 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.411376953125); + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.409423828125); private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftCANcoderInverted = false; + private static final boolean kBackLeftEncoderInverted = false; private static final Distance kBackLeftXPos = Inches.of(-10.375); private static final Distance kBackLeftYPos = Inches.of(10.375); @@ -164,63 +173,144 @@ public class TunerConstants { private static final int kBackRightEncoderId = 12; private static final Angle kBackRightEncoderOffset = Rotations.of(0.086669921875); private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightCANcoderInverted = false; + private static final boolean kBackRightEncoderInverted = false; private static final Distance kBackRightXPos = Inches.of(-10.375); private static final Distance kBackRightYPos = Inches.of(-10.375); - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftCANcoderInverted); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightCANcoderInverted); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftCANcoderInverted); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightCANcoderInverted); - - // /** - // * Creates a CommandSwerveDrivetrain instance. - // * This should only be called once in your robot program,. - // */ - // public static CommandSwerveDrivetrain createDrivetrain() { - // return new CommandSwerveDrivetrain( - // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - // ); - // } + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + + // /** + // * Creates a CommandSwerveDrivetrain instance. + // * This should only be called once in your robot program,. + // */ + // public static CommandSwerveDrivetrain createDrivetrain() { + // return new CommandSwerveDrivetrain( + // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + // ); + // } + + /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { + super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + modules); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation in the form + * [x, y, theta]ᵀ, with units in meters and radians + * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, + * y, theta]ᵀ, with units in meters and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); + } + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index d4d7eb73..ab479358 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -20,6 +20,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -67,7 +68,9 @@ * "/Drive/ModuleX/TurnAbsolutePositionRad" */ public class ModuleIOBlended implements ModuleIO { - private final SwerveModuleConstants constants; + private final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + constants; // CAN Devices private final TalonFX driveTalon; @@ -165,7 +168,7 @@ public ModuleIOBlended(int module) { driveTalon = new TalonFX(constants.DriveMotorId, SwerveConstants.kCANbusName); turnSpark = new SparkMax(constants.SteerMotorId, MotorType.kBrushless); - cancoder = new CANcoder(constants.CANcoderId, SwerveConstants.kCANbusName); + cancoder = new CANcoder(constants.EncoderId, SwerveConstants.kCANbusName); turnController = turnSpark.getClosedLoopController(); @@ -194,7 +197,7 @@ public ModuleIOBlended(int module) { .voltageCompensation(12.0); turnConfig .absoluteEncoder - .inverted(constants.CANcoderInverted) + .inverted(constants.EncoderInverted) .positionConversionFactor(turnEncoderPositionFactor) .velocityConversionFactor(turnEncoderVelocityFactor) .averageDepth(2); @@ -225,10 +228,10 @@ public ModuleIOBlended(int module) { turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); // Configure CANCoder - CANcoderConfiguration cancoderConfig = constants.CANcoderInitialConfigs; - cancoderConfig.MagnetSensor.MagnetOffset = constants.CANcoderOffset; + CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs; + cancoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset; cancoderConfig.MagnetSensor.SensorDirection = - constants.CANcoderInverted + constants.EncoderInverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; cancoder.getConfigurator().apply(cancoderConfig); @@ -331,20 +334,23 @@ public void setDriveVelocity(double velocityRadPerSec) { public void setTurnPosition(Rotation2d rotation) { double setpoint = MathUtil.inputModulus( - rotation.plus(Rotation2d.fromRotations(constants.CANcoderOffset)).getRadians(), + rotation.plus(Rotation2d.fromRotations(constants.EncoderOffset)).getRadians(), turnPIDMinInput, turnPIDMaxInput); turnController.setReference(setpoint, ControlType.kPosition); } - private SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadiusMeters) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + private SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + ConstantCreator = + new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadiusMeters) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 671a4ca5..52950e24 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -15,6 +15,8 @@ package frc.robot.subsystems.drive; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; @@ -53,7 +55,9 @@ public class ModuleIOSim implements ModuleIO { private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; - public ModuleIOSim(SwerveModuleConstants constants) { + public ModuleIOSim( + SwerveModuleConstants + constants) { // Create drive and turn sim models driveSim = new DCMotorSim( diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 4f5ed18e..21e97cd8 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -54,7 +54,9 @@ *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. */ public class ModuleIOTalonFX implements ModuleIO { - private final SwerveModuleConstants constants; + private final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + constants; // Hardware objects private final TalonFX driveTalon; @@ -111,7 +113,7 @@ public ModuleIOTalonFX(int module) { driveTalon = new TalonFX(constants.DriveMotorId, SwerveConstants.kCANbusName); turnTalon = new TalonFX(constants.SteerMotorId, SwerveConstants.kCANbusName); - cancoder = new CANcoder(constants.CANcoderId, SwerveConstants.kCANbusName); + cancoder = new CANcoder(constants.EncoderId, SwerveConstants.kCANbusName); // Configure drive motor var driveConfig = constants.DriveMotorInitialConfigs; @@ -133,7 +135,7 @@ public ModuleIOTalonFX(int module) { var turnConfig = new TalonFXConfiguration(); turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; turnConfig.Slot0 = constants.SteerMotorGains; - turnConfig.Feedback.FeedbackRemoteSensorID = constants.CANcoderId; + turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; turnConfig.Feedback.RotorToSensorRatio = SwerveConstants.kSteerGearRatio; turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / SwerveConstants.kSteerGearRatio; turnConfig.MotionMagic.MotionMagicAcceleration = @@ -148,10 +150,10 @@ public ModuleIOTalonFX(int module) { tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); // Configure CANCoder - CANcoderConfiguration cancoderConfig = constants.CANcoderInitialConfigs; - cancoderConfig.MagnetSensor.MagnetOffset = constants.CANcoderOffset; + CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs; + cancoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset; cancoderConfig.MagnetSensor.SensorDirection = - constants.CANcoderInverted + constants.EncoderInverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; cancoder.getConfigurator().apply(cancoderConfig); diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index 870aedc0..5cefd9a1 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -123,9 +123,10 @@ public class SwerveConstants { kDriveCurrentLimit = 120.0; // Example from CTRE documentation kDriveSlipCurrent = TunerConstants.FrontLeft.SlipCurrent; kOptimalVoltage = 12.0; // Assumed Ideal + // Front Left kFLDriveMotorId = TunerConstants.FrontLeft.DriveMotorId; kFLSteerMotorId = TunerConstants.FrontLeft.SteerMotorId; - kFLEncoderId = TunerConstants.FrontLeft.CANcoderId; + kFLEncoderId = TunerConstants.FrontLeft.EncoderId; kFLDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; kFLSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; kFLEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; @@ -133,30 +134,32 @@ public class SwerveConstants { kFLSteerType = "kraken"; kFLEncoderType = "cancoder"; kFLEncoderOffset = - -Units.rotationsToRadians(TunerConstants.FrontLeft.CANcoderOffset) + Math.PI; + -Units.rotationsToRadians(TunerConstants.FrontLeft.EncoderOffset) + Math.PI; kFLDriveInvert = TunerConstants.FrontLeft.DriveMotorInverted; kFLSteerInvert = TunerConstants.FrontLeft.SteerMotorInverted; kFLEncoderInvert = false; kFLXPosMeters = TunerConstants.FrontLeft.LocationX; kFLYPosMeters = TunerConstants.FrontLeft.LocationY; + // Front Right kFRDriveMotorId = TunerConstants.FrontRight.DriveMotorId; kFRSteerMotorId = TunerConstants.FrontRight.SteerMotorId; - kFREncoderId = TunerConstants.FrontRight.CANcoderId; + kFREncoderId = TunerConstants.FrontRight.EncoderId; kFRDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; kFRSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; kFREncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; kFRDriveType = "kraken"; kFRSteerType = "kraken"; kFREncoderType = "cancoder"; - kFREncoderOffset = -Units.rotationsToRadians(TunerConstants.FrontRight.CANcoderOffset); + kFREncoderOffset = -Units.rotationsToRadians(TunerConstants.FrontRight.EncoderOffset); kFRDriveInvert = TunerConstants.FrontRight.DriveMotorInverted; kFRSteerInvert = TunerConstants.FrontRight.SteerMotorInverted; kFREncoderInvert = false; kFRXPosMeters = TunerConstants.FrontRight.LocationX; kFRYPosMeters = TunerConstants.FrontRight.LocationY; + // Back Left kBLDriveMotorId = TunerConstants.BackLeft.DriveMotorId; kBLSteerMotorId = TunerConstants.BackLeft.SteerMotorId; - kBLEncoderId = TunerConstants.BackLeft.CANcoderId; + kBLEncoderId = TunerConstants.BackLeft.EncoderId; kBLDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; kBLSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; kBLEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; @@ -164,22 +167,23 @@ public class SwerveConstants { kBLSteerType = "kraken"; kBLEncoderType = "cancoder"; kBLEncoderOffset = - -Units.rotationsToRadians(TunerConstants.BackLeft.CANcoderOffset) + Math.PI; + -Units.rotationsToRadians(TunerConstants.BackLeft.EncoderOffset) + Math.PI; kBLDriveInvert = TunerConstants.BackLeft.DriveMotorInverted; kBLSteerInvert = TunerConstants.BackLeft.SteerMotorInverted; kBLEncoderInvert = false; kBLXPosMeters = TunerConstants.BackLeft.LocationX; kBLYPosMeters = TunerConstants.BackLeft.LocationY; + // Back Right kBRDriveMotorId = TunerConstants.BackRight.DriveMotorId; kBRSteerMotorId = TunerConstants.BackRight.SteerMotorId; - kBREncoderId = TunerConstants.BackRight.CANcoderId; + kBREncoderId = TunerConstants.BackRight.EncoderId; kBRDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; kBRSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; kBREncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; kBRDriveType = "kraken"; kBRSteerType = "kraken"; kBREncoderType = "cancoder"; - kBREncoderOffset = -Units.rotationsToRadians(TunerConstants.BackRight.CANcoderOffset); + kBREncoderOffset = -Units.rotationsToRadians(TunerConstants.BackRight.EncoderOffset); kBRDriveInvert = TunerConstants.BackRight.DriveMotorInverted; kBRSteerInvert = TunerConstants.BackRight.SteerMotorInverted; kBREncoderInvert = false; @@ -203,6 +207,7 @@ public class SwerveConstants { kDriveCurrentLimit = YagslConstants.kDriveCurrentLimit; kDriveSlipCurrent = 120.0; kOptimalVoltage = YagslConstants.kOptimalVoltage; + // Front Left kFLDriveMotorId = YagslConstants.kFrontLeftDriveMotorId; kFLSteerMotorId = YagslConstants.kFrontLeftSteerMotorId; kFLEncoderId = YagslConstants.kFrontLeftEncoderId; @@ -218,6 +223,7 @@ public class SwerveConstants { kFLEncoderInvert = YagslConstants.kFrontLeftEncoderInvert; kFLXPosMeters = Units.inchesToMeters(YagslConstants.kFrontLeftXPosInches); kFLYPosMeters = Units.inchesToMeters(YagslConstants.kFrontLeftYPosInches); + // Front Right kFRDriveMotorId = YagslConstants.kFrontRightDriveMotorId; kFRSteerMotorId = YagslConstants.kFrontRightSteerMotorId; kFREncoderId = YagslConstants.kFrontRightEncoderId; @@ -233,6 +239,7 @@ public class SwerveConstants { kFREncoderInvert = YagslConstants.kFrontRightEncoderInvert; kFRXPosMeters = Units.inchesToMeters(YagslConstants.kFrontRightXPosInches); kFRYPosMeters = Units.inchesToMeters(YagslConstants.kFrontRightYPosInches); + // Back Left kBLDriveMotorId = YagslConstants.kBackLeftDriveMotorId; kBLSteerMotorId = YagslConstants.kBackLeftSteerMotorId; kBLEncoderId = YagslConstants.kBackLeftEncoderId; @@ -248,6 +255,7 @@ public class SwerveConstants { kBLEncoderInvert = YagslConstants.kBackLeftEncoderInvert; kBLXPosMeters = Units.inchesToMeters(YagslConstants.kBackLeftXPosInches); kBLYPosMeters = Units.inchesToMeters(YagslConstants.kBackLeftYPosInches); + // Back Right kBRDriveMotorId = YagslConstants.kBackRightDriveMotorId; kBRSteerMotorId = YagslConstants.kBackRightSteerMotorId; kBREncoderId = YagslConstants.kBackRightEncoderId; From c637e492dec32e3e6e1a50632d33847c9610295d Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 10 Jan 2025 11:15:18 -0700 Subject: [PATCH 26/30] More 2025 vendordep updates modified: vendordeps/ChoreoLib2025.json modified: vendordeps/PathplannerLib.json modified: vendordeps/Phoenix5-frc2025-latest.json modified: vendordeps/maple-sim.json --- vendordeps/ChoreoLib2025.json | 6 +++--- vendordeps/PathplannerLib.json | 6 +++--- vendordeps/Phoenix5-frc2025-latest.json | 22 +++++++++++----------- vendordeps/maple-sim.json | 4 ++-- 4 files changed, 19 insertions(+), 19 deletions(-) diff --git a/vendordeps/ChoreoLib2025.json b/vendordeps/ChoreoLib2025.json index 16de3a67..faef0111 100644 --- a/vendordeps/ChoreoLib2025.json +++ b/vendordeps/ChoreoLib2025.json @@ -1,7 +1,7 @@ { "fileName": "ChoreoLib2025.json", "name": "ChoreoLib", - "version": "2025.0.0", + "version": "2025.0.1", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2025.0.0" + "version": "2025.0.1" }, { "groupId": "com.google.code.gson", @@ -26,7 +26,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index c7d2e948..1371a406 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2025.1.1", + "version": "2025.2.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.1.1" + "version": "2025.2.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.1.1", + "version": "2025.2.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix5-frc2025-latest.json b/vendordeps/Phoenix5-frc2025-latest.json index 23e9208a..aa08d95b 100644 --- a/vendordeps/Phoenix5-frc2025-latest.json +++ b/vendordeps/Phoenix5-frc2025-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix5-frc2025-latest.json", "name": "CTRE-Phoenix (v5)", - "version": "5.35.0", + "version": "5.35.1", "frcYear": "2025", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ @@ -32,19 +32,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.35.0" + "version": "5.35.1" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.35.0" + "version": "5.35.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.35.0", + "version": "5.35.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.35.0", + "version": "5.35.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -74,7 +74,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.35.0", + "version": "5.35.1", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.35.0", + "version": "5.35.1", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -106,7 +106,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.35.0", + "version": "5.35.1", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -122,7 +122,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.35.0", + "version": "5.35.1", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.35.0", + "version": "5.35.1", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -154,7 +154,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.35.0", + "version": "5.35.1", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index b0669799..39a6c80b 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.2.8", + "version": "0.3.1", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.2.8" + "version": "0.3.1" }, { "groupId": "org.dyn4j", From 18e5cc69194d1cbb5829ffb7bccd081437ac8dc6 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 10 Jan 2025 17:26:56 -0700 Subject: [PATCH 27/30] Apply TalonFX settings correctly Correctly apply the TalonFX settings to the motor controllers after all settings been, well, set. modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/commands/DriveCommands.java modified: src/main/java/frc/robot/generated/TunerConstants.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/ModuleIOTalonFX.java modified: src/main/java/frc/robot/subsystems/drive/SwerveConstants.java --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/DriveCommands.java | 7 +++---- src/main/java/frc/robot/generated/TunerConstants.java | 8 ++++---- src/main/java/frc/robot/subsystems/drive/Drive.java | 9 ++++++--- .../frc/robot/subsystems/drive/ModuleIOBlended.java | 8 +++++--- .../frc/robot/subsystems/drive/ModuleIOTalonFX.java | 10 ++++++---- .../frc/robot/subsystems/drive/SwerveConstants.java | 8 ++++---- 8 files changed, 31 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 35d6aec0..1ed48e94 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -69,7 +69,7 @@ public final class Constants { // under strict caveat emptor -- and submit any error and bugfixes // via GitHub issues. private static SwerveType swerveType = SwerveType.PHOENIX6; // PHOENIX6, YAGSL - private static CTREPro phoenixPro = CTREPro.UNLICENSED; // LICENSED, UNLICENSED + private static CTREPro phoenixPro = CTREPro.LICENSED; // LICENSED, UNLICENSED private static AutoType autoType = AutoType.PATHPLANNER; // PATHPLANNER, CHOREO private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE @@ -137,7 +137,7 @@ public static final class DrivebaseConstants { // Drive and Turn PID constants public static final PIDConstants drivePID = new PIDConstants(0.05, 0.0, 0.0); - public static final PIDConstants steerPID = new PIDConstants(5.0, 0.0, 0.4); + public static final PIDConstants steerPID = new PIDConstants(2.0, 0.0, 0.4); // Hold time on motor brakes when disabled public static final double kWheelLockTime = 10; // seconds diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8274e393..a2bedaaa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -228,7 +228,7 @@ private void configureBindings() { m_drivebase, () -> -driveStickY.value(), () -> -driveStickX.value(), - () -> turnStickX.value())); + () -> -turnStickX.value())); // ** Example Commands -- Remap, remove, or change as desired ** // Press B button while driving --> ROBOT-CENTRIC diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 9e284802..ac130384 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -179,7 +179,7 @@ public static Command fieldRelativeDriveAtAngle( private static Translation2d getLinearVelocity(double x, double y) { // Apply deadband double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), OperatorConstants.kDeadband); - Rotation2d linearDirection = new Rotation2d(x, y); + Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); // Square magnitude for more precise control // NOTE: The x & y values range from -1 to +1, so their squares are as well @@ -187,8 +187,7 @@ private static Translation2d getLinearVelocity(double x, double y) { // Return new linear velocity return new Pose2d(new Translation2d(), linearDirection) - .transformBy( - new Transform2d(linearVelocityFilter.calculate(linearMagnitude), 0.0, new Rotation2d())) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) .getTranslation(); } @@ -198,7 +197,7 @@ private static Translation2d getLinearVelocity(double x, double y) { */ private static double getOmega(double omega) { omega = MathUtil.applyDeadband(omega, OperatorConstants.kDeadband); - return omegaFilter.calculate(Math.copySign(omega * omega, omega)); + return Math.copySign(omega * omega, omega); } /***************************************************************************/ diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 5ab0a8a7..4c4239af 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.472412109375); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.087158203125); 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.11376953125); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.410888671875); 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.409423828125); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.1142578125); 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.086669921875); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.471923828125); 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 052e2b3b..7d468d9a 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -129,9 +129,12 @@ public Drive() { for (int i = 0; i < 4; i++) { switch (modType) { case 0b00000000: // ALL-CTRE - // TODO: ADD CASE FOR USING NAVX!!! - throw new RuntimeException( - "For an all-CTRE drive base, use Phoenix Tuner X Swerve Generator instead of YAGSL!"); + if (kImuType == "navx" || kImuType == "navx_spi") { + modules[i] = new Module(new ModuleIOTalonFX(i), i); + } else { + throw new RuntimeException( + "For an all-CTRE drive base, use Phoenix Tuner X Swerve Generator instead of YAGSL!"); + } case 0b00010000: // Blended Talon Drive / NEO Steer modules[i] = new Module(new ModuleIOBlended(i), i); break; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index ab479358..d7f6e9da 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -185,8 +185,6 @@ public ModuleIOBlended(int module) { constants.DriveMotorInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - PhoenixUtil.tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); - PhoenixUtil.tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); // Configure turn motor var turnConfig = new SparkMaxConfig(); @@ -234,7 +232,6 @@ public ModuleIOBlended(int module) { constants.EncoderInverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; - cancoder.getConfigurator().apply(cancoderConfig); // Set motor Closed Loop Output type based on Phoenix Pro status constants.DriveMotorClosedLoopOutput = @@ -243,6 +240,11 @@ public ModuleIOBlended(int module) { case UNLICENSED -> ClosedLoopOutputType.Voltage; }; + // Finally, apply the configs to the motor controllers + PhoenixUtil.tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + PhoenixUtil.tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + cancoder.getConfigurator().apply(cancoderConfig); + // Create timestamp queue timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 21e97cd8..20d5db3e 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -128,8 +128,6 @@ public ModuleIOTalonFX(int module) { constants.DriveMotorInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); - tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); // Configure turn motor var turnConfig = new TalonFXConfiguration(); @@ -147,7 +145,6 @@ public ModuleIOTalonFX(int module) { constants.SteerMotorInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); // Configure CANCoder CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs; @@ -156,7 +153,6 @@ public ModuleIOTalonFX(int module) { constants.EncoderInverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; - cancoder.getConfigurator().apply(cancoderConfig); // Set motor Closed Loop Output type and CANCoder feedback type based on Phoenix Pro status switch (Constants.getPhoenixPro()) { @@ -170,6 +166,12 @@ public ModuleIOTalonFX(int module) { constants.SteerMotorClosedLoopOutput = ClosedLoopOutputType.Voltage; } + // Finally, apply the configs to the motor controllers + tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); + cancoder.getConfigurator().apply(cancoderConfig); + // Create timestamp queue timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index 5cefd9a1..d5119272 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -137,7 +137,7 @@ public class SwerveConstants { -Units.rotationsToRadians(TunerConstants.FrontLeft.EncoderOffset) + Math.PI; kFLDriveInvert = TunerConstants.FrontLeft.DriveMotorInverted; kFLSteerInvert = TunerConstants.FrontLeft.SteerMotorInverted; - kFLEncoderInvert = false; + kFLEncoderInvert = TunerConstants.FrontLeft.EncoderInverted; kFLXPosMeters = TunerConstants.FrontLeft.LocationX; kFLYPosMeters = TunerConstants.FrontLeft.LocationY; // Front Right @@ -153,7 +153,7 @@ public class SwerveConstants { kFREncoderOffset = -Units.rotationsToRadians(TunerConstants.FrontRight.EncoderOffset); kFRDriveInvert = TunerConstants.FrontRight.DriveMotorInverted; kFRSteerInvert = TunerConstants.FrontRight.SteerMotorInverted; - kFREncoderInvert = false; + kFREncoderInvert = TunerConstants.FrontRight.EncoderInverted; kFRXPosMeters = TunerConstants.FrontRight.LocationX; kFRYPosMeters = TunerConstants.FrontRight.LocationY; // Back Left @@ -170,7 +170,7 @@ public class SwerveConstants { -Units.rotationsToRadians(TunerConstants.BackLeft.EncoderOffset) + Math.PI; kBLDriveInvert = TunerConstants.BackLeft.DriveMotorInverted; kBLSteerInvert = TunerConstants.BackLeft.SteerMotorInverted; - kBLEncoderInvert = false; + kBLEncoderInvert = TunerConstants.BackLeft.EncoderInverted; kBLXPosMeters = TunerConstants.BackLeft.LocationX; kBLYPosMeters = TunerConstants.BackLeft.LocationY; // Back Right @@ -186,7 +186,7 @@ public class SwerveConstants { kBREncoderOffset = -Units.rotationsToRadians(TunerConstants.BackRight.EncoderOffset); kBRDriveInvert = TunerConstants.BackRight.DriveMotorInverted; kBRSteerInvert = TunerConstants.BackRight.SteerMotorInverted; - kBREncoderInvert = false; + kBREncoderInvert = TunerConstants.BackRight.EncoderInverted; kBRXPosMeters = TunerConstants.BackRight.LocationX; kBRYPosMeters = TunerConstants.BackRight.LocationY; break; From 133ba5e74adb99c45523515bf1704ded0a26d9b4 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 10 Jan 2025 18:18:09 -0700 Subject: [PATCH 28/30] Cleanup of unused things; set CTRE UNLICENSED modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/commands/DriveCommands.java --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/commands/DriveCommands.java | 6 ------ 3 files changed, 1 insertion(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1ed48e94..6d6eeac6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -69,7 +69,7 @@ public final class Constants { // under strict caveat emptor -- and submit any error and bugfixes // via GitHub issues. private static SwerveType swerveType = SwerveType.PHOENIX6; // PHOENIX6, YAGSL - private static CTREPro phoenixPro = CTREPro.LICENSED; // LICENSED, UNLICENSED + private static CTREPro phoenixPro = CTREPro.UNLICENSED; // LICENSED, UNLICENSED private static AutoType autoType = AutoType.PATHPLANNER; // PATHPLANNER, CHOREO private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a2bedaaa..7cd9608a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -222,7 +222,6 @@ private void configureBindings() { } // SET STANDARD DRIVING AS DEFAULT COMMAND FOR THE DRIVEBASE - // TODO: With a re-do of Phoenix Tuner X on George, ensure the signs are all correct!!!!! m_drivebase.setDefaultCommand( DriveCommands.fieldRelativeDrive( m_drivebase, diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index ac130384..7909b911 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -50,12 +50,6 @@ public class DriveCommands { private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 - // Create slew rate limiters for smoothing erratic joystick motions - private static final SlewRateLimiter linearVelocityFilter = - new SlewRateLimiter(OperatorConstants.kJoystickSlewLimit); - private static final SlewRateLimiter omegaFilter = - new SlewRateLimiter(OperatorConstants.kJoystickSlewLimit); - private DriveCommands() {} /** From e0992875a273483d4da0b5c1cff4121e23b83534 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 10 Jan 2025 18:20:28 -0700 Subject: [PATCH 29/30] YAGSL vendorlib update renamed: vendordeps/yagsl-2025.1.3.json -> vendordeps/yagsl-2025.1.3.jwt151.json --- .../{yagsl-2025.1.3.json => yagsl-2025.1.3.jwt151.json} | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) rename vendordeps/{yagsl-2025.1.3.json => yagsl-2025.1.3.jwt151.json} (95%) diff --git a/vendordeps/yagsl-2025.1.3.json b/vendordeps/yagsl-2025.1.3.jwt151.json similarity index 95% rename from vendordeps/yagsl-2025.1.3.json rename to vendordeps/yagsl-2025.1.3.jwt151.json index b3be624b..97dc57eb 100644 --- a/vendordeps/yagsl-2025.1.3.json +++ b/vendordeps/yagsl-2025.1.3.jwt151.json @@ -1,7 +1,7 @@ { - "fileName": "yagsl-2025.1.3.json", + "fileName": "yagsl-2025.1.3.jwt151.json", "name": "YAGSL", - "version": "2025.1.3", + "version": "2025.1.3.jwt151", "frcYear": "2025", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2025.1.3" + "version": "2025.1.3.jwt151" } ], "requires": [ From 80d22013c5e3d2ca9438e4eb34795ba862e87282 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 11 Jan 2025 08:15:04 -0700 Subject: [PATCH 30/30] Update to WPILib 2025.2.1 The WPILib Week 1 update, including all of the 2025 field images and AprilTags. modified: build.gradle --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 8de0975c..f1e53c3d 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.2.1" id "com.peterabeles.gversion" version "1.10.3" id "com.diffplug.spotless" version "6.25.0" id "io.freefair.lombok" version "8.11"