From 309a34c0581b912457bc7a63ce19bfed52f8125d Mon Sep 17 00:00:00 2001 From: Luis Leyva Date: Sun, 4 Oct 2026 18:57:34 -0600 Subject: [PATCH 1/3] Remove pathplanner so we can use either pathplanner or bline --- README.md | 24 +- build.gradle | 14 +- documentation/README.md | 17 + documentation/swerve/README.md | 159 ++++++ documentation/swerve/examples/BLinePaths.java | 74 +++ documentation/swerve/examples/Chassis.java | 217 ++++++++ .../swerve/examples/PathPlannerConfig.java | 61 +++ .../lib/subsystems/swerve/SwerveBase.java | 489 +++++++++--------- .../lib/subsystems/swerve/SwerveChassis.java | 19 +- 9 files changed, 809 insertions(+), 265 deletions(-) create mode 100644 documentation/README.md create mode 100644 documentation/swerve/README.md create mode 100644 documentation/swerve/examples/BLinePaths.java create mode 100644 documentation/swerve/examples/Chassis.java create mode 100644 documentation/swerve/examples/PathPlannerConfig.java diff --git a/README.md b/README.md index 049e062..4f5b52a 100644 --- a/README.md +++ b/README.md @@ -41,12 +41,21 @@ directories, lowercased: - **`SwerveBase` extends `SubsystemBase`.** C++ mixes the two in via multiple inheritance, which Java does not have, so the hierarchy is `SubsystemBase` → `SwerveBase` → `SwerveChassis` → your drivetrain. +- **Path following is wired up in the robot project.** The C++ `SwerveBase` configures + PathPlanner's `AutoBuilder` itself. The Java one does not, and has no PathPlanner + dependency at all, so a robot can follow paths with PathPlanner, BLine or anything else. + [documentation/swerve](documentation/swerve/README.md) has a worked example of each. - **Standard deviations are `Matrix`** (built with `VecBuilder.fill`) rather than the C++ `wpi::array`, because that is what the WPILib pose estimator takes. - `LimelightHelpers.java` is vendored from [limelightlib-wpijava](https://github.com/LimelightVision/limelightlib-wpijava) (v1.14). Only its `package` declaration and one malformed Javadoc tag were changed. +## Documentation + +Guides and example robot code live in [documentation/](documentation/README.md). The +examples are based on Shelby's code and are not compiled by this build. + ## Building ```bash @@ -115,14 +124,23 @@ Java vendor dependencies are read directly out of `vendordeps/*.json` at configu time, so the Java and C++ builds always consume the same vendor versions. Adding or bumping a vendordep JSON is all that is needed — there is no second list to update. -Currently: WPILib New Commands, CTRE Phoenix 6, PathPlannerLib, PhotonLib and maple-sim. +Currently: WPILib New Commands, CTRE Phoenix 6, PhotonLib and maple-sim. + +`vendordeps/PathplannerLib.json` is the one exception to "no second list". Only the C++ +sources still use PathPlanner, so `build.gradle` names that file as C++ only and keeps it +off the Java classpath. That is what stops a PathPlanner import from compiling in +`src/main/java`. ### Robot projects install these themselves `OvertureLib.json` declares only the OvertureLib artifact, so none of the vendor libraries above come along with it. That has always been true: a robot project already installs -Phoenix 6, PathPlanner and PhotonLib of its own accord even though it reaches most of them -through this library rather than directly. +Phoenix 6 and PhotonLib of its own accord even though it reaches most of them through this +library rather than directly. + +A path following library is the robot's own choice and its own vendordep. Nothing in the +Java library needs one, so a robot that follows paths with BLine does not install +PathPlanner, and the other way round. maple-sim is simply the newest entry on that list, and the one a robot set up before 2026 will not have. Without it, simulation fails immediately. A real robot most likely runs diff --git a/build.gradle b/build.gradle index 8a4bda2..fe98928 100644 --- a/build.gradle +++ b/build.gradle @@ -31,11 +31,21 @@ var javaVersion = "17" // Vendordep java artifacts are read straight out of vendordeps/*.json so that the // Java and C++ builds always consume the same vendor versions. A dependency whose // version is "wpilib" ships with WPILib itself and takes wpilibVersion. +// +// PathplannerLib is the exception. Only the C++ sources still use it, so its json +// stays in vendordeps/ for a -PjavaOnly=false build and is kept off the Java +// classpath. Path following is wired up in the robot project (see +// documentation/swerve), and a PathPlanner that is still importable from here is +// how it would grow back in without anyone deciding it should. +def cppOnlyVendordeps = ['PathplannerLib.json'] + ext.vendordepJava = { def repos = [] as LinkedHashSet def deps = [] as LinkedHashSet def slurper = new groovy.json.JsonSlurper() - file('vendordeps').listFiles().findAll {it.name.endsWith('.json')}.sort().each { vendordep -> + file('vendordeps').listFiles().findAll { + it.name.endsWith('.json') && !(it.name in cppOnlyVendordeps) + }.sort().each { vendordep -> def json = slurper.parse(vendordep) json.mavenUrls.each {repos.add(it)} json.javaDependencies.each { dep -> @@ -90,7 +100,7 @@ dependencies { implementation "com.fasterxml.jackson.core:jackson-databind:2.19.2" implementation 'edu.wpi.first.thirdparty.frc2025.opencv:opencv-java:4.10.0-3' - // WPILibNewCommands, Phoenix 6, PathPlannerLib and PhotonLib + // WPILibNewCommands, Phoenix 6, PhotonLib and maple-sim vendordepJava.deps.each {implementation it} testImplementation 'org.junit.jupiter:junit-jupiter:5.13.4' diff --git a/documentation/README.md b/documentation/README.md new file mode 100644 index 0000000..5b22285 --- /dev/null +++ b/documentation/README.md @@ -0,0 +1,17 @@ +# OvertureLib documentation + +Guides for using the library from a robot project, with the example code they refer to. + +| Guide | What it covers | +| ----- | -------------- | +| [swerve](swerve/README.md) | Building a drivetrain on `SwerveChassis` and following paths with it, using PathPlanner or BLine | + +## About the examples + +The examples are plain Java files, written the way they would sit in a robot project and +based on Shelby's code ([FRC-Shelby-2026OS](https://github.com/Overture-7421/FRC-Shelby-2026OS)). +Copy them into a robot and change the numbers. + +They are not part of the Gradle build. They depend on libraries OvertureLib deliberately +does not, so nothing compiles them when the library changes: if you change an API one of +them uses, update the example with it. diff --git a/documentation/swerve/README.md b/documentation/swerve/README.md new file mode 100644 index 0000000..edb0660 --- /dev/null +++ b/documentation/swerve/README.md @@ -0,0 +1,159 @@ +# Swerve and path following + +OvertureLib builds the drivetrain. It does not choose how the robot follows paths: that is +wired up in the robot project, so a robot can use PathPlanner, BLine, both or neither +without the library carrying any of them. + +## What a path follower plugs into + +Every path follower asks for the same handful of things, and `SwerveChassis` already has +them as public methods: + +| The follower wants | Give it | Notes | +| ------------------ | ------- | ----- | +| The robot pose | `chassis::getEstimatedPose` | Odometry fused with vision | +| A way to reset the pose | `chassis::resetPose` | Not `resetOdometry`, see below | +| The current speeds | `chassis::getCurrentSpeeds` | Robot relative | +| Somewhere to send speeds | `chassis::setTargetSpeeds` | Robot relative | +| The subsystem it requires | `chassis` | | + +**`resetPose` or `resetOdometry`?** A path follower resetting the pose means "the robot is +here now", so `resetPose` also places the simulated robot there. `resetOdometry` only +corrects the estimate, which is what a heading reset or a vision correction wants. Hand a +follower `resetOdometry` and autos still run on the real robot, but in simulation the +physics robot stays where it was while odometry jumps to the start of the auto. + +Speeds sent to `setTargetSpeeds` go through the same path as the driver's: an active +`SpeedsHelper` still rewrites them, and they are discretized and desaturated before +reaching the modules. + +## The examples + +| File | What it is | +| ---- | ---------- | +| [Chassis.java](examples/Chassis.java) | Shelby's drivetrain. No path follower in sight | +| [PathPlannerConfig.java](examples/PathPlannerConfig.java) | Hands that chassis to PathPlanner | +| [BLinePaths.java](examples/BLinePaths.java) | Hands that chassis to BLine | + +The chassis is the same for both followers. Each follower lives in one small class beside +it, in `frc.robot.Subsystems.Chassis`, and switching is a matter of which one +`RobotContainer` uses. + +They were compiled against PathPlannerLib 2026.1.2 and BLine-Lib v0.9.2. + +## PathPlanner + +1. Install the vendordep in the robot project (WPILib: Manage Vendor Libraries, Install new + libraries (online)): + + ```text + https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json + ``` + +2. Open the robot project in the PathPlanner app and fill in the robot config. It is saved + to `src/main/deploy/pathplanner/settings.json`, which is what + `RobotConfig.fromGUISettings()` reads. Nothing checks it against the chassis, so the + numbers have to be kept in step by hand. For Shelby: + + | `settings.json` | Value | Same number in `Chassis.java` | + | --------------- | ----- | ----------------------------- | + | `robotMass` | 61.235 | `withRobotMass(Kilograms.of(61.235))` | + | `driveGearing` | 7.03 | `kDriveGearRatio` | + | `driveWheelRadius` | 0.0508 | `kWheelRadiusInches` (2 in) | + | `maxDriveSpeed` | 4.541 | `getMaxModuleSpeed()` | + | `flModuleX`, `flModuleY` | 0.282575, 0.276225 | `kTrackXInches`, `kTrackYInches` (11.125 in, 10.875 in) | + | `driveCurrentLimit` | 60.0 | `SupplyCurrentLimit` | + +3. Copy [PathPlannerConfig.java](examples/PathPlannerConfig.java) next to the chassis and + call it from `RobotContainer`: + + ```java + public final Chassis chassis = new Chassis(); + private final SendableChooser autoChooser; + + public RobotContainer() { + // Before the chooser, which builds every auto in deploy/pathplanner/autos as it is + // created. NamedCommands.registerCommand(...) goes before it too, for the same reason. + PathPlannerConfig.configure(chassis); + + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + } + + public Command getAutonomousCommand() { + return autoChooser.getSelected(); + } + ``` + +The path following gains are the two `PIDConstants` at the top of `PathPlannerConfig`. + +## BLine + +1. Install the vendordep in the robot project: + + ```text + https://bline-metrics.edan-liahovetsky.workers.dev/vendor/BLine-Lib.json + ``` + + If that URL is down, BLine's README gives + `https://raw.githubusercontent.com/edanliahovetsky/BLine-Lib/main/BLine-Lib.json` as + the fallback. + +2. Draw the paths in the [BLine editor](https://bline-web.pages.dev/). BLine loads them + from `src/main/deploy/autos/paths/.json`, and the default constraints from + `src/main/deploy/autos/config.json`. + +3. Copy [BLinePaths.java](examples/BLinePaths.java) next to the chassis and build the + autos from it in `RobotContainer`: + + ```java + public final Chassis chassis = new Chassis(); + private final BLinePaths paths = new BLinePaths(chassis); + private final SendableChooser autoChooser = new SendableChooser<>(); + + public RobotContainer() { + autoChooser.setDefaultOption("None", Commands.none()); + autoChooser.addOption( + "Left double pass", + Commands.sequence( + paths.resetAndFollow("LeftDoublePass1"), + paths.follow("LeftDoublePass2"))); + SmartDashboard.putData("Auto Chooser", autoChooser); + } + + public Command getAutonomousCommand() { + return autoChooser.getSelected(); + } + ``` + +Two things about BLine that the example is built around: + +- **The pose reset belongs to the first path only.** BLine resets the pose every time a + command built with a reset starts. `resetAndFollow` is for the first path of an auto and + `follow` for the rest; a reset on a later path throws away what odometry and vision + worked out on the way there. +- **A path file is read when its command is built**, and a missing one throws. Building + the autos in the `RobotContainer` constructor, as above, makes that a crash at startup + in the pits instead of at the start of a match. + +If a path has event triggers that schedule commands, BLine's README recommends composing +the auto with its own `BLineCommands.sequence` instead of `Commands.sequence`. + +The path following gains are the three `PIDController`s in `BLinePaths`. They are BLine's +suggested starting values and have not been tuned on a robot. + +## Updating a robot from 2026.4.0-beta-3 or earlier + +Up to 2026.4.0-beta-3, `SwerveBase.configureSwerveBase()` configured PathPlanner itself. A +robot written against those versions needs three changes when it updates: + +1. **Delete `getTranslationPID()` and `getRotationPID()` from the chassis.** They no longer + override anything, so the `@Override` on them stops compiling. The gains move to + `PathPlannerConfig`. +2. **Add `PathPlannerConfig` and call `PathPlannerConfig.configure(chassis)`.** Without it + `AutoBuilder` is never configured, and PathPlanner refuses to build autos. +3. **Stop relying on `/PathPlanner/ResetPose`.** The library no longer publishes that + NetworkTables topic. It existed for the old external simulator; the built in simulation + is moved by `resetPose` directly. + +Robots that never ran an auto need only the first. diff --git a/documentation/swerve/examples/BLinePaths.java b/documentation/swerve/examples/BLinePaths.java new file mode 100644 index 0000000..a94e7a8 --- /dev/null +++ b/documentation/swerve/examples/BLinePaths.java @@ -0,0 +1,74 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Subsystems.Chassis; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.lib.BLine.FollowPath; +import frc.robot.lib.BLine.Path; + +/** + * Hands the chassis to BLine and builds the commands that follow its paths. + * + *

This is robot code. OvertureLib does not know BLine exists, so this class is the only place + * the two meet. Build one of these in RobotContainer and ask it for every path command. + */ +public final class BLinePaths { + private final Chassis chassis; + private final FollowPath.Builder builder; + + /** + * Wires BLine to the chassis. + * + * @param chassis the drivetrain BLine will drive + */ + public BLinePaths(Chassis chassis) { + this.chassis = chassis; + + // The gains are BLine's own starting values, not ones tuned on Shelby. + builder = + new FollowPath.Builder( + chassis, + chassis::getEstimatedPose, + // Robot relative, both the speeds read and the speeds commanded. + chassis::getCurrentSpeeds, + chassis::setTargetSpeeds, + // Translation: how hard to drive at what is left of the path. + new PIDController(5.0, 0.0, 0.0), + // Rotation: how hard to turn towards the heading the path asks for. + new PIDController(3.0, 0.0, 0.0), + // Cross track: how hard to pull back onto the line between two points. + new PIDController(2.0, 0.0, 0.0)) + // Paths are drawn on the blue side and flipped when we are red. + .withDefaultShouldFlip(); + } + + /** + * Follows a path from wherever the robot believes it is. + * + * @param pathName the path file in deploy/autos/paths, without the .json + * @return the command + */ + public Command follow(String pathName) { + // The builder remembers its pose reset between builds, so it is set on every build rather + // than left to whichever method happened to run last. + return builder.withPoseReset(pose -> {}).build(new Path(pathName)); + } + + /** + * Places the robot at the start of a path, then follows it. For the first path of an auto. + * + *

Only the first. BLine resets the pose every time a command built with a reset starts, so + * using this for a later path throws away whatever odometry and vision worked out on the way + * there. + * + * @param pathName the path file in deploy/autos/paths, without the .json + * @return the command + */ + public Command resetAndFollow(String pathName) { + // resetPose rather than resetOdometry: it places the simulated robot there as well. + return builder.withPoseReset(chassis::resetPose).build(new Path(pathName)); + } +} diff --git a/documentation/swerve/examples/Chassis.java b/documentation/swerve/examples/Chassis.java new file mode 100644 index 0000000..79bf67d --- /dev/null +++ b/documentation/swerve/examples/Chassis.java @@ -0,0 +1,217 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Subsystems.Chassis; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Kilograms; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.signals.InvertedValue; +import com.overture.lib.sensors.OverPigeon; +import com.overture.lib.subsystems.swerve.SwerveChassis; +import com.overture.lib.subsystems.swerve.SwerveModule; +import com.overture.lib.subsystems.swerve.SwerveModuleConfig; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; + +/** + * Shelby's drivetrain, from FRC-Shelby-2026OS, as an example of a concrete {@link SwerveChassis}. + * + *

Nothing in here knows how the robot follows paths. A path follower is wired in from outside, + * through the public methods the chassis inherits: see PathPlannerConfig and BLinePaths next to + * this file. Swapping one for the other does not touch this class. + */ +public class Chassis extends SwerveChassis { + /** The CANivore carrying the drivetrain. */ + private static final CANBus kCanBus = new CANBus("OverCANivore"); + + /** Drive reduction, shared by the real module config and the simulated drivetrain below. */ + private static final double kDriveGearRatio = 7.03; + + /** Steer reduction, likewise shared. */ + private static final double kTurnGearRatio = 287.0 / 11.0; + + private static final double kWheelRadiusInches = 2.0; + private static final double kTrackXInches = 11.125; + private static final double kTrackYInches = 10.875; + + private final OverPigeon pigeon = new OverPigeon(13, kCanBus); + + private final SwerveModule frontLeftModule = new SwerveModule(frontLeftConfig()); + private final SwerveModule frontRightModule = new SwerveModule(frontRightConfig()); + private final SwerveModule backLeftModule = new SwerveModule(backLeftConfig()); + private final SwerveModule backRightModule = new SwerveModule(backRightConfig()); + + // Front left, front right, back left, back right. Every list of four in the library is in this + // order. + private final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics( + new Translation2d( + Units.inchesToMeters(kTrackXInches), Units.inchesToMeters(kTrackYInches)), + new Translation2d( + Units.inchesToMeters(kTrackXInches), Units.inchesToMeters(-kTrackYInches)), + new Translation2d( + Units.inchesToMeters(-kTrackXInches), Units.inchesToMeters(kTrackYInches)), + new Translation2d( + Units.inchesToMeters(-kTrackXInches), Units.inchesToMeters(-kTrackYInches))); + + /** + * Drive feedforward: ks volts, kv volts per m/s, ka volts per m/s^2. + * + *

One factory for the four modules, so tuning it cannot end up tuning a single wheel. + * + * @return the drive feedforward + */ + private static SimpleMotorFeedforward driveFeedForward() { + return new SimpleMotorFeedforward(0.0, 2.0879, 0.098433); + } + + /** Builds the drivetrain. */ + public Chassis() { + super(); + configureSwerveBase(); + setAcceptingVisionMeasurements(true); + resetHeading(); + } + + /** + * Describes this drivetrain to the physics simulation. + * + *

The gearing constants are the same ones the module configs use below. That is the entire + * point: kept anywhere else, a drive ratio can sit at 6.03 against a robot geared 7.03 without + * anyone noticing. + */ + @Override + protected DriveTrainSimulationConfig getSimulationConfig() { + return DriveTrainSimulationConfig.Default() + .withRobotMass(Kilograms.of(61.235)) + .withGyro(COTS.ofPigeon2()) + .withSwerveModule( + new SwerveModuleSimulationConfig( + DCMotor.getKrakenX60(1), + DCMotor.getFalcon500(1), + kDriveGearRatio, + kTurnGearRatio, + Volts.of(0.01), + Volts.of(0.01), + Inches.of(kWheelRadiusInches), + KilogramSquareMeters.of(0.03), + // Conservative tyre friction. The COTS value was flagged as abnormal by the + // simulation (~2.106); 0.9 is realistic for rubber on carpet. + 0.9)) + .withCustomModuleTranslations( + new Translation2d[] { + new Translation2d(Inches.of(kTrackXInches), Inches.of(kTrackYInches)), + new Translation2d(Inches.of(kTrackXInches), Inches.of(-kTrackYInches)), + new Translation2d(Inches.of(-kTrackXInches), Inches.of(kTrackYInches)), + new Translation2d(Inches.of(-kTrackXInches), Inches.of(-kTrackYInches)) + }) + .withBumperSize(Inches.of(30.5), Inches.of(31.5)); + } + + @Override + protected Pigeon2 getSimulationPigeon() { + return pigeon; + } + + @Override + public double getMaxModuleSpeed() { + return 4.541; + } + + @Override + protected SwerveModule getFrontLeftModule() { + return frontLeftModule; + } + + @Override + protected SwerveModule getFrontRightModule() { + return frontRightModule; + } + + @Override + protected SwerveModule getBackLeftModule() { + return backLeftModule; + } + + @Override + protected SwerveModule getBackRightModule() { + return backRightModule; + } + + @Override + protected SwerveDriveKinematics getKinematics() { + return kinematics; + } + + @Override + public Rotation2d getRotation2d() { + return pigeon.getRotation2d(); + } + + private static SwerveModuleConfig baseModuleConfig() { + SwerveModuleConfig config = new SwerveModuleConfig(driveFeedForward(), 0, 0); + config.CanBus = kCanBus; + config.DriveGearRatio = kDriveGearRatio; + config.TurnGearRatio = kTurnGearRatio; + config.WheelDiameter = Units.inchesToMeters(2.0 * kWheelRadiusInches); + config.DriveMotorConfig.CurrentLimits.SupplyCurrentLowerLimit = 30.0; + config.DriveMotorConfig.CurrentLimits.SupplyCurrentLimit = 60.0; + config.TurnMotorConfig.Slot0.withKP(40).withKS(0.15); + config.TurnMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + config.DriveMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + return config; + } + + private static SwerveModuleConfig frontLeftConfig() { + SwerveModuleConfig config = baseModuleConfig(); + config.driveMotorId = 4; + config.turnMotorId = 3; + config.EncoderConfig.CanCoderId = 12; + config.EncoderConfig.Offset = -0.147705078125; + config.ModuleName = "Front Left"; + return config; + } + + private static SwerveModuleConfig frontRightConfig() { + SwerveModuleConfig config = baseModuleConfig(); + config.driveMotorId = 8; + config.turnMotorId = 7; + config.EncoderConfig.CanCoderId = 11; + config.EncoderConfig.Offset = 0.116943359375; + config.ModuleName = "Front Right"; + return config; + } + + private static SwerveModuleConfig backLeftConfig() { + SwerveModuleConfig config = baseModuleConfig(); + config.driveMotorId = 2; + config.turnMotorId = 1; + config.EncoderConfig.CanCoderId = 9; + config.EncoderConfig.Offset = -0.035888671875; + config.ModuleName = "Back Left"; + return config; + } + + private static SwerveModuleConfig backRightConfig() { + SwerveModuleConfig config = baseModuleConfig(); + config.driveMotorId = 6; + config.turnMotorId = 5; + config.EncoderConfig.CanCoderId = 10; + config.EncoderConfig.Offset = 0.37744140625; + config.ModuleName = "Back Right"; + return config; + } +} diff --git a/documentation/swerve/examples/PathPlannerConfig.java b/documentation/swerve/examples/PathPlannerConfig.java new file mode 100644 index 0000000..fa766e3 --- /dev/null +++ b/documentation/swerve/examples/PathPlannerConfig.java @@ -0,0 +1,61 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Subsystems.Chassis; + +import com.overture.lib.utils.UtilityFunctions; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** + * Hands the chassis to PathPlanner. + * + *

This is robot code. OvertureLib does not know PathPlanner exists, so this class is the only + * place the two meet, and a robot that follows its paths some other way never installs it. + */ +public final class PathPlannerConfig { + /** Gains correcting position error along a path. */ + private static final PIDConstants kTranslationPID = new PIDConstants(6.0, 0.0, 0.0); + + /** Gains correcting heading error along a path. */ + private static final PIDConstants kRotationPID = new PIDConstants(6.0, 0.0, 0.0); + + private PathPlannerConfig() {} + + /** + * Configures PathPlanner's AutoBuilder. Call it once, after the chassis is built and before + * anything asks PathPlanner for an auto, a path command or the auto chooser. + * + * @param chassis the drivetrain PathPlanner will drive + */ + public static void configure(Chassis chassis) { + // Mass, MOI, module positions and motor limits, read from deploy/pathplanner/settings.json. + // They are typed into the PathPlanner app by hand, so nothing but care keeps them agreeing + // with the Chassis. + RobotConfig robotConfig; + try { + robotConfig = RobotConfig.fromGUISettings(); + } catch (Exception e) { + throw new RuntimeException("Failed to load the PathPlanner RobotConfig from GUI settings", e); + } + + AutoBuilder.configure( + chassis::getEstimatedPose, + // resetPose rather than resetOdometry. An auto resetting the pose means "the robot is + // here now", and resetPose places the simulated robot there as well. + chassis::resetPose, + chassis::getCurrentSpeeds, + // Robot relative. The parameter type is spelled out to pick the overload that takes the + // speeds alone, without the per module feedforwards. + (ChassisSpeeds speeds) -> chassis.setTargetSpeeds(speeds), + new PPHolonomicDriveController(kTranslationPID, kRotationPID), + robotConfig, + // Paths are drawn on the blue side and flipped when we are red. + UtilityFunctions::isRedAlliance, + chassis); + } +} diff --git a/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java b/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java index ec47e83..ab6d526 100644 --- a/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java +++ b/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java @@ -6,10 +6,6 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.overture.lib.simulation.swerve.SimSwerveDrivetrain; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,256 +13,255 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.Optional; import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; /** - * Wiring shared by every swerve drivetrain: the pose estimator and the PathPlanner AutoBuilder - * hookup. + * Wiring shared by every swerve drivetrain: the pose estimator and the physics + * simulation hookup. * - *

The C++ version mixes this in alongside SubsystemBase; Java has no multiple inheritance, so - * this class extends SubsystemBase directly and {@link SwerveChassis} extends it. + *

+ * Path following is deliberately not wired here. Whether a robot follows paths + * with PathPlanner, + * BLine or something else is the robot project's call, and all any of them need + * is already public: + * {@link #getEstimatedPose()}, {@link #resetPose(Pose2d)}, + * {@link #getCurrentSpeeds()} and {@link + * #setTargetSpeeds(ChassisSpeeds)}. Worked examples of both live in + * documentation/swerve. + * + *

+ * The C++ version mixes this in alongside SubsystemBase; Java has no multiple + * inheritance, so + * this class extends SubsystemBase directly and {@link SwerveChassis} extends + * it. */ public abstract class SwerveBase extends SubsystemBase { - /** Whether {@link #configureSwerveBase()} has run. */ - protected boolean configuredChassis = false; - - /** Pose estimator fusing odometry and vision. */ - protected SwerveDrivePoseEstimator odometry; - - /** Latest module positions, in front-left, front-right, back-left, back-right order. */ - protected SwerveModulePosition[] modulesPositions = - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; - - /** Latest module states, in front-left, front-right, back-left, back-right order. */ - protected SwerveModuleState[] modulesStates = - new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; - - private final StructPublisher resetOdometryPosePublisher = - NetworkTableInstance.getDefault() - .getStructTopic("/PathPlanner/ResetPose", Pose2d.struct) - .publish(); - - /** - * Returns the latest estimated pose. - * - * @return the estimated pose - */ - public abstract Pose2d getEstimatedPose(); - - /** - * Resets the pose estimator to a known pose. - * - * @param initPose the pose to reset to - */ - public abstract void resetOdometry(Pose2d initPose); - - /** - * Returns the current robot relative speeds. - * - * @return the current speeds - */ - public abstract ChassisSpeeds getCurrentSpeeds(); - - /** - * Requests new robot relative speeds. - * - * @param speeds the desired speeds - */ - public abstract void setTargetSpeeds(ChassisSpeeds speeds); - - /** - * Returns the maximum speed a single module can reach. - * - * @return the maximum module speed, in meters per second - */ - public abstract double getMaxModuleSpeed(); - - /** - * Returns the gyro heading. - * - * @return the heading - */ - public abstract Rotation2d getRotation2d(); - - /** The physics simulation, or null on a real robot or when no simulation config was given. */ - private SimSwerveDrivetrain simDrivetrain; - - /** - * Wires up PathPlanner and builds the pose estimator. Call this from the constructor of the - * concrete drivetrain, once its modules exist. - */ - protected void configureSwerveBase() { - RobotConfig robotConfig; - try { - robotConfig = RobotConfig.fromGUISettings(); - } catch (Exception e) { - throw new RuntimeException("Failed to load the PathPlanner RobotConfig from GUI settings", e); - } - - configuredChassis = true; - AutoBuilder.configure( - this::getEstimatedPose, - pose -> { - // PathPlanner resetting the pose means "the robot is here now", so the simulated robot - // is placed there too. This replaces the /PathPlanner/ResetPose topic the external - // simulator listened on, and with it the trap that republishing an identical pose - // silently did nothing, because NetworkTables only notified on change. - // - // Deliberately not done inside resetOdometry itself, tempting as that is. resetHeading - // routes through it, and the MegaTag1 watchdog routes through resetHeading, so teleport - // there would let a vision correction shove the physics robot to match its own estimate - // and the watchdog could never be caught being wrong. - resetOdometryPosePublisher.set(pose); - resetSimulatedPose(pose); - resetOdometry(pose); - }, - this::getCurrentSpeeds, - (ChassisSpeeds speeds) -> setTargetSpeeds(speeds), - new PPHolonomicDriveController(getTranslationPID(), getRotationPID()), - robotConfig, - () -> - DriverStation.getAlliance() - .map(alliance -> alliance == DriverStation.Alliance.Red) - .orElse(false), - this); - - odometry = - new SwerveDrivePoseEstimator( - getKinematics(), new Rotation2d(), modulesPositions, new Pose2d()); - - startSimulation(); - } - - /** - * Describes this drivetrain to the physics simulation, or returns null to run without one. - * - *

This is the only thing a robot has to write to get a simulated drivetrain, and it lives in - * the robot project on purpose. Gear ratios and module positions belong next to the {@link - * SwerveModuleConfig} values they have to agree with: keeping them in a separate simulation - * project is how a drive ratio of 6.03 sat beside a real robot geared 7.03 for a whole season - * without anyone noticing. - * - * @return the simulation config, or null for no simulation - */ - protected DriveTrainSimulationConfig getSimulationConfig() { - return null; - } - - /** - * Returns the gyro the simulation should drive, or null to leave it alone. - * - * @return the Pigeon to simulate - */ - protected Pigeon2 getSimulationPigeon() { - return null; - } - - /** - * Returns where the physics says the robot really is, for comparing odometry against. - * - * @return the true pose, or empty on a real robot or without a simulation config - */ - public Optional getSimulatedPose() { - return Optional.ofNullable(simDrivetrain).map(SimSwerveDrivetrain::getPose); - } - - /** - * Teleports the simulated robot. Does nothing on a real one. - * - * @param pose where to place it - */ - public void resetSimulatedPose(Pose2d pose) { - if (simDrivetrain != null) { - simDrivetrain.resetPose(pose); - } - } - - // Guarded so the maple-sim classes are never touched on the roboRIO. Java has no preprocessor, - // so they are linked either way; what matters is that nothing constructs or steps them. - private void startSimulation() { - if (!RobotBase.isSimulation() || getSimulationConfig() == null) { - return; - } - - simDrivetrain = - new SimSwerveDrivetrain(getSimulationConfig(), new Pose2d(), getSimulationPigeon()); - - // Same order as the module translations and the kinematics. - getFrontLeftModule().attachSimulation(simDrivetrain.getModule(0)); - getFrontRightModule().attachSimulation(simDrivetrain.getModule(1)); - getBackLeftModule().attachSimulation(simDrivetrain.getModule(2)); - getBackRightModule().attachSimulation(simDrivetrain.getModule(3)); - } - - @Override - public void simulationPeriodic() { - if (simDrivetrain != null) { - simDrivetrain.update(); - } - } - - /** - * Returns the front left module. - * - * @return the front left module - */ - protected abstract SwerveModule getFrontLeftModule(); - - /** - * Returns the front right module. - * - * @return the front right module - */ - protected abstract SwerveModule getFrontRightModule(); - - /** - * Returns the back left module. - * - * @return the back left module - */ - protected abstract SwerveModule getBackLeftModule(); - - /** - * Returns the back right module. - * - * @return the back right module - */ - protected abstract SwerveModule getBackRightModule(); - - /** - * Returns the drivetrain kinematics. - * - * @return the kinematics - */ - protected abstract SwerveDriveKinematics getKinematics(); - - /** - * Returns the PathPlanner translation gains. - * - * @return the translation PID constants - */ - protected abstract PIDConstants getTranslationPID(); - - /** - * Returns the PathPlanner rotation gains. - * - * @return the rotation PID constants - */ - protected abstract PIDConstants getRotationPID(); + /** Whether {@link #configureSwerveBase()} has run. */ + protected boolean configuredChassis = false; + + /** Pose estimator fusing odometry and vision. */ + protected SwerveDrivePoseEstimator odometry; + + /** + * Latest module positions, in front-left, front-right, back-`left, back-right + * order. + */ + protected SwerveModulePosition[] modulesPositions = new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + + /** + * Latest module states, in front-left, front-right, back-left, back-right + * order. + */ + protected SwerveModuleState[] modulesStates = new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + + /** + * Returns the latest estimated pose. + * + * @return the estimated pose + */ + public abstract Pose2d getEstimatedPose(); + + /** + * Resets the pose estimator to a known pose. + * + * @param initPose the pose to reset to + */ + public abstract void resetOdometry(Pose2d initPose); + + /** + * Returns the current robot relative speeds. + * + * @return the current speeds + */ + public abstract ChassisSpeeds getCurrentSpeeds(); + + /** + * Requests new robot relative speeds. + * + * @param speeds the desired speeds + */ + public abstract void setTargetSpeeds(ChassisSpeeds speeds); + + /** + * Returns the maximum speed a single module can reach. + * + * @return the maximum module speed, in meters per second + */ + public abstract double getMaxModuleSpeed(); + + /** + * Returns the gyro heading. + * + * @return the heading + */ + public abstract Rotation2d getRotation2d(); + + /** + * The physics simulation, or null on a real robot or when no simulation config + * was given. + */ + private SimSwerveDrivetrain simDrivetrain; + + /** + * Builds the pose estimator and starts the simulation. Call this from the + * constructor of the + * concrete drivetrain, once its modules exist. + */ + protected void configureSwerveBase() { + configuredChassis = true; + + odometry = new SwerveDrivePoseEstimator( + getKinematics(), new Rotation2d(), modulesPositions, new Pose2d()); + + startSimulation(); + } + + /** + * Places the robot at a known pose. This is the reset to hand a path follower. + * + *

+ * A path follower resetting the pose means "the robot is here now", so the + * simulated robot is + * placed there too. {@link #resetOdometry(Pose2d)} only corrects the estimate, + * and stays the one + * for anything that is a correction rather than a placement. + * + * @param pose where the robot is + */ + public void resetPose(Pose2d pose) { + // Deliberately not done inside resetOdometry itself, tempting as that is. + // resetHeading + // routes through it, and the MegaTag1 watchdog routes through resetHeading, so + // teleport + // there would let a vision correction shove the physics robot to match its own + // estimate + // and the watchdog could never be caught being wrong. + resetSimulatedPose(pose); + resetOdometry(pose); + } + + /** + * Describes this drivetrain to the physics simulation, or returns null to run + * without one. + * + *

+ * This is the only thing a robot has to write to get a simulated drivetrain, + * and it lives in + * the robot project on purpose. Gear ratios and module positions belong next to + * the {@link + * SwerveModuleConfig} values they have to agree with: keeping them in a + * separate simulation + * project is how a drive ratio of 6.03 sat beside a real robot geared 7.03 for + * a whole season + * without anyone noticing. + * + * @return the simulation config, or null for no simulation + */ + protected DriveTrainSimulationConfig getSimulationConfig() { + return null; + } + + /** + * Returns the gyro the simulation should drive, or null to leave it alone. + * + * @return the Pigeon to simulate + */ + protected Pigeon2 getSimulationPigeon() { + return null; + } + + /** + * Returns where the physics says the robot really is, for comparing odometry + * against. + * + * @return the true pose, or empty on a real robot or without a simulation + * config + */ + public Optional getSimulatedPose() { + return Optional.ofNullable(simDrivetrain).map(SimSwerveDrivetrain::getPose); + } + + /** + * Teleports the simulated robot. Does nothing on a real one. + * + * @param pose where to place it + */ + public void resetSimulatedPose(Pose2d pose) { + if (simDrivetrain != null) { + simDrivetrain.resetPose(pose); + } + } + + // Guarded so the maple-sim classes are never touched on the roboRIO. Java has + // no preprocessor, + // so they are linked either way; what matters is that nothing constructs or + // steps them. + private void startSimulation() { + if (!RobotBase.isSimulation() || getSimulationConfig() == null) { + return; + } + + simDrivetrain = new SimSwerveDrivetrain(getSimulationConfig(), new Pose2d(), getSimulationPigeon()); + + // Same order as the module translations and the kinematics. + getFrontLeftModule().attachSimulation(simDrivetrain.getModule(0)); + getFrontRightModule().attachSimulation(simDrivetrain.getModule(1)); + getBackLeftModule().attachSimulation(simDrivetrain.getModule(2)); + getBackRightModule().attachSimulation(simDrivetrain.getModule(3)); + } + + @Override + public void simulationPeriodic() { + if (simDrivetrain != null) { + simDrivetrain.update(); + } + } + + /** + * Returns the front left module. + * + * @return the front left module + */ + protected abstract SwerveModule getFrontLeftModule(); + + /** + * Returns the front right module. + * + * @return the front right module + */ + protected abstract SwerveModule getFrontRightModule(); + + /** + * Returns the back left module. + * + * @return the back left module + */ + protected abstract SwerveModule getBackLeftModule(); + + /** + * Returns the back right module. + * + * @return the back right module + */ + protected abstract SwerveModule getBackRightModule(); + + /** + * Returns the drivetrain kinematics. + * + * @return the kinematics + */ + protected abstract SwerveDriveKinematics getKinematics(); } diff --git a/src/main/java/com/overture/lib/subsystems/swerve/SwerveChassis.java b/src/main/java/com/overture/lib/subsystems/swerve/SwerveChassis.java index bd62a6e..4bd6f64 100644 --- a/src/main/java/com/overture/lib/subsystems/swerve/SwerveChassis.java +++ b/src/main/java/com/overture/lib/subsystems/swerve/SwerveChassis.java @@ -27,10 +27,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import java.util.Optional; -/** - * A swerve drivetrain. Concrete robots extend this and supply the modules, kinematics and - * PathPlanner gains. - */ +/** A swerve drivetrain. Concrete robots extend this and supply the modules and kinematics. */ public abstract class SwerveChassis extends SwerveBase { private Pose2d latestPose = new Pose2d(); @@ -187,15 +184,11 @@ public Pose2d getEstimatedPose() { @Override public void resetOdometry(Pose2d initPose) { odometry.resetPosition(getRotation2d(), modulesPositions, initPose); - // Refresh the cache getEstimatedPose() serves. Without this the estimator knows - // the new pose - // but every reader keeps seeing the old one until the next periodic(), and - // CommandScheduler - // runs subsystem periodics BEFORE it initializes commands. PathPlanner - // registers - // resetOdometry as its reset consumer and getEstimatedPose as its pose - // supplier, so a - // reset-then-follow scheduled from a trigger would start the path from the + // Refresh the cache getEstimatedPose() serves. Without this the estimator knows the new pose + // but every reader keeps seeing the old one until the next periodic(), and CommandScheduler + // runs subsystem periodics BEFORE it initializes commands. A path follower is handed + // resetPose, which lands here, as its reset consumer and getEstimatedPose as its pose + // supplier, so a reset-then-follow scheduled from a trigger would start the path from the // pre-reset pose. latestPose = odometry.getEstimatedPosition(); } From 13e8b4dca433ae3a7815c2a30f05a95de920cd90 Mon Sep 17 00:00:00 2001 From: Luis Leyva Date: Sun, 4 Oct 2026 18:58:02 -0600 Subject: [PATCH 2/3] Forgot Spotless --- .../lib/motorcontrollers/OverTalonFX.java | 2 +- .../lib/subsystems/swerve/SwerveBase.java | 456 +++++++++--------- 2 files changed, 216 insertions(+), 242 deletions(-) diff --git a/src/main/java/com/overture/lib/motorcontrollers/OverTalonFX.java b/src/main/java/com/overture/lib/motorcontrollers/OverTalonFX.java index a63da38..266103c 100644 --- a/src/main/java/com/overture/lib/motorcontrollers/OverTalonFX.java +++ b/src/main/java/com/overture/lib/motorcontrollers/OverTalonFX.java @@ -180,7 +180,7 @@ public void configureSoftwareLimitSwitch(SoftwareLimitSwitchConfigs configs) { /** * Sets the neutral mode, keeping the stored configuration in step with the device. - * + * * @param mode the neutral mode to apply */ public void configureNeutralMode(NeutralModeValue mode) { diff --git a/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java b/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java index ab6d526..53898ec 100644 --- a/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java +++ b/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java @@ -19,249 +19,223 @@ import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; /** - * Wiring shared by every swerve drivetrain: the pose estimator and the physics - * simulation hookup. + * Wiring shared by every swerve drivetrain: the pose estimator and the physics simulation hookup. * - *

- * Path following is deliberately not wired here. Whether a robot follows paths - * with PathPlanner, - * BLine or something else is the robot project's call, and all any of them need - * is already public: - * {@link #getEstimatedPose()}, {@link #resetPose(Pose2d)}, - * {@link #getCurrentSpeeds()} and {@link - * #setTargetSpeeds(ChassisSpeeds)}. Worked examples of both live in - * documentation/swerve. + *

Path following is deliberately not wired here. Whether a robot follows paths with PathPlanner, + * BLine or something else is the robot project's call, and all any of them need is already public: + * {@link #getEstimatedPose()}, {@link #resetPose(Pose2d)}, {@link #getCurrentSpeeds()} and {@link + * #setTargetSpeeds(ChassisSpeeds)}. Worked examples of both live in documentation/swerve. * - *

- * The C++ version mixes this in alongside SubsystemBase; Java has no multiple - * inheritance, so - * this class extends SubsystemBase directly and {@link SwerveChassis} extends - * it. + *

The C++ version mixes this in alongside SubsystemBase; Java has no multiple inheritance, so + * this class extends SubsystemBase directly and {@link SwerveChassis} extends it. */ public abstract class SwerveBase extends SubsystemBase { - /** Whether {@link #configureSwerveBase()} has run. */ - protected boolean configuredChassis = false; - - /** Pose estimator fusing odometry and vision. */ - protected SwerveDrivePoseEstimator odometry; - - /** - * Latest module positions, in front-left, front-right, back-`left, back-right - * order. - */ - protected SwerveModulePosition[] modulesPositions = new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; - - /** - * Latest module states, in front-left, front-right, back-left, back-right - * order. - */ - protected SwerveModuleState[] modulesStates = new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; - - /** - * Returns the latest estimated pose. - * - * @return the estimated pose - */ - public abstract Pose2d getEstimatedPose(); - - /** - * Resets the pose estimator to a known pose. - * - * @param initPose the pose to reset to - */ - public abstract void resetOdometry(Pose2d initPose); - - /** - * Returns the current robot relative speeds. - * - * @return the current speeds - */ - public abstract ChassisSpeeds getCurrentSpeeds(); - - /** - * Requests new robot relative speeds. - * - * @param speeds the desired speeds - */ - public abstract void setTargetSpeeds(ChassisSpeeds speeds); - - /** - * Returns the maximum speed a single module can reach. - * - * @return the maximum module speed, in meters per second - */ - public abstract double getMaxModuleSpeed(); - - /** - * Returns the gyro heading. - * - * @return the heading - */ - public abstract Rotation2d getRotation2d(); - - /** - * The physics simulation, or null on a real robot or when no simulation config - * was given. - */ - private SimSwerveDrivetrain simDrivetrain; - - /** - * Builds the pose estimator and starts the simulation. Call this from the - * constructor of the - * concrete drivetrain, once its modules exist. - */ - protected void configureSwerveBase() { - configuredChassis = true; - - odometry = new SwerveDrivePoseEstimator( - getKinematics(), new Rotation2d(), modulesPositions, new Pose2d()); - - startSimulation(); - } - - /** - * Places the robot at a known pose. This is the reset to hand a path follower. - * - *

- * A path follower resetting the pose means "the robot is here now", so the - * simulated robot is - * placed there too. {@link #resetOdometry(Pose2d)} only corrects the estimate, - * and stays the one - * for anything that is a correction rather than a placement. - * - * @param pose where the robot is - */ - public void resetPose(Pose2d pose) { - // Deliberately not done inside resetOdometry itself, tempting as that is. - // resetHeading - // routes through it, and the MegaTag1 watchdog routes through resetHeading, so - // teleport - // there would let a vision correction shove the physics robot to match its own - // estimate - // and the watchdog could never be caught being wrong. - resetSimulatedPose(pose); - resetOdometry(pose); - } - - /** - * Describes this drivetrain to the physics simulation, or returns null to run - * without one. - * - *

- * This is the only thing a robot has to write to get a simulated drivetrain, - * and it lives in - * the robot project on purpose. Gear ratios and module positions belong next to - * the {@link - * SwerveModuleConfig} values they have to agree with: keeping them in a - * separate simulation - * project is how a drive ratio of 6.03 sat beside a real robot geared 7.03 for - * a whole season - * without anyone noticing. - * - * @return the simulation config, or null for no simulation - */ - protected DriveTrainSimulationConfig getSimulationConfig() { - return null; - } - - /** - * Returns the gyro the simulation should drive, or null to leave it alone. - * - * @return the Pigeon to simulate - */ - protected Pigeon2 getSimulationPigeon() { - return null; - } - - /** - * Returns where the physics says the robot really is, for comparing odometry - * against. - * - * @return the true pose, or empty on a real robot or without a simulation - * config - */ - public Optional getSimulatedPose() { - return Optional.ofNullable(simDrivetrain).map(SimSwerveDrivetrain::getPose); - } - - /** - * Teleports the simulated robot. Does nothing on a real one. - * - * @param pose where to place it - */ - public void resetSimulatedPose(Pose2d pose) { - if (simDrivetrain != null) { - simDrivetrain.resetPose(pose); - } - } - - // Guarded so the maple-sim classes are never touched on the roboRIO. Java has - // no preprocessor, - // so they are linked either way; what matters is that nothing constructs or - // steps them. - private void startSimulation() { - if (!RobotBase.isSimulation() || getSimulationConfig() == null) { - return; - } - - simDrivetrain = new SimSwerveDrivetrain(getSimulationConfig(), new Pose2d(), getSimulationPigeon()); - - // Same order as the module translations and the kinematics. - getFrontLeftModule().attachSimulation(simDrivetrain.getModule(0)); - getFrontRightModule().attachSimulation(simDrivetrain.getModule(1)); - getBackLeftModule().attachSimulation(simDrivetrain.getModule(2)); - getBackRightModule().attachSimulation(simDrivetrain.getModule(3)); - } - - @Override - public void simulationPeriodic() { - if (simDrivetrain != null) { - simDrivetrain.update(); - } - } - - /** - * Returns the front left module. - * - * @return the front left module - */ - protected abstract SwerveModule getFrontLeftModule(); - - /** - * Returns the front right module. - * - * @return the front right module - */ - protected abstract SwerveModule getFrontRightModule(); - - /** - * Returns the back left module. - * - * @return the back left module - */ - protected abstract SwerveModule getBackLeftModule(); - - /** - * Returns the back right module. - * - * @return the back right module - */ - protected abstract SwerveModule getBackRightModule(); - - /** - * Returns the drivetrain kinematics. - * - * @return the kinematics - */ - protected abstract SwerveDriveKinematics getKinematics(); + /** Whether {@link #configureSwerveBase()} has run. */ + protected boolean configuredChassis = false; + + /** Pose estimator fusing odometry and vision. */ + protected SwerveDrivePoseEstimator odometry; + + /** Latest module positions, in front-left, front-right, back-`left, back-right order. */ + protected SwerveModulePosition[] modulesPositions = + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + + /** Latest module states, in front-left, front-right, back-left, back-right order. */ + protected SwerveModuleState[] modulesStates = + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + + /** + * Returns the latest estimated pose. + * + * @return the estimated pose + */ + public abstract Pose2d getEstimatedPose(); + + /** + * Resets the pose estimator to a known pose. + * + * @param initPose the pose to reset to + */ + public abstract void resetOdometry(Pose2d initPose); + + /** + * Returns the current robot relative speeds. + * + * @return the current speeds + */ + public abstract ChassisSpeeds getCurrentSpeeds(); + + /** + * Requests new robot relative speeds. + * + * @param speeds the desired speeds + */ + public abstract void setTargetSpeeds(ChassisSpeeds speeds); + + /** + * Returns the maximum speed a single module can reach. + * + * @return the maximum module speed, in meters per second + */ + public abstract double getMaxModuleSpeed(); + + /** + * Returns the gyro heading. + * + * @return the heading + */ + public abstract Rotation2d getRotation2d(); + + /** The physics simulation, or null on a real robot or when no simulation config was given. */ + private SimSwerveDrivetrain simDrivetrain; + + /** + * Builds the pose estimator and starts the simulation. Call this from the constructor of the + * concrete drivetrain, once its modules exist. + */ + protected void configureSwerveBase() { + configuredChassis = true; + + odometry = + new SwerveDrivePoseEstimator( + getKinematics(), new Rotation2d(), modulesPositions, new Pose2d()); + + startSimulation(); + } + + /** + * Places the robot at a known pose. This is the reset to hand a path follower. + * + *

A path follower resetting the pose means "the robot is here now", so the simulated robot is + * placed there too. {@link #resetOdometry(Pose2d)} only corrects the estimate, and stays the one + * for anything that is a correction rather than a placement. + * + * @param pose where the robot is + */ + public void resetPose(Pose2d pose) { + // Deliberately not done inside resetOdometry itself, tempting as that is. + // resetHeading + // routes through it, and the MegaTag1 watchdog routes through resetHeading, so + // teleport + // there would let a vision correction shove the physics robot to match its own + // estimate + // and the watchdog could never be caught being wrong. + resetSimulatedPose(pose); + resetOdometry(pose); + } + + /** + * Describes this drivetrain to the physics simulation, or returns null to run without one. + * + *

This is the only thing a robot has to write to get a simulated drivetrain, and it lives in + * the robot project on purpose. Gear ratios and module positions belong next to the {@link + * SwerveModuleConfig} values they have to agree with: keeping them in a separate simulation + * project is how a drive ratio of 6.03 sat beside a real robot geared 7.03 for a whole season + * without anyone noticing. + * + * @return the simulation config, or null for no simulation + */ + protected DriveTrainSimulationConfig getSimulationConfig() { + return null; + } + + /** + * Returns the gyro the simulation should drive, or null to leave it alone. + * + * @return the Pigeon to simulate + */ + protected Pigeon2 getSimulationPigeon() { + return null; + } + + /** + * Returns where the physics says the robot really is, for comparing odometry against. + * + * @return the true pose, or empty on a real robot or without a simulation config + */ + public Optional getSimulatedPose() { + return Optional.ofNullable(simDrivetrain).map(SimSwerveDrivetrain::getPose); + } + + /** + * Teleports the simulated robot. Does nothing on a real one. + * + * @param pose where to place it + */ + public void resetSimulatedPose(Pose2d pose) { + if (simDrivetrain != null) { + simDrivetrain.resetPose(pose); + } + } + + // Guarded so the maple-sim classes are never touched on the roboRIO. Java has + // no preprocessor, + // so they are linked either way; what matters is that nothing constructs or + // steps them. + private void startSimulation() { + if (!RobotBase.isSimulation() || getSimulationConfig() == null) { + return; + } + + simDrivetrain = + new SimSwerveDrivetrain(getSimulationConfig(), new Pose2d(), getSimulationPigeon()); + + // Same order as the module translations and the kinematics. + getFrontLeftModule().attachSimulation(simDrivetrain.getModule(0)); + getFrontRightModule().attachSimulation(simDrivetrain.getModule(1)); + getBackLeftModule().attachSimulation(simDrivetrain.getModule(2)); + getBackRightModule().attachSimulation(simDrivetrain.getModule(3)); + } + + @Override + public void simulationPeriodic() { + if (simDrivetrain != null) { + simDrivetrain.update(); + } + } + + /** + * Returns the front left module. + * + * @return the front left module + */ + protected abstract SwerveModule getFrontLeftModule(); + + /** + * Returns the front right module. + * + * @return the front right module + */ + protected abstract SwerveModule getFrontRightModule(); + + /** + * Returns the back left module. + * + * @return the back left module + */ + protected abstract SwerveModule getBackLeftModule(); + + /** + * Returns the back right module. + * + * @return the back right module + */ + protected abstract SwerveModule getBackRightModule(); + + /** + * Returns the drivetrain kinematics. + * + * @return the kinematics + */ + protected abstract SwerveDriveKinematics getKinematics(); } From 5fe2356a3f4f58b61dc94a8af680205375f3cff7 Mon Sep 17 00:00:00 2001 From: Luis Leyva Date: Sun, 4 Oct 2026 19:12:26 -0600 Subject: [PATCH 3/3] Remove PathPlanner vendordep --- README.md | 17 ++---- build.gradle | 12 +--- .../lib/subsystems/swerve/SwerveBase.java | 17 ++---- .../Swerve/SwerveChassis/SwerveChassis.cpp | 2 +- .../Swerve/SwerveChassis/SwerveBase.h | 61 +++++++------------ vendordeps/PathplannerLib.json | 38 ------------ 6 files changed, 36 insertions(+), 111 deletions(-) delete mode 100644 vendordeps/PathplannerLib.json diff --git a/README.md b/README.md index 4f5b52a..89f1828 100644 --- a/README.md +++ b/README.md @@ -41,10 +41,10 @@ directories, lowercased: - **`SwerveBase` extends `SubsystemBase`.** C++ mixes the two in via multiple inheritance, which Java does not have, so the hierarchy is `SubsystemBase` → `SwerveBase` → `SwerveChassis` → your drivetrain. -- **Path following is wired up in the robot project.** The C++ `SwerveBase` configures - PathPlanner's `AutoBuilder` itself. The Java one does not, and has no PathPlanner - dependency at all, so a robot can follow paths with PathPlanner, BLine or anything else. - [documentation/swerve](documentation/swerve/README.md) has a worked example of each. +- **Path following is wired up in the robot project.** The library does not configure a + path follower or depend on one, in either language, so a robot can follow paths with + PathPlanner, BLine or anything else. + [documentation/swerve](documentation/swerve/README.md) has a worked Java example of each. - **Standard deviations are `Matrix`** (built with `VecBuilder.fill`) rather than the C++ `wpi::array`, because that is what the WPILib pose estimator takes. - `LimelightHelpers.java` is vendored from @@ -126,11 +126,6 @@ bumping a vendordep JSON is all that is needed — there is no second list to up Currently: WPILib New Commands, CTRE Phoenix 6, PhotonLib and maple-sim. -`vendordeps/PathplannerLib.json` is the one exception to "no second list". Only the C++ -sources still use PathPlanner, so `build.gradle` names that file as C++ only and keeps it -off the Java classpath. That is what stops a PathPlanner import from compiling in -`src/main/java`. - ### Robot projects install these themselves `OvertureLib.json` declares only the OvertureLib artifact, so none of the vendor libraries @@ -139,8 +134,8 @@ Phoenix 6 and PhotonLib of its own accord even though it reaches most of them th library rather than directly. A path following library is the robot's own choice and its own vendordep. Nothing in the -Java library needs one, so a robot that follows paths with BLine does not install -PathPlanner, and the other way round. +library needs one, so a robot that follows paths with BLine does not install PathPlanner, +and the other way round. maple-sim is simply the newest entry on that list, and the one a robot set up before 2026 will not have. Without it, simulation fails immediately. A real robot most likely runs diff --git a/build.gradle b/build.gradle index fe98928..c41729d 100644 --- a/build.gradle +++ b/build.gradle @@ -31,21 +31,11 @@ var javaVersion = "17" // Vendordep java artifacts are read straight out of vendordeps/*.json so that the // Java and C++ builds always consume the same vendor versions. A dependency whose // version is "wpilib" ships with WPILib itself and takes wpilibVersion. -// -// PathplannerLib is the exception. Only the C++ sources still use it, so its json -// stays in vendordeps/ for a -PjavaOnly=false build and is kept off the Java -// classpath. Path following is wired up in the robot project (see -// documentation/swerve), and a PathPlanner that is still importable from here is -// how it would grow back in without anyone deciding it should. -def cppOnlyVendordeps = ['PathplannerLib.json'] - ext.vendordepJava = { def repos = [] as LinkedHashSet def deps = [] as LinkedHashSet def slurper = new groovy.json.JsonSlurper() - file('vendordeps').listFiles().findAll { - it.name.endsWith('.json') && !(it.name in cppOnlyVendordeps) - }.sort().each { vendordep -> + file('vendordeps').listFiles().findAll {it.name.endsWith('.json')}.sort().each { vendordep -> def json = slurper.parse(vendordep) json.mavenUrls.each {repos.add(it)} json.javaDependencies.each { dep -> diff --git a/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java b/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java index 53898ec..61c03ad 100644 --- a/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java +++ b/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java @@ -36,7 +36,7 @@ public abstract class SwerveBase extends SubsystemBase { /** Pose estimator fusing odometry and vision. */ protected SwerveDrivePoseEstimator odometry; - /** Latest module positions, in front-left, front-right, back-`left, back-right order. */ + /** Latest module positions, in front-left, front-right, back-left, back-right order. */ protected SwerveModulePosition[] modulesPositions = new SwerveModulePosition[] { new SwerveModulePosition(), @@ -123,12 +123,9 @@ protected void configureSwerveBase() { * @param pose where the robot is */ public void resetPose(Pose2d pose) { - // Deliberately not done inside resetOdometry itself, tempting as that is. - // resetHeading - // routes through it, and the MegaTag1 watchdog routes through resetHeading, so - // teleport - // there would let a vision correction shove the physics robot to match its own - // estimate + // Deliberately not done inside resetOdometry itself, tempting as that is. resetHeading + // routes through it, and the MegaTag1 watchdog routes through resetHeading, so teleport + // there would let a vision correction shove the physics robot to match its own estimate // and the watchdog could never be caught being wrong. resetSimulatedPose(pose); resetOdometry(pose); @@ -178,10 +175,8 @@ public void resetSimulatedPose(Pose2d pose) { } } - // Guarded so the maple-sim classes are never touched on the roboRIO. Java has - // no preprocessor, - // so they are linked either way; what matters is that nothing constructs or - // steps them. + // Guarded so the maple-sim classes are never touched on the roboRIO. Java has no preprocessor, + // so they are linked either way; what matters is that nothing constructs or steps them. private void startSimulation() { if (!RobotBase.isSimulation() || getSimulationConfig() == null) { return; diff --git a/src/main/native/cpp/Subsystems/Swerve/SwerveChassis/SwerveChassis.cpp b/src/main/native/cpp/Subsystems/Swerve/SwerveChassis/SwerveChassis.cpp index 8a20673..c4c2fdd 100644 --- a/src/main/native/cpp/Subsystems/Swerve/SwerveChassis/SwerveChassis.cpp +++ b/src/main/native/cpp/Subsystems/Swerve/SwerveChassis/SwerveChassis.cpp @@ -8,7 +8,7 @@ /** * @brief Builds an object of swerve chassis */ -SwerveChassis::SwerveChassis() : SwerveBase(this) { +SwerveChassis::SwerveChassis() { // auto config = pathplanner::RobotConfig::fromGUISettings(); // m_setpointGenerator = pathplanner::SwerveSetpointGenerator(config, diff --git a/src/main/native/include/OvertureLib/Subsystems/Swerve/SwerveChassis/SwerveBase.h b/src/main/native/include/OvertureLib/Subsystems/Swerve/SwerveChassis/SwerveBase.h index 395c4d4..446531f 100644 --- a/src/main/native/include/OvertureLib/Subsystems/Swerve/SwerveChassis/SwerveBase.h +++ b/src/main/native/include/OvertureLib/Subsystems/Swerve/SwerveChassis/SwerveBase.h @@ -6,20 +6,25 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include #include - -#include -#include -#include -using namespace pathplanner; +#include +#include +#include // spotless:off +// Path following is not wired here. Whether a robot follows paths with PathPlanner, BLine or +// something else is the robot project's call, and all any of them need is already public: +// getEstimatedPose, resetPose, getCurrentSpeeds and setTargetSpeeds. class SwerveBase { public: - SwerveBase(frc2::Subsystem* driveSubsystem) : driveSubsystem(driveSubsystem) { - - } - virtual const frc::Pose2d& getEstimatedPose() = 0; virtual void resetOdometry(frc::Pose2d initPose) = 0; virtual frc::ChassisSpeeds getCurrentSpeeds() = 0; @@ -31,34 +36,17 @@ class SwerveBase { virtual frc::Rotation2d getRotation2d() = 0; virtual frc::Rotation3d getRotation3d() = 0; + // Places the robot at a known pose. This is the reset to hand a path follower: it also tells + // the simulator where the robot was put, on the topic it listens to. resetOdometry only + // corrects the estimate. + void resetPose(frc::Pose2d pose) { + resetOdometryPosePublisher.Set(pose); + resetOdometry(pose); + } + protected: void configureSwerveBase() { - - pathplanner::RobotConfig robotConfig = RobotConfig::fromGUISettings(); - configuredChassis = true; - AutoBuilder::configure( - [this]() {return getEstimatedPose();}, - [this](frc::Pose2d pose) { - resetOdometryPosePublisher.Set(pose); - resetOdometry(pose); - }, - [this]() {return getCurrentSpeeds();}, - [this](frc::ChassisSpeeds speeds) {setTargetSpeeds(speeds);}, - std::make_shared( - getTranslationPID(), - getRotationPID() - ), - robotConfig, - []() { - auto alliance = frc::DriverStation::GetAlliance(); - if (alliance) { - return alliance.value() == frc::DriverStation::Alliance::kRed; - } - return false; - }, - driveSubsystem - ); odometry = std::make_unique>(getKinematics(), frc::Rotation2d{}, modulesPositions, frc::Pose2d{}); } @@ -70,9 +58,6 @@ class SwerveBase { virtual frc::SwerveDriveKinematics<4>& getKinematics() = 0; - virtual PIDConstants getTranslationPID() = 0; - virtual PIDConstants getRotationPID() = 0; - bool configuredChassis = false; std::unique_ptr> odometry; @@ -84,8 +69,6 @@ class SwerveBase { frc::SwerveModuleState(), frc::SwerveModuleState() }; private: - frc2::Subsystem* driveSubsystem; - nt::StructPublisher resetOdometryPosePublisher = nt::NetworkTableInstance::GetDefault().GetStructTopic < frc::Pose2d >("/PathPlanner/ResetPose").Publish(); diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json deleted file mode 100644 index 4414707..0000000 --- a/vendordeps/PathplannerLib.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib.json", - "name": "PathplannerLib", - "version": "2026.1.2", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2026", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2026.1.2" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2026.1.2", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file