diff --git a/README.md b/README.md index 89f1828..936545d 100644 --- a/README.md +++ b/README.md @@ -124,7 +124,7 @@ 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, PhotonLib and maple-sim. +Currently: WPILib New Commands, CTRE Phoenix 6 and PhotonLib. ### Robot projects install these themselves @@ -137,8 +137,8 @@ A path following library is the robot's own choice and its own vendordep. Nothin 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 -anyway, because everything touching `org.ironmaple` sits behind a `RobotBase.isSimulation()` -check and the JVM resolves classes lazily — but that is how HotSpot happens to behave -rather than a guarantee, and it has not been tested. +maple-sim used to be on that list and no longer is: the library does not simulate the +drivetrain any more. A simulator that drives the devices themselves, such as WPI Emulator, +runs the unmodified robot code, and a second simulation inside the library would fight it +over the same motor and gyro states. A robot that still wants maple-sim installs it and +wires it up in its own project. diff --git a/build.gradle b/build.gradle index c41729d..9b92f78 100644 --- a/build.gradle +++ b/build.gradle @@ -90,7 +90,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, PhotonLib and maple-sim + // WPILibNewCommands, Phoenix 6 and PhotonLib vendordepJava.deps.each {implementation it} testImplementation 'org.junit.jupiter:junit-jupiter:5.13.4' diff --git a/documentation/swerve/README.md b/documentation/swerve/README.md index edb0660..dd80633 100644 --- a/documentation/swerve/README.md +++ b/documentation/swerve/README.md @@ -17,11 +17,11 @@ them as public methods: | 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. +**`resetPose` or `resetOdometry`?** Today they do the same thing, but they mean different +things. A path follower resetting the pose says "the robot is here now", while +`resetOdometry` is also what heading resets and vision corrections go through. Hand a +follower `resetPose`, so that anything that should happen on a placement and not on a +correction has one place to live. 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 @@ -57,7 +57,7 @@ They were compiled against PathPlannerLib 2026.1.2 and BLine-Lib v0.9.2. | `settings.json` | Value | Same number in `Chassis.java` | | --------------- | ----- | ----------------------------- | - | `robotMass` | 61.235 | `withRobotMass(Kilograms.of(61.235))` | + | `robotMass` | 61.235 | None: the chassis does not need the mass. Weigh the robot | | `driveGearing` | 7.03 | `kDriveGearRatio` | | `driveWheelRadius` | 0.0508 | `kWheelRadiusInches` (2 in) | | `maxDriveSpeed` | 4.541 | `getMaxModuleSpeed()` | @@ -153,7 +153,36 @@ robot written against those versions needs three changes when it updates: 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. + NetworkTables topic. It existed for the old external simulator. Robots that never ran an auto need only the first. + +## Updating a robot from 2026.4.0-beta-4 + +2026.4.0-beta-4 simulated the drivetrain with maple-sim. That is gone: a simulator that +drives the devices themselves, such as WPI Emulator, runs the unmodified robot code, and a +second simulation inside the library fought it over the same motor and gyro states. A +robot written against beta-4 needs these changes: + +1. **Delete `getSimulationConfig()` and `getSimulationPigeon()` from the chassis,** with + their `org.ironmaple` imports. They no longer override anything, so the `@Override` on + them stops compiling. +2. **Stop calling `getSimulatedPose()` and `resetSimulatedPose()`.** They are gone with the + simulation. `resetPose` stays, and is still the reset to hand a path follower. +3. **Remove the maple-sim vendordep,** unless the robot uses maple-sim on its own. + +Outside a simulator the drivetrain no longer moves in simulation, because nothing feeds +the motors' simulated sensors. + +Nothing else changes for the robot, but the drivetrain reads its sensors differently. The +chassis refreshes all four modules in one batched call per CAN bus at the start of its +`periodic()`, and a module owned by a chassis no longer reads its sensors again in its own +`periodic()`. A module's `getVoltageDrive()` returns the voltage as of that refresh. X mode +brakes through the drive requests rather than by rewriting the neutral mode config. + +Telemetry moved, with no values lost. The per-module `/Swerve/Modules//State`, +`/TargetState` and `/Distance` topics are gone; the same values are in the +`/Swerve/Modules/States`, `/Swerve/Modules/TargetStates` and `/Swerve/Modules/Positions` +arrays, indexed front left, front right, back left, back right. Point any dashboard or +AdvantageScope layout that used the old names at the arrays. `RequestedVoltage` and +`AppliedVoltage` stay per module. diff --git a/documentation/swerve/examples/BLinePaths.java b/documentation/swerve/examples/BLinePaths.java index a94e7a8..2b8165d 100644 --- a/documentation/swerve/examples/BLinePaths.java +++ b/documentation/swerve/examples/BLinePaths.java @@ -68,7 +68,7 @@ public Command follow(String pathName) { * @return the command */ public Command resetAndFollow(String pathName) { - // resetPose rather than resetOdometry: it places the simulated robot there as well. + // resetPose rather than resetOdometry: placing the robot is a reset, not a correction. return builder.withPoseReset(chassis::resetPose).build(new Path(pathName)); } } diff --git a/documentation/swerve/examples/Chassis.java b/documentation/swerve/examples/Chassis.java index 79bf67d..aadede3 100644 --- a/documentation/swerve/examples/Chassis.java +++ b/documentation/swerve/examples/Chassis.java @@ -4,13 +4,7 @@ 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; @@ -20,11 +14,7 @@ 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}. @@ -37,7 +27,7 @@ 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. */ + /** Drive reduction, shared by the four module configs below. */ private static final double kDriveGearRatio = 7.03; /** Steer reduction, likewise shared. */ @@ -86,46 +76,6 @@ public Chassis() { 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; diff --git a/documentation/swerve/examples/PathPlannerConfig.java b/documentation/swerve/examples/PathPlannerConfig.java index fa766e3..c193136 100644 --- a/documentation/swerve/examples/PathPlannerConfig.java +++ b/documentation/swerve/examples/PathPlannerConfig.java @@ -46,7 +46,7 @@ public static void configure(Chassis chassis) { 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. + // here now", which is a placement, not a correction. chassis::resetPose, chassis::getCurrentSpeeds, // Robot relative. The parameter type is spelled out to pick the overload that takes the diff --git a/src/main/java/com/overture/lib/motorcontrollers/OverTalonFX.java b/src/main/java/com/overture/lib/motorcontrollers/OverTalonFX.java index 266103c..e42b69c 100644 --- a/src/main/java/com/overture/lib/motorcontrollers/OverTalonFX.java +++ b/src/main/java/com/overture/lib/motorcontrollers/OverTalonFX.java @@ -44,27 +44,27 @@ public OverTalonFX(TalonFXConfiguration config, int id, CANBus bus) { } /** - * Sets the sensor to mechanism ratio of the TalonFX. + * Sets the sensor to mechanism ratio of the TalonFX. Writes the feedback configs. * * @param gearRatio the gear ratio between the sensor and the mechanism */ public void setSensorToMechanism(double gearRatio) { ctreConfig.Feedback.withSensorToMechanismRatio(gearRatio); - getConfigurator().apply(ctreConfig); + getConfigurator().apply(ctreConfig.Feedback); } /** - * Sets the rotor to sensor ratio of the TalonFX. + * Sets the rotor to sensor ratio of the TalonFX. Writes the feedback configs. * * @param gearRatio the gear ratio between the rotor and the sensor */ public void setRotorToSensorRatio(double gearRatio) { ctreConfig.Feedback.withRotorToSensorRatio(gearRatio); - getConfigurator().apply(ctreConfig); + getConfigurator().apply(ctreConfig.Feedback); } /** - * Uses a remote CANcoder as the feedback source. + * Uses a remote CANcoder as the feedback source. Writes the feedback configs. * * @param id the device id of the remote CANcoder */ @@ -73,11 +73,11 @@ public void setRemoteCANCoder(int id) { .Feedback .withFeedbackRemoteSensorID(id) .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder); - getConfigurator().apply(ctreConfig); + getConfigurator().apply(ctreConfig.Feedback); } /** - * Uses a fused CANcoder as the feedback source. + * Uses a fused CANcoder as the feedback source. Writes the feedback configs. * * @param id the device id of the fused CANcoder */ @@ -86,11 +86,11 @@ public void setFusedCANCoder(int id) { .Feedback .withFeedbackRemoteSensorID(id) .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder); - getConfigurator().apply(ctreConfig); + getConfigurator().apply(ctreConfig.Feedback); } /** - * Uses a synced CANcoder as the feedback source. + * Uses a synced CANcoder as the feedback source. Writes the feedback configs. * * @param id the device id of the sync CANcoder */ @@ -99,21 +99,21 @@ public void setSyncCANCoder(int id) { .Feedback .withFeedbackRemoteSensorID(id) .withFeedbackSensorSource(FeedbackSensorSourceValue.SyncCANcoder); - getConfigurator().apply(ctreConfig); + getConfigurator().apply(ctreConfig.Feedback); } /** - * Sets the closed loop torque ramp rate of the TalonFX. + * Sets the closed loop torque ramp rate of the TalonFX. Writes the closed loop ramp configs. * * @param rampSeconds the ramp period, in seconds */ public void setClosedLoopTorqueRamp(double rampSeconds) { ctreConfig.ClosedLoopRamps.withTorqueClosedLoopRampPeriod(rampSeconds); - getConfigurator().apply(ctreConfig); + getConfigurator().apply(ctreConfig.ClosedLoopRamps); } /** - * Sets the torque current limits of the TalonFX. + * Sets the torque current limits of the TalonFX. Writes the torque current configs. * * @param peakForward the peak forward torque current, in amps * @param peakBackward the peak reverse torque current, in amps @@ -125,7 +125,7 @@ public void setTorqueCurrentLimit(double peakForward, double peakBackward, doubl .withPeakForwardTorqueCurrent(peakForward) .withPeakReverseTorqueCurrent(peakBackward) .withTorqueNeutralDeadband(deadband); - getConfigurator().apply(ctreConfig); + getConfigurator().apply(ctreConfig.TorqueCurrent); } /** @@ -163,11 +163,12 @@ public void configureMotionMagic(double cruiseVelocity, double acceleration, dou .withMotionMagicAcceleration(acceleration) .withMotionMagicJerk(jerk); - getConfigurator().apply(ctreConfig); + getConfigurator().apply(ctreConfig.MotionMagic); } /** - * Configures the software limit switches of the TalonFX. + * Configures the software limit switches of the TalonFX. Writes the software limit switch + * configs. * * @param configs the software limit switch configuration */ @@ -175,11 +176,12 @@ public void configureSoftwareLimitSwitch(SoftwareLimitSwitchConfigs configs) { // Cloned for the same reason as the slot 0 gains: the C++ took this parameter // by value. ctreConfig.withSoftwareLimitSwitch(configs.clone()); - getConfigurator().apply(ctreConfig); + getConfigurator().apply(ctreConfig.SoftwareLimitSwitch); } /** - * Sets the neutral mode, keeping the stored configuration in step with the device. + * Sets the neutral mode, keeping the stored configuration in step with the device. Writes the + * motor output configs. * * @param mode the neutral mode to apply */ @@ -188,10 +190,13 @@ public void configureNeutralMode(NeutralModeValue mode) { getConfigurator().apply(ctreConfig.MotorOutput); } - /** Makes the closed loop take the shortest path across the rotation wrap point. */ + /** + * Makes the closed loop take the shortest path across the rotation wrap point. Writes the closed + * loop general configs. + */ public void setContinuousWrap() { ctreConfig.ClosedLoopGeneral.ContinuousWrap = true; - getConfigurator().apply(ctreConfig); + getConfigurator().apply(ctreConfig.ClosedLoopGeneral); } /** diff --git a/src/main/java/com/overture/lib/simulation/SimPhotonVisionManager.java b/src/main/java/com/overture/lib/simulation/SimPhotonVisionManager.java index a870160..b1c1c07 100644 --- a/src/main/java/com/overture/lib/simulation/SimPhotonVisionManager.java +++ b/src/main/java/com/overture/lib/simulation/SimPhotonVisionManager.java @@ -59,10 +59,9 @@ private StructSubscriber simulatedDriveTrainPoseEntry() { /** * Tells the manager where the simulated robot is. * - *

Pass {@code chassis::getSimulatedPose} when the physics runs in this process. The - * NetworkTables fallback below only exists for an external simulator publishing the AdvantageKit - * field pose, which is what this used to depend on; that path lags by roughly a tenth of a second - * and disappears entirely once the drivetrain simulates itself. + *

Pass a supplier of the true pose when something in this process knows it. The NetworkTables + * fallback below only exists for an external simulator publishing the AdvantageKit field pose, + * which is what this used to depend on; that path lags by roughly a tenth of a second. * * @param poseSupplier supplies the true robot pose, empty when it is not known yet */ diff --git a/src/main/java/com/overture/lib/simulation/mechanisms/SimMechanism.java b/src/main/java/com/overture/lib/simulation/mechanisms/SimMechanism.java index f8bcfcf..223afd9 100644 --- a/src/main/java/com/overture/lib/simulation/mechanisms/SimMechanism.java +++ b/src/main/java/com/overture/lib/simulation/mechanisms/SimMechanism.java @@ -14,10 +14,10 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import java.util.ArrayList; import java.util.List; -import org.ironmaple.simulation.motorsims.SimulatedBattery; /** * A simulated mechanism bolted to the robot. @@ -88,10 +88,9 @@ protected final double getTimeStep() { */ protected final double getAppliedVolts() { TalonFXSimState sim = motor.getSimState(); - // maple-sim owns the battery model, and asking it directly works before the arena has stepped - // even once. The arena does mirror its result onto RoboRIO input voltage every sub-tick, but - // only once it is running, and a mechanism can be spinning before then. - sim.setSupplyVoltage(SimulatedBattery.getBatteryVoltage()); + // The robot's own battery reading: a steady 12 V in a plain simulation, and the sagging battery + // of whatever simulator drives the HAL, so a mechanism slows with the rest of the robot. + sim.setSupplyVoltage(RobotController.getBatteryVoltage()); return sim.getMotorVoltage(); } diff --git a/src/main/java/com/overture/lib/simulation/swerve/SimSwerveDrivetrain.java b/src/main/java/com/overture/lib/simulation/swerve/SimSwerveDrivetrain.java deleted file mode 100644 index 3dc5c01..0000000 --- a/src/main/java/com/overture/lib/simulation/swerve/SimSwerveDrivetrain.java +++ /dev/null @@ -1,99 +0,0 @@ -// 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 com.overture.lib.simulation.swerve; - -import static edu.wpi.first.units.Units.DegreesPerSecond; - -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.sim.Pigeon2SimState; -import edu.wpi.first.math.geometry.Pose2d; -import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; -import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; - -/** - * Runs a maple-sim drivetrain in the same process as the robot code. - * - *

This used to be a separate program talking over NetworkTables, because the library was C++ and - * maple-sim is Java only. NetworkTables was the language bridge, not a design choice, and it cost - * roughly 100 ms of lag, a startup race that silently corrupted runs, and timestamps in the wrong - * time base. With both sides in Java the physics can simply run here. - * - *

Nothing constructs this on a real robot: {@link com.overture.lib.subsystems.swerve.SwerveBase} - * only builds it when {@code RobotBase.isSimulation()} is true and the drivetrain has supplied a - * simulation config, so the maple-sim classes are never touched on the roboRIO. - */ -public class SimSwerveDrivetrain { - private final SwerveDriveSimulation drivetrain; - private final Pigeon2SimState pigeonSim; - - /** - * Builds the simulated drivetrain and registers it with the arena. - * - * @param config the drivetrain the robot is simulating, supplied by the robot project because - * gear ratios and module positions belong beside the real ones - * @param startingPose where the robot starts on the field - * @param pigeon the gyro to drive from the simulated one, or null to leave it alone - */ - public SimSwerveDrivetrain( - DriveTrainSimulationConfig config, Pose2d startingPose, Pigeon2 pigeon) { - drivetrain = new SwerveDriveSimulation(config, startingPose); - pigeonSim = pigeon == null ? null : pigeon.getSimState(); - SimulatedArena.getInstance().addDriveTrainSimulation(drivetrain); - } - - /** - * Returns one of the four simulated modules. - * - *

The order matches the module translations the config was built with, which is the same order - * the kinematics use: front left, front right, back left, back right. - * - * @param index the module index, 0 to 3 - * @return the simulated module - */ - public SwerveModuleSimulation getModule(int index) { - return drivetrain.getModules()[index]; - } - - /** - * Steps the physics and pushes the result into the gyro. - * - *

Called once per robot loop rather than from a timer, so the physics and the code that reads - * it advance together instead of racing. - */ - public void update() { - SimulatedArena.getInstance().simulationPeriodic(); - - if (pigeonSim != null) { - var gyro = drivetrain.getGyroSimulation(); - pigeonSim.setRawYaw(gyro.getGyroReading().getMeasure()); - pigeonSim.setAngularVelocityZ( - DegreesPerSecond.of(gyro.getMeasuredAngularVelocity().in(DegreesPerSecond))); - } - } - - /** - * Returns where the physics says the robot actually is. - * - *

This is ground truth, not an estimate. Comparing it against the pose estimator is how you - * tell whether odometry is working; feeding it back into the robot as if it were a measurement - * would hide exactly the bugs the simulation exists to find. - * - * @return the true field pose - */ - public Pose2d getPose() { - return drivetrain.getSimulatedDriveTrainPose(); - } - - /** - * Teleports the simulated robot. - * - * @param pose where to place it - */ - public void resetPose(Pose2d pose) { - drivetrain.setSimulationWorldPose(pose); - } -} 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 61c03ad..182215b 100644 --- a/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java +++ b/src/main/java/com/overture/lib/subsystems/swerve/SwerveBase.java @@ -4,8 +4,7 @@ package com.overture.lib.subsystems.swerve; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.overture.lib.simulation.swerve.SimSwerveDrivetrain; +import com.ctre.phoenix6.BaseStatusSignal; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -13,19 +12,21 @@ 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.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.Optional; -import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import java.util.Arrays; /** - * 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 batched sensor reads. * *

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. * + *

Nor is the drivetrain simulated here. A simulator that drives the devices themselves, such as + * WPI Emulator, runs the unmodified robot code; a second simulation in here would fight it over the + * same motor and gyro states. + * *

The C++ version mixes this in alongside SubsystemBase; Java has no multiple inheritance, so * this class extends SubsystemBase directly and {@link SwerveChassis} extends it. */ @@ -54,6 +55,13 @@ public abstract class SwerveBase extends SubsystemBase { new SwerveModuleState() }; + /** + * The sensors of all four modules, refreshed once a loop: one group when the modules share a CAN + * bus, one per module when they do not, because refreshAll refreshes nothing at all when handed + * signals from two buses. + */ + private BaseStatusSignal[][] moduleSignalGroups = new BaseStatusSignal[0][]; + /** * Returns the latest estimated pose. * @@ -96,107 +104,68 @@ public abstract class SwerveBase extends SubsystemBase { */ 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. + * Builds the pose estimator and takes over reading the modules' sensors. Call this from the + * constructor of the concrete drivetrain, once its modules exist. */ protected void configureSwerveBase() { + if (!configuredChassis) { + SwerveModule[] modules = { + getFrontLeftModule(), getFrontRightModule(), getBackLeftModule(), getBackRightModule() + }; + moduleSignalGroups = new BaseStatusSignal[modules.length][]; + boolean oneBus = true; + for (int i = 0; i < modules.length; i++) { + moduleSignalGroups[i] = modules[i].handInputsToChassis(); + oneBus &= modules[i].getCanBus().equals(modules[0].getCanBus()); + } + if (oneBus) { + moduleSignalGroups = + new BaseStatusSignal[][] { + Arrays.stream(moduleSignalGroups) + .flatMap(Arrays::stream) + .toArray(BaseStatusSignal[]::new) + }; + } + } 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. + * Refreshes the sensors of all four modules and reads them into their cached states and + * positions. * - *

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 + *

With every module on one bus, a single non-blocking native call covers them all, where + * reading each signal on its own took one call per signal. Modules split across buses take one + * call each. */ - 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); - } + protected void updateModuleInputs() { + for (BaseStatusSignal[] group : moduleSignalGroups) { + BaseStatusSignal.refreshAll(group); + } - /** - * 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; + getFrontLeftModule().readInputs(); + getFrontRightModule().readInputs(); + getBackLeftModule().readInputs(); + getBackRightModule().readInputs(); } /** - * 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. + * Places the robot at a known pose. This is the reset to hand a path follower. * - * @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. + *

Today this is {@link #resetOdometry(Pose2d)} by another name. It stays separate because the + * two calls mean different things: a path follower resetting the pose says "the robot is here + * now", while heading resets and vision corrections also go through resetOdometry. Anything that + * has to happen on a placement but not on a correction belongs here. * - * @param pose where to place it + * @param pose where the robot is */ - 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(); - } + public void resetPose(Pose2d pose) { + resetOdometry(pose); } /** 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 4bd6f64..5e424f2 100644 --- a/src/main/java/com/overture/lib/subsystems/swerve/SwerveChassis.java +++ b/src/main/java/com/overture/lib/subsystems/swerve/SwerveChassis.java @@ -8,7 +8,6 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; -import com.ctre.phoenix6.signals.NeutralModeValue; import com.overture.lib.math.ChassisAccels; import com.overture.lib.robots.RobotConstants; import com.overture.lib.utils.Logging; @@ -37,6 +36,10 @@ public abstract class SwerveChassis extends SwerveBase { private ChassisSpeeds lastSpeeds = new ChassisSpeeds(); private ChassisAccels currentAccels = new ChassisAccels(); + // Reused every loop for the TargetStates log, in front-left, front-right, back-left, back-right + // order like every other list of four. + private final SwerveModuleState[] targetStates = new SwerveModuleState[4]; + // Thresholds are a starting point, not a tuned value: 1.3 means the fastest // module may read 30% // above the slowest before anyone is accused, and below 0.3 m/s of translation @@ -260,17 +263,12 @@ public void setXMode(boolean enabled) { } xModeEnabled = enabled; - if (enabled) { - getFrontLeftModule().setDriveNeutralMode(NeutralModeValue.Brake); - getFrontRightModule().setDriveNeutralMode(NeutralModeValue.Brake); - getBackLeftModule().setDriveNeutralMode(NeutralModeValue.Brake); - getBackRightModule().setDriveNeutralMode(NeutralModeValue.Brake); - } else { - getFrontLeftModule().restoreDriveNeutralMode(); - getFrontRightModule().restoreDriveNeutralMode(); - getBackLeftModule().restoreDriveNeutralMode(); - getBackRightModule().restoreDriveNeutralMode(); - } + // Braked through the drive requests rather than the neutral mode config. Writing the config + // blocked the loop on four motor acknowledgements every time X mode toggled. + getFrontLeftModule().setDriveBrakeOverride(enabled); + getFrontRightModule().setDriveBrakeOverride(enabled); + getBackLeftModule().setDriveBrakeOverride(enabled); + getBackRightModule().setDriveBrakeOverride(enabled); } @Override @@ -424,8 +422,13 @@ public void shuffleboardPeriodic() { Logging.logBoolean( "/Swerve/Chassis/AcceptingVision", acceptingVisionMeasurements, Logging.Destination.BOTH); - // The array of measured states is what AdvantageScope's swerve view consumes. + // The arrays of measured and target states are what AdvantageScope's swerve view consumes. Logging.logModuleStates("/Swerve/Modules/States", modulesStates, Logging.Destination.BOTH); + targetStates[0] = getFrontLeftModule().getTargetState(); + targetStates[1] = getFrontRightModule().getTargetState(); + targetStates[2] = getBackLeftModule().getTargetState(); + targetStates[3] = getBackRightModule().getTargetState(); + Logging.logModuleStates("/Swerve/Modules/TargetStates", targetStates, Logging.Destination.BOTH); Logging.logModulePositions( "/Swerve/Modules/Positions", modulesPositions, Logging.Destination.LOG_ONLY); @@ -441,10 +444,7 @@ public void periodic() { throw new IllegalStateException("Have not called SwerveBase.configureSwerveBase!!!"); } - getFrontLeftModule().updateInputs(); - getFrontRightModule().updateInputs(); - getBackLeftModule().updateInputs(); - getBackRightModule().updateInputs(); + updateModuleInputs(); if (characterizing) { return; diff --git a/src/main/java/com/overture/lib/subsystems/swerve/SwerveModule.java b/src/main/java/com/overture/lib/subsystems/swerve/SwerveModule.java index fa984f2..1252dc5 100644 --- a/src/main/java/com/overture/lib/subsystems/swerve/SwerveModule.java +++ b/src/main/java/com/overture/lib/subsystems/swerve/SwerveModule.java @@ -4,13 +4,14 @@ package com.overture.lib.subsystems.swerve; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.sim.ChassisReference; import com.overture.lib.motorcontrollers.OverTalonFX; import com.overture.lib.sensors.OverCANCoder; import com.overture.lib.utils.Logging; @@ -18,9 +19,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; -import org.ironmaple.simulation.motorsims.SimulatedBattery; /** A single swerve module: one drive motor, one turn motor and one absolute encoder. */ public class SwerveModule extends SubsystemBase { @@ -33,6 +35,20 @@ public class SwerveModule extends SubsystemBase { // Declaration of sensors private final OverCANCoder canCoder; + // Sensor signals, looked up once. A device getter such as getPosition() looks its signal up again + // and refreshes it with a native call every time it is called; these are refreshed in one batch + // instead, by the chassis or by updateInputs(). + private final StatusSignal drivePositionSignal; + private final StatusSignal driveVelocitySignal; + private final StatusSignal driveVoltageSignal; + private final StatusSignal turnAngleSignal; + + // The four above, for BaseStatusSignal.refreshAll. All on config.CanBus, as refreshAll requires. + private final BaseStatusSignal[] inputSignals; + + // Set once a chassis has taken over refreshing this module's sensors. + private boolean updatedByChassis = false; + // FeedForward private final SimpleMotorFeedforward feedForward; @@ -50,6 +66,16 @@ public class SwerveModule extends SubsystemBase { private final VoltageOut driveVoltage = new VoltageOut(0); private final VoltageOut turnVoltageOut = new VoltageOut(0); + // Static-brakes the drive motor while its output is neutral, whatever its neutral mode config. + private boolean driveBrakeOverride = false; + + // The feedforward of the latest setState, kept for telemetry so it is computed once a loop. + private double requestedVolts = 0.0; + + // Telemetry paths, built once instead of concatenated every loop. + private final String requestedVoltagePath; + private final String appliedVoltagePath; + /** * Constructs a SwerveModule. * @@ -80,6 +106,19 @@ public SwerveModule(SwerveModuleConfig config) { turnMotor.setPositionUpdateFrequency(200); canCoder.getPosition().setUpdateFrequency(200); driveMotor.setVelocityUpdateFrequency(200); + + drivePositionSignal = driveMotor.getPosition(false); + driveVelocitySignal = driveMotor.getVelocity(false); + driveVoltageSignal = driveMotor.getMotorVoltage(false); + turnAngleSignal = canCoder.getAbsolutePosition(false); + inputSignals = + new BaseStatusSignal[] { + drivePositionSignal, driveVelocitySignal, driveVoltageSignal, turnAngleSignal + }; + + String base = "/Swerve/Modules/" + this.config.ModuleName; + requestedVoltagePath = base + "/RequestedVoltage"; + appliedVoltagePath = base + "/AppliedVoltage"; } /** @@ -128,12 +167,14 @@ public void setState(SwerveModuleState state) { // second, not rotor. double wheelRotationsPerSecond = targetState.speedMetersPerSecond / (Math.PI * config.WheelDiameter); + requestedVolts = feedForward.calculate(targetState.speedMetersPerSecond); driveMotor.setControl( driveVelocity .withVelocity(wheelRotationsPerSecond) - .withFeedForward(feedForward.calculate(targetState.speedMetersPerSecond)) + .withFeedForward(requestedVolts) .withEnableFOC(config.useFOCDrive) + .withOverrideBrakeDurNeutral(driveBrakeOverride) .withSlot(0)); } @@ -156,151 +197,127 @@ public void setVoltageDrive(double volts) { } /** - * Returns the voltage currently applied to the drive motor. + * Returns the voltage applied to the drive motor as of the last sensor refresh, which a chassis + * does at the start of every loop. * * @return the drive voltage, in volts */ public double getVoltageDrive() { - return driveMotor.getMotorVoltage().getValueAsDouble(); + return driveVoltageSignal.getValueAsDouble(); } /** * Sets the neutral mode of the drive motor. * + *

This writes the motor's configuration, which blocks until the motor acknowledges it. Keep it + * out of anything that runs every loop. + * * @param mode the neutral mode to apply */ public void setDriveNeutralMode(NeutralModeValue mode) { driveMotor.configureNeutralMode(mode); } - /** Restores the drive motor to the neutral mode it was configured with. */ + /** + * Restores the drive motor to the neutral mode it was configured with. Blocks like {@link + * #setDriveNeutralMode(NeutralModeValue)}. + */ public void restoreDriveNeutralMode() { driveMotor.configureNeutralMode(config.DriveMotorConfig.MotorOutput.NeutralMode); } /** - * Wires this module's motors to a simulated one, so the physics drives the encoders and the - * motors' own closed loops drive the physics. - * - *

maple-sim asks a controller for the voltage it is commanding and hands back where the - * mechanism actually ended up. Answering with the real {@link - * com.ctre.phoenix6.sim.TalonFXSimState} means the control loops under test are the ones that run - * on the robot, rather than a stand-in PID controller that only behaves similarly. + * Static-brakes the drive motor whenever its output is neutral, whatever its neutral mode is + * configured to. * - *

Every value crossing this boundary is a unit-carrying Measure, not a bare double. The - * Phoenix simulation setters take rotations in some places and degrees in others, and passing - * doubles is how a heading ends up multiplied by 360. + *

This is how X mode holds the robot. It used to switch the neutral mode config on all four + * drive motors, and a config write blocks the loop until the motor acknowledges it, so every + * toggle cost four of those. This rides on the drive request instead and takes effect with the + * next {@link #setState(SwerveModuleState)}. * - * @param moduleSim the simulated module to attach to + * @param brake whether to brake while the output is neutral */ - public void attachSimulation(SwerveModuleSimulation moduleSim) { - driveMotor.getSimState().Orientation = - simOrientation(driveMotor.getCTREConfig().MotorOutput.Inverted); - turnMotor.getSimState().Orientation = - simOrientation(turnMotor.getCTREConfig().MotorOutput.Inverted); - - // Phoenix subtracts this from whatever we hand setRawPosition below, cancelling the magnet - // offset the encoder applies on the way out. Without it every module reads its true angle plus - // its calibration, so the modules point wrong by the four numbers a team measured on the real - // robot -- in simulation only, which invites zeroing the offsets for simulation and calling it - // fixed. - canCoder.getSimState().SensorOffset = canCoder.getConfiguration().MagnetSensor.MagnetOffset; - - moduleSim.useDriveMotorController( - (mechanismAngle, mechanismVelocity, encoderAngle, encoderVelocity) -> { - var sim = driveMotor.getSimState(); - sim.setSupplyVoltage(SimulatedBattery.getBatteryVoltage()); - sim.setRawRotorPosition(encoderAngle); - sim.setRotorVelocity(encoderVelocity); - return sim.getMotorVoltageMeasure(); - }); - - moduleSim.useSteerMotorController( - (mechanismAngle, mechanismVelocity, encoderAngle, encoderVelocity) -> { - // The CANcoder is fused to the turn motor and reads the module's true facing, so it gets - // the mechanism side of the gearbox while the motor gets the rotor side. - var encoderSim = canCoder.getSimState(); - encoderSim.setRawPosition(mechanismAngle); - encoderSim.setVelocity(mechanismVelocity); - - var sim = turnMotor.getSimState(); - sim.setSupplyVoltage(SimulatedBattery.getBatteryVoltage()); - sim.setRawRotorPosition(encoderAngle); - sim.setRotorVelocity(encoderVelocity); - return sim.getMotorVoltageMeasure(); - }); + void setDriveBrakeOverride(boolean brake) { + driveBrakeOverride = brake; } /** - * Maps a motor's software inversion onto its physical orientation for the simulation. + * Returns the state the module was last asked for, after optimizing and cosine scaling. * - *

Phoenix documents Orientation as describing the mechanical linkage and warns against setting - * it from the invert. The two are the same fact on a robot configured the usual way: a module is - * marked inverted precisely because its motor drives the wheel backwards. Tying them together - * beats asking for the same fact twice, which is how the two answers end up disagreeing. - * - * @param inverted the motor's configured output inversion - * @return the matching simulation orientation + * @return the target state */ - private static ChassisReference simOrientation(InvertedValue inverted) { - return inverted == InvertedValue.Clockwise_Positive - ? ChassisReference.Clockwise_Positive - : ChassisReference.CounterClockwise_Positive; + public SwerveModuleState getTargetState() { + return targetState; } /** - * Publishes module telemetry, recorded and live. + * Publishes what only this module knows: the feedforward it asked for and the voltage applied. * - *

Speed and angle travel together as a SwerveModuleState rather than as four loose numbers, - * which halves the signal count and lets AdvantageScope draw the module instead of plotting it. + *

Its measured and target states and its distance are logged by the chassis, as the + * /Swerve/Modules/States, TargetStates and Positions arrays (front left, front right, back left, + * back right). Logging them here as well wrote every value twice. */ public void shuffleboardPeriodic() { - String base = "/Swerve/Modules/" + config.ModuleName; - - Logging.logStruct( - base + "/TargetState", SwerveModuleState.struct, targetState, Logging.Destination.BOTH); - Logging.logStruct( - base + "/State", SwerveModuleState.struct, latestState, Logging.Destination.BOTH); - - Logging.logDouble( - base + "/Distance", latestPosition.distanceMeters, "m", Logging.Destination.LOG_ONLY); - Logging.logDouble( - base + "/RequestedVoltage", - feedForward.calculate(targetState.speedMetersPerSecond), - "V", - Logging.Destination.LOG_ONLY); - Logging.logDouble( - base + "/AppliedVoltage", - driveMotor.getMotorVoltage().getValueAsDouble(), - "V", - Logging.Destination.LOG_ONLY); + Logging.logDouble(requestedVoltagePath, requestedVolts, "V", Logging.Destination.LOG_ONLY); + Logging.logDouble(appliedVoltagePath, getVoltageDrive(), "V", Logging.Destination.LOG_ONLY); } /** - * Reads the sensors into the cached state that {@link #getState()} and {@link #getPosition()} - * serve. + * Refreshes this module's sensors and reads them into the cached state that {@link #getState()} + * and {@link #getPosition()} serve. * - *

The chassis calls this first thing in its own periodic. Doing it here rather than only in - * {@link #periodic()} is what keeps the ordering explicit: a module registers with the scheduler - * after the chassis does, so left to itself it would update after everything had already read it. + *

A chassis does not call this. It refreshes all four modules in one batch and then reads each + * one, which is the same work in one native call instead of one per signal. */ public void updateInputs() { - Rotation2d angle = Rotation2d.fromRotations(canCoder.getAbsolutePosition().getValueAsDouble()); + BaseStatusSignal.refreshAll(inputSignals); + readInputs(); + } + + /** + * Hands this module's sensors to a chassis, which refreshes them along with every other module's + * and then calls {@link #readInputs()}. From then on {@link #periodic()} leaves them alone. + * + * @return the signals to refresh, all on {@link #getCanBus()} + */ + BaseStatusSignal[] handInputsToChassis() { + updatedByChassis = true; + return inputSignals; + } + + /** + * Returns the CAN bus all of this module's devices are on. + * + * @return the module's CAN bus + */ + CANBus getCanBus() { + return config.CanBus; + } + + /** + * Reads the already refreshed sensors into the cached state and position. No native calls: a + * signal's value is a plain field once it has been refreshed. + */ + void readInputs() { + Rotation2d angle = Rotation2d.fromRotations(turnAngleSignal.getValueAsDouble()); latestState.speedMetersPerSecond = - driveMotor.getVelocity().getValueAsDouble() * config.WheelDiameter * Math.PI; + driveVelocitySignal.getValueAsDouble() * config.WheelDiameter * Math.PI; latestState.angle = angle; latestPosition.distanceMeters = - driveMotor.getPosition().getValueAsDouble() * config.WheelDiameter * Math.PI; + drivePositionSignal.getValueAsDouble() * config.WheelDiameter * Math.PI; latestPosition.angle = angle; } @Override public void periodic() { - // Harmless second read on a loop the chassis already served: these are cached signal values, - // not - // bus traffic. It is here so a module built outside a chassis still updates itself. - updateInputs(); + // A chassis has already refreshed and read this module by the time this runs, as its first + // step every loop. Reading again here used to cost three more native calls per module per loop + // for the values the chassis had just read. A module built outside a chassis still updates + // itself. + if (!updatedByChassis) { + updateInputs(); + } } } diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json deleted file mode 100644 index c73bea4..0000000 --- a/vendordeps/maple-sim.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "fileName": "maple-sim.json", - "name": "maplesim", - "version": "0.4.0-beta", - "frcYear": "2026", - "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", - "mavenUrls": [ - "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases", - "https://repo1.maven.org/maven2" - ], - "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", - "javaDependencies": [ - { - "groupId": "org.ironmaple", - "artifactId": "maplesim-java", - "version": "0.4.0-beta" - }, - { - "groupId": "org.dyn4j", - "artifactId": "dyn4j", - "version": "5.0.2" - } - ], - "jniDependencies": [], - "cppDependencies": [] -} \ No newline at end of file