From 4336723c8567f2bb35f599dd7519cbcba8ac81fb Mon Sep 17 00:00:00 2001 From: Glutinfree Date: Thu, 20 Aug 2026 19:24:08 -0700 Subject: [PATCH 1/5] added littleton swerve code and cleaned up imports --- .../first/robot/subsystems/drive/Drive.java | 235 +++++++++ .../subsystems/drive/DriveConstants.java | 96 ++++ .../first/robot/subsystems/drive/GyroIO.java | 22 + .../subsystems/drive/GyroIOOnboardIMU.java | 22 + .../first/robot/subsystems/drive/Module.java | 115 +++++ .../robot/subsystems/drive/ModuleIO.java | 49 ++ .../robot/subsystems/drive/ModuleIOSim.java | 105 ++++ .../subsystems/drive/ModuleIOTalonFX.java | 205 ++++++++ vendordeps/AdvantageKit.json | 35 ++ vendordeps/PathplannerLibSystemCoreAlpha.json | 37 ++ vendordeps/Phoenix6-26.50.0-alpha-1.json | 449 ++++++++++++++++++ 11 files changed, 1370 insertions(+) create mode 100644 src/main/java/first/robot/subsystems/drive/Drive.java create mode 100644 src/main/java/first/robot/subsystems/drive/DriveConstants.java create mode 100644 src/main/java/first/robot/subsystems/drive/GyroIO.java create mode 100644 src/main/java/first/robot/subsystems/drive/GyroIOOnboardIMU.java create mode 100644 src/main/java/first/robot/subsystems/drive/Module.java create mode 100644 src/main/java/first/robot/subsystems/drive/ModuleIO.java create mode 100644 src/main/java/first/robot/subsystems/drive/ModuleIOSim.java create mode 100644 src/main/java/first/robot/subsystems/drive/ModuleIOTalonFX.java create mode 100644 vendordeps/AdvantageKit.json create mode 100644 vendordeps/PathplannerLibSystemCoreAlpha.json create mode 100644 vendordeps/Phoenix6-26.50.0-alpha-1.json diff --git a/src/main/java/first/robot/subsystems/drive/Drive.java b/src/main/java/first/robot/subsystems/drive/Drive.java new file mode 100644 index 0000000..feabf06 --- /dev/null +++ b/src/main/java/first/robot/subsystems/drive/Drive.java @@ -0,0 +1,235 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2025.subsystems.drive; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +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.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.frc2025.Constants; +import org.littletonrobotics.frc2025.Constants.Mode; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Drive extends SubsystemBase { + static final Lock odometryLock = new ReentrantLock(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final Alert gyroDisconnectedAlert = + new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); + + private SwerveDriveKinematics kinematics = + new SwerveDriveKinematics(DriveConstants.moduleTranslations); + private Rotation2d rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private SwerveDrivePoseEstimator poseEstimator = + new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO) { + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + } + + @Override + public void periodic() { + odometryLock.lock(); // Prevents odometry updates while reading data + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + odometryLock.unlock(); + + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + // Calculate odometry + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = modules[moduleIndex].getPosition(); + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePositions[moduleIndex].distance - lastModulePositions[moduleIndex].distance, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.yawPosition; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + poseEstimator.updateWithTime(Timer.getTimestamp(), rawGyroRotation, modulePositions); + + // Update gyro alert + gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.getMode() != Mode.SIM); + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = speeds.discretize(Constants.loopPeriodSecs); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.maxLinearSpeed); + + // Log unoptimized setpoints and setpoint speeds + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds); + + // Send setpoints to modules + for (int i = 0; i < 4; i++) { + modules[i].runSetpoint(setpointStates[i]); + } + + // Log optimized setpoints (runSetpoint mutates each state) + Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); + } + + /** Runs the drive in a straight line with the specified drive output. */ + public void runCharacterization(double output) { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(output); + } + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = DriveConstants.moduleTranslations[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getState(); + } + return states; + } + + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getPosition(); + } + return states; + } + + /** Returns the measured chassis speeds of the robot. */ + @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") + private ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } + + /** Returns the position of each module in radians. */ + public double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[4]; + for (int i = 0; i < 4; i++) { + values[i] = modules[i].getWheelRadiusCharacterizationPosition(); + } + return values; + } + + /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + double output = 0.0; + for (int i = 0; i < 4; i++) { + output += modules[i].getFFCharacterizationVelocity() / 4.0; + } + return output; + } + + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Resets the current odometry pose. */ + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** Adds a new timestamped vision measurement. */ + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + poseEstimator.addVisionMeasurement( + visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); + } + + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return DriveConstants.maxLinearSpeed; + } + + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return getMaxLinearSpeedMetersPerSec() / DriveConstants.driveBaseRadius; + } +} diff --git a/src/main/java/first/robot/subsystems/drive/DriveConstants.java b/src/main/java/first/robot/subsystems/drive/DriveConstants.java new file mode 100644 index 0000000..872709f --- /dev/null +++ b/src/main/java/first/robot/subsystems/drive/DriveConstants.java @@ -0,0 +1,96 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2025.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import lombok.Builder; +import org.littletonrobotics.frc2025.Constants; +import org.littletonrobotics.frc2025.Constants.RobotType; + +public class DriveConstants { + public static final double trackWidthX = Units.inchesToMeters(20.75); + public static final double trackWidthY = Units.inchesToMeters(20.75); + public static final double driveBaseRadius = Math.hypot(trackWidthX / 2, trackWidthY / 2); + public static final double maxLinearSpeed = 4.69; + public static final double maxAngularSpeed = 4.69 / driveBaseRadius; + public static final double maxLinearAcceleration = 22.0; + + public static final double driveKs = 5.0; + public static final double driveKv = 0.0; + public static final double driveKp = 35.0; + public static final double driveKd = 0.0; + public static final double turnKp = 4000.0; + public static final double turnKd = 50.0; + + /** Includes bumpers! */ + public static final double robotWidth = + Units.inchesToMeters(28.0) + 2 * Units.inchesToMeters(2.0); + + public static final Translation2d[] moduleTranslations = { + new Translation2d(trackWidthX / 2, trackWidthY / 2), + new Translation2d(trackWidthX / 2, -trackWidthY / 2), + new Translation2d(-trackWidthX / 2, trackWidthY / 2), + new Translation2d(-trackWidthX / 2, -trackWidthY / 2) + }; + + public static final double wheelRadius = Units.inchesToMeters(1.9413001940413326); + + public static final ModuleConfig[] moduleConfigs = { + // FL + ModuleConfig.builder() + .driveMotorId(12) + .turnMotorId(9) + .encoderChannel(2) + .encoderOffset(Rotation2d.fromRadians(0.9022009671847623)) + .turnInverted(true) + .encoderInverted(false) + .build(), + // FR + ModuleConfig.builder() + .driveMotorId(2) + .turnMotorId(10) + .encoderChannel(3) + .encoderOffset(Rotation2d.fromRadians(1.6663099495963458)) + .turnInverted(true) + .encoderInverted(false) + .build(), + // BL + ModuleConfig.builder() + .driveMotorId(15) + .turnMotorId(11) + .encoderChannel(4) + .encoderOffset(Rotation2d.fromRadians(-0.09896592242077659)) + .turnInverted(true) + .encoderInverted(false) + .build(), + // BR + ModuleConfig.builder() + .driveMotorId(3) + .turnMotorId(8) + .encoderChannel(5) + .encoderOffset(Rotation2d.fromRadians(-3.051832863487227)) + .turnInverted(true) + .encoderInverted(false) + .build() + }; + + public static class PigeonConstants { + public static final int id = Constants.getRobot() == RobotType.DEVBOT ? 3 : 30; + } + + @Builder + public record ModuleConfig( + int driveMotorId, + int turnMotorId, + int encoderChannel, + Rotation2d encoderOffset, + boolean turnInverted, + boolean encoderInverted) {} +} diff --git a/src/main/java/first/robot/subsystems/drive/GyroIO.java b/src/main/java/first/robot/subsystems/drive/GyroIO.java new file mode 100644 index 0000000..adc4354 --- /dev/null +++ b/src/main/java/first/robot/subsystems/drive/GyroIO.java @@ -0,0 +1,22 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2025.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; + } + + public default void updateInputs(GyroIOInputs inputs) {} +} diff --git a/src/main/java/first/robot/subsystems/drive/GyroIOOnboardIMU.java b/src/main/java/first/robot/subsystems/drive/GyroIOOnboardIMU.java new file mode 100644 index 0000000..4de2b41 --- /dev/null +++ b/src/main/java/first/robot/subsystems/drive/GyroIOOnboardIMU.java @@ -0,0 +1,22 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2025.subsystems.drive; + +import edu.wpi.first.wpilibj.OnboardIMU; +import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation; + +public class GyroIOOnboardIMU implements GyroIO { + private final OnboardIMU imu = new OnboardIMU(MountOrientation.kFlat); + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = true; + inputs.yawPosition = imu.getRotation2d(); + inputs.yawVelocityRadPerSec = imu.getGyroRateZ(); + } +} diff --git a/src/main/java/first/robot/subsystems/drive/Module.java b/src/main/java/first/robot/subsystems/drive/Module.java new file mode 100644 index 0000000..10fa366 --- /dev/null +++ b/src/main/java/first/robot/subsystems/drive/Module.java @@ -0,0 +1,115 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2025.subsystems.drive; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +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.math.util.Units; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation; +import org.littletonrobotics.junction.Logger; + +public class Module { + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + private SimpleMotorFeedforward ffModel = + new SimpleMotorFeedforward(DriveConstants.driveKs, DriveConstants.driveKv); + + private final Alert driveDisconnectedAlert; + private final Alert turnDisconnectedAlert; + + public Module(ModuleIO io, int index) { + this.io = io; + this.index = index; + driveDisconnectedAlert = + new Alert( + "Disconnected drive motor on module " + Integer.toString(index) + ".", + AlertType.kError); + turnDisconnectedAlert = + new Alert( + "Disconnected turn motor on module " + Integer.toString(index) + ".", AlertType.kError); + } + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // Update alerts + driveDisconnectedAlert.set(!inputs.driveConnected); + turnDisconnectedAlert.set(!inputs.turnConnected); + + // Coast when disabled + if (DriverStation.isDisabled()) { + io.coast(); + } + } + + /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ + public void runSetpoint(SwerveModuleState state) { + // Optimize velocity setpoint + state.optimize(getAngle()); + state.cosineScale(inputs.turnPosition); + + // Apply setpoints + double speedRadPerSec = state.speed / DriveConstants.wheelRadius; + io.runDriveVelocity(speedRadPerSec, ffModel.calculate(speedRadPerSec)); + io.runTurnPosition(state.angle); + } + + /** Runs the module with the specified output while controlling to zero degrees. */ + public void runCharacterization(double output) { + io.runDriveOpenLoop(output); + io.runTurnPosition(new Rotation2d()); + } + + /** Disables all outputs to motors. */ + public void stop() { + io.runDriveOpenLoop(0.0); + io.runTurnOpenLoop(0.0); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + return inputs.turnPosition; + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return inputs.drivePositionRad * DriveConstants.wheelRadius; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * DriveConstants.wheelRadius; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the module position in radians. */ + public double getWheelRadiusCharacterizationPosition() { + return inputs.drivePositionRad; + } + + /** Returns the module velocity in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + return Units.radiansToRotations(inputs.driveVelocityRadPerSec); + } +} diff --git a/src/main/java/first/robot/subsystems/drive/ModuleIO.java b/src/main/java/first/robot/subsystems/drive/ModuleIO.java new file mode 100644 index 0000000..f44b490 --- /dev/null +++ b/src/main/java/first/robot/subsystems/drive/ModuleIO.java @@ -0,0 +1,49 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2025.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs { + public boolean driveConnected = false; + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveSupplyCurrentAmps = 0.0; + public double driveTorqueCurrentAmps = 0.0; + + public boolean turnConnected = false; + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnSupplyCurrentAmps = 0.0; + public double turnTorqueCurrentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified open loop value. */ + public default void runDriveOpenLoop(double output) {} + + /** Run the turn motor at the specified open loop value. */ + public default void runTurnOpenLoop(double output) {} + + /** Run the drive motor at the specified velocity. */ + public default void runDriveVelocity(double velocityRadPerSec, double feedforward) {} + + /** Run the turn motor to the specified rotation. */ + public default void runTurnPosition(Rotation2d rotation) {} + + /** Run in coast mode. */ + public default void coast() {} +} diff --git a/src/main/java/first/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/first/robot/subsystems/drive/ModuleIOSim.java new file mode 100644 index 0000000..f7d6576 --- /dev/null +++ b/src/main/java/first/robot/subsystems/drive/ModuleIOSim.java @@ -0,0 +1,105 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2025.subsystems.drive; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import org.littletonrobotics.frc2025.Constants; + +/** + * Physics sim implementation of module IO. The sim models are configured using a set of module + * constants from Phoenix. Simulation is always based on voltage control. + */ +public class ModuleIOSim implements ModuleIO { + private static final DCMotor driveMotorModel = DCMotor.getKrakenX60Foc(1); + private static final DCMotor turnMotorModel = DCMotor.getKrakenX60Foc(1); + + private final DCMotorSim driveSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + driveMotorModel, 0.025, ModuleIOTalonFX.driveReduction), + driveMotorModel); + private final DCMotorSim turnSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(turnMotorModel, 0.004, ModuleIOTalonFX.turnReduction), + turnMotorModel); + + private boolean driveClosedLoop = false; + private boolean turnClosedLoop = false; + private PIDController driveController = new PIDController(0, 0, 0); + private PIDController turnController = new PIDController(0, 0, 0); + private double driveFFVolts = 0; + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + public ModuleIOSim() { + // Enable wrapping for turn PID + turnController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Run closed-loop control + if (driveClosedLoop) { + driveAppliedVolts = driveFFVolts + driveController.calculate(driveSim.getAngularVelocity()); + } else { + driveController.reset(); + } + if (turnClosedLoop) { + turnAppliedVolts = turnController.calculate(turnSim.getAngularPosition()); + } else { + turnController.reset(); + } + + // Update simulation state + driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); + turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); + driveSim.update(Constants.loopPeriodSecs); + turnSim.update(Constants.loopPeriodSecs); + + inputs.driveConnected = true; + inputs.drivePositionRad = driveSim.getAngularPosition(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocity(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveSupplyCurrentAmps = Math.abs(driveSim.getCurrentDraw()); + + inputs.turnConnected = true; + inputs.turnPosition = new Rotation2d(turnSim.getAngularPosition()); + inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPosition()); + inputs.turnSupplyCurrentAmps = Math.abs(turnSim.getCurrentDraw()); + } + + @Override + public void runDriveOpenLoop(double output) { + driveClosedLoop = false; + driveAppliedVolts = output; + } + + @Override + public void runTurnOpenLoop(double output) { + turnClosedLoop = false; + turnAppliedVolts = output; + } + + @Override + public void runDriveVelocity(double velocityRadPerSec, double feedforward) { + driveClosedLoop = true; + driveFFVolts = feedforward; + driveController.setSetpoint(velocityRadPerSec); + } + + @Override + public void runTurnPosition(Rotation2d rotation) { + turnClosedLoop = true; + turnController.setSetpoint(rotation.getRadians()); + } +} diff --git a/src/main/java/first/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/first/robot/subsystems/drive/ModuleIOTalonFX.java new file mode 100644 index 0000000..09419cb --- /dev/null +++ b/src/main/java/first/robot/subsystems/drive/ModuleIOTalonFX.java @@ -0,0 +1,205 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2025.subsystems.drive; + +import static org.littletonrobotics.frc2025.util.PhoenixUtil.tryUntilOk; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.CoastOut; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.AnalogInput; +import java.util.function.Supplier; +import org.littletonrobotics.frc2025.Constants; + +public class ModuleIOTalonFX implements ModuleIO { + private static final double driveCurrentLimitAmps = 80; + private static final double turnCurrentLimitAmps = 40; + public static final double driveReduction = (50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0); + public static final double turnReduction = (150.0 / 7.0); + + // Hardware objects + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final AnalogInput encoder; + + // Config + private final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); + private final TalonFXConfiguration turnConfig = new TalonFXConfiguration(); + private final Rotation2d encoderOffset; + + // Control requests + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0).withUpdateFreqHz(0); + private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = + new PositionTorqueCurrentFOC(0.0).withUpdateFreqHz(0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0).withUpdateFreqHz(0); + private final CoastOut coast = new CoastOut(); + + // Inputs from drive motor + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveSupplyCurrentAmps; + private final StatusSignal driveTorqueCurrentAmps; + + // Inputs from turn motor + private final Supplier turnAbsolutePosition; + private final StatusSignal turnPosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnSupplyCurrentAmps; + private final StatusSignal turnTorqueCurrentAmps; + + public ModuleIOTalonFX(DriveConstants.ModuleConfig config) { + driveTalon = new TalonFX(config.driveMotorId(), "can_s0"); + turnTalon = new TalonFX(config.turnMotorId(), "can_s0"); + encoder = new AnalogInput(config.encoderChannel()); + encoderOffset = config.encoderOffset(); + // Configure drive motor + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfig.Slot0 = + new Slot0Configs().withKP(DriveConstants.driveKp).withKI(0).withKD(DriveConstants.driveKd); + driveConfig.Feedback.SensorToMechanismRatio = driveReduction; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = driveCurrentLimitAmps; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -driveCurrentLimitAmps; + driveConfig.CurrentLimits.StatorCurrentLimit = driveCurrentLimitAmps; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.02; + tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + + // Configure turn motor + turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + turnConfig.Slot0 = + new Slot0Configs().withKP(DriveConstants.turnKp).withKI(0).withKD(DriveConstants.turnKd); + turnConfig.Feedback.SensorToMechanismRatio = turnReduction; + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + turnConfig.TorqueCurrent.PeakForwardTorqueCurrent = turnCurrentLimitAmps; + turnConfig.TorqueCurrent.PeakReverseTorqueCurrent = -turnCurrentLimitAmps; + turnConfig.CurrentLimits.StatorCurrentLimit = turnCurrentLimitAmps; + turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; + turnConfig.MotorOutput.Inverted = + config.turnInverted() + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); + + // Configure absolute encoder and set position on turn talon + turnAbsolutePosition = + () -> + Rotation2d.fromRadians((double) encoder.getValue() / 3200 * 2.0 * Math.PI) + .plus(encoderOffset); + tryUntilOk(5, () -> turnTalon.setPosition(turnAbsolutePosition.get().getRotations(), 0.25)); + + // Create drive status signals + drivePosition = driveTalon.getPosition(); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveSupplyCurrentAmps = driveTalon.getSupplyCurrent(); + driveTorqueCurrentAmps = driveTalon.getTorqueCurrent(); + + // Create turn status signals + turnPosition = turnTalon.getPosition(); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnSupplyCurrentAmps = turnTalon.getSupplyCurrent(); + turnTorqueCurrentAmps = turnTalon.getTorqueCurrent(); + + // Configure periodic frames + BaseStatusSignal.setUpdateFrequencyForAll( + 1.0 / Constants.loopPeriodSecs, drivePosition, turnPosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrentAmps, + driveTorqueCurrentAmps, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrentAmps, + turnTorqueCurrentAmps); + ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); + } + + @Override + public void updateInputs(ModuleIO.ModuleIOInputs inputs) { + // Update drive inputs + inputs.driveConnected = + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrentAmps, + driveTorqueCurrentAmps) + .isOK(); + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveSupplyCurrentAmps = driveSupplyCurrentAmps.getValueAsDouble(); + inputs.driveTorqueCurrentAmps = driveTorqueCurrentAmps.getValueAsDouble(); + + inputs.turnConnected = + BaseStatusSignal.refreshAll( + turnPosition, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrentAmps, + turnTorqueCurrentAmps) + .isOK(); + inputs.turnAbsolutePosition = turnAbsolutePosition.get().minus(encoderOffset); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnSupplyCurrentAmps = turnSupplyCurrentAmps.getValueAsDouble(); + inputs.turnTorqueCurrentAmps = turnTorqueCurrentAmps.getValueAsDouble(); + } + + @Override + public void runDriveOpenLoop(double output) { + driveTalon.setControl(torqueCurrentRequest.withOutput(output)); + } + + @Override + public void runTurnOpenLoop(double output) { + turnTalon.setControl(torqueCurrentRequest.withOutput(output)); + } + + @Override + public void runDriveVelocity(double velocityRadPerSec, double feedforward) { + driveTalon.setControl( + velocityTorqueCurrentRequest + .withVelocity(Units.radiansToRotations(velocityRadPerSec)) + .withFeedForward(feedforward)); + } + + @Override + public void runTurnPosition(Rotation2d rotation) { + turnTalon.setControl(positionTorqueCurrentRequest.withPosition(rotation.getRotations())); + } + + @Override + public void coast() { + driveTalon.setControl(coast); + turnTalon.setControl(coast); + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..177ee85 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "27.0.0-alpha-4", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "wpilibYear": "2027_alpha5", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "27.0.0-alpha-4" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "27.0.0-alpha-4", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxsystemcore", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLibSystemCoreAlpha.json b/vendordeps/PathplannerLibSystemCoreAlpha.json new file mode 100644 index 0000000..24d3c8c --- /dev/null +++ b/vendordeps/PathplannerLibSystemCoreAlpha.json @@ -0,0 +1,37 @@ +{ + "fileName": "PathplannerLibSystemCoreAlpha.json", + "name": "PathplannerLib", + "version": "2027.0.0-alpha-3", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "wpilibYear": "2027_alpha5", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLibSystemCoreAlpha.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2027.0.0-alpha-3" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2027.0.0-alpha-3", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxsystemcore", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6-26.50.0-alpha-1.json b/vendordeps/Phoenix6-26.50.0-alpha-1.json new file mode 100644 index 0000000..f7db60a --- /dev/null +++ b/vendordeps/Phoenix6-26.50.0-alpha-1.json @@ -0,0 +1,449 @@ +{ + "fileName": "Phoenix6-26.50.0-alpha-1.json", + "name": "CTRE-Phoenix (v6)", + "version": "26.50.0-alpha-1", + "wpilibYear": "2027_alpha5", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2027-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users cannot have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2027-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "26.50.0-alpha-1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxsystemcore" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxsystemcore" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.50.0-alpha-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "26.50.0-alpha-1", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxsystemcore" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.50.0-alpha-1", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxsystemcore" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "26.50.0-alpha-1", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.50.0-alpha-1", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.50.0-alpha-1", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.50.0-alpha-1", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.50.0-alpha-1", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.50.0-alpha-1", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.50.0-alpha-1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.50.0-alpha-1", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.50.0-alpha-1", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.50.0-alpha-1", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.50.0-alpha-1", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.50.0-alpha-1", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file From 6d93d7d67849e4f4b0ab4b38b54181cd56de5e7d Mon Sep 17 00:00:00 2001 From: Glutinfree Date: Tue, 1 Sep 2026 19:14:40 -0700 Subject: [PATCH 2/5] efforts --- .../first/robot/subsystems/drive/Drive.java | 43 +++++++++---------- .../subsystems/drive/DriveConstants.java | 10 ++--- .../first/robot/subsystems/drive/GyroIO.java | 4 +- .../subsystems/drive/GyroIOOnboardIMU.java | 9 ++-- .../first/robot/subsystems/drive/Module.java | 10 ++--- 5 files changed, 38 insertions(+), 38 deletions(-) diff --git a/src/main/java/first/robot/subsystems/drive/Drive.java b/src/main/java/first/robot/subsystems/drive/Drive.java index feabf06..0430f36 100644 --- a/src/main/java/first/robot/subsystems/drive/Drive.java +++ b/src/main/java/first/robot/subsystems/drive/Drive.java @@ -5,28 +5,27 @@ // license that can be found in the LICENSE file at // the root directory of this project. -package org.littletonrobotics.frc2025.subsystems.drive; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -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.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +package first.robot.subsystems.drive; + +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.estimator.SwerveDrivePoseEstimator; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Twist2d; +import org.wpilib.math.kinematics.ChassisVelocities; +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleVelocity; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N3; +import org.wpilib.driverstation.Alert; +import org.wpilib.driverstation.Alert.Level; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.system.Timer; +import org.wpilib.command2.SubsystemBase; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; -import org.littletonrobotics.frc2025.Constants; -import org.littletonrobotics.frc2025.Constants.Mode; +import first.robot.Constants; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -113,8 +112,8 @@ public void periodic() { */ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints - ChassisSpeeds discreteSpeeds = speeds.discretize(Constants.loopPeriodSecs); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + ChassisVelocities discreteSpeeds = speeds.discretize(Constants.loopPeriodSecs); + SwerveModuleVelocity[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.maxLinearSpeed); // Log unoptimized setpoints and setpoint speeds diff --git a/src/main/java/first/robot/subsystems/drive/DriveConstants.java b/src/main/java/first/robot/subsystems/drive/DriveConstants.java index 872709f..66c7e25 100644 --- a/src/main/java/first/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/first/robot/subsystems/drive/DriveConstants.java @@ -5,13 +5,13 @@ // license that can be found in the LICENSE file at // the root directory of this project. -package org.littletonrobotics.frc2025.subsystems.drive; +package first.robot.subsystems.drive; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.util.Units; import lombok.Builder; -import org.littletonrobotics.frc2025.Constants; +import first.robot.Constants; import org.littletonrobotics.frc2025.Constants.RobotType; public class DriveConstants { diff --git a/src/main/java/first/robot/subsystems/drive/GyroIO.java b/src/main/java/first/robot/subsystems/drive/GyroIO.java index adc4354..be2518e 100644 --- a/src/main/java/first/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/first/robot/subsystems/drive/GyroIO.java @@ -5,9 +5,9 @@ // license that can be found in the LICENSE file at // the root directory of this project. -package org.littletonrobotics.frc2025.subsystems.drive; +package first.robot.subsystems.drive; -import edu.wpi.first.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public interface GyroIO { diff --git a/src/main/java/first/robot/subsystems/drive/GyroIOOnboardIMU.java b/src/main/java/first/robot/subsystems/drive/GyroIOOnboardIMU.java index 4de2b41..a4c9c59 100644 --- a/src/main/java/first/robot/subsystems/drive/GyroIOOnboardIMU.java +++ b/src/main/java/first/robot/subsystems/drive/GyroIOOnboardIMU.java @@ -5,13 +5,14 @@ // license that can be found in the LICENSE file at // the root directory of this project. -package org.littletonrobotics.frc2025.subsystems.drive; +package first.robot.subsystems.drive; + +import org.wpilib.hardware.imu.OnboardIMU; +import org.wpilib.hardware.imu.OnboardIMU.MountOrientation; -import edu.wpi.first.wpilibj.OnboardIMU; -import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation; public class GyroIOOnboardIMU implements GyroIO { - private final OnboardIMU imu = new OnboardIMU(MountOrientation.kFlat); + private final OnboardIMU imu = new OnboardIMU(MountOrientation.FLAT); @Override public void updateInputs(GyroIOInputs inputs) { diff --git a/src/main/java/first/robot/subsystems/drive/Module.java b/src/main/java/first/robot/subsystems/drive/Module.java index 10fa366..0c33513 100644 --- a/src/main/java/first/robot/subsystems/drive/Module.java +++ b/src/main/java/first/robot/subsystems/drive/Module.java @@ -5,13 +5,13 @@ // license that can be found in the LICENSE file at // the root directory of this project. -package org.littletonrobotics.frc2025.subsystems.drive; +package first.robot.subsystems.drive; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -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.math.util.Units; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.util.Units; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; From e629ca53b12e63163f051f643989bf3989b825c2 Mon Sep 17 00:00:00 2001 From: Glutinfree Date: Fri, 4 Sep 2026 18:34:23 -0700 Subject: [PATCH 3/5] cleaned up accept logger --- src/main/java/first/robot/Constants.java | 76 ++++- src/main/java/first/robot/Robot.java | 107 ++++--- src/main/java/first/robot/RobotContainer.java | 147 +++++++-- .../first/robot/commands/DriveCommands.java | 284 ++++++++++++++++++ .../first/robot/subsystems/drive/Drive.java | 32 +- .../subsystems/drive/DriveConstants.java | 2 +- .../first/robot/subsystems/drive/Module.java | 25 +- .../robot/subsystems/drive/ModuleIO.java | 4 +- .../robot/subsystems/drive/ModuleIOSim.java | 24 +- .../subsystems/drive/ModuleIOTalonFX.java | 33 +- 10 files changed, 610 insertions(+), 124 deletions(-) create mode 100644 src/main/java/first/robot/commands/DriveCommands.java diff --git a/src/main/java/first/robot/Constants.java b/src/main/java/first/robot/Constants.java index 446e758..3e8085f 100644 --- a/src/main/java/first/robot/Constants.java +++ b/src/main/java/first/robot/Constants.java @@ -4,16 +4,76 @@ package first.robot; +import org.wpilib.driverstation.Alert; +import org.wpilib.driverstation.Alert.Level; +import org.wpilib.framework.RobotBase; + /** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared - * globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. + * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running + * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay" + * (log replay from a file). */ public final class Constants { - public static class OperatorConstants { - public static final int kDriverControllerPort = 0; + public static final double loopPeriodSecs = 0.005; + private static RobotType robotType = RobotType.DEVBOT; + public static final boolean tuningMode = false; + + @SuppressWarnings("resource") + public static RobotType getRobot() { + if (!disableHAL && RobotBase.isReal() && robotType == RobotType.SIMBOT) { + new Alert("Invalid robot selected, using competition robot as default.", Level.MEDIUM) + .set(true); + robotType = RobotType.DEVBOT; + } + return robotType; + } + + public static Mode getMode() { + return switch (robotType) { + case DEVBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; + case SIMBOT -> Mode.SIM; + }; + } + + public enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } + + public enum RobotType { + DEVBOT, + SIMBOT + } + + public static boolean disableHAL = false; + + public static void disableHAL() { + disableHAL = true; + } + + /** Checks whether the correct robot is selected when deploying. */ + public static class CheckDeploy { + public static void main(String... args) { + if (robotType == RobotType.SIMBOT) { + System.err.println("Cannot deploy, invalid robot selected: " + robotType); + System.exit(1); + } + } + } + + /** Checks that the default robot is selected and tuning mode is disabled. */ + public static class CheckPullRequest { + public static void main(String... args) { + if (robotType != RobotType.DEVBOT || tuningMode) { + System.err.println("Do not merge, non-default constants are configured."); + System.exit(1); + } + } } } diff --git a/src/main/java/first/robot/Robot.java b/src/main/java/first/robot/Robot.java index 703d454..f2f89f8 100644 --- a/src/main/java/first/robot/Robot.java +++ b/src/main/java/first/robot/Robot.java @@ -3,51 +3,90 @@ // the WPILib BSD license file in the root directory of this project. package first.robot; - import org.wpilib.command2.Command; import org.wpilib.command2.CommandScheduler; -import org.wpilib.framework.TimedRobot; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; /** - * The methods in this class are called automatically corresponding to each mode, as described in - * the TimedRobot documentation. If you change the name of this class or the package after creating - * this project, you must also update the Main.java file in the project. + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. */ -public class Robot extends TimedRobot { +public class Robot extends LoggedRobot { private Command autonomousCommand; + private RobotContainer robotContainer; - private final RobotContainer robotContainer; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ public Robot() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. + + + // Set up data receivers & replay source + switch (Constants.getMode()) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter()); + // Logger.addDataReceiver(new WPILOGXZWriter()); + Logger.addDataReceiver(new NT4Publisher()); + break; + + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + // String inPath = LogFileUtil.findReplayLog(); + // String outPath = LogFileUtil.addPathSuffix(inPath, "_sim"); + // Logger.setReplaySource(inPath.endsWith(".wpilogxz") ? new WPILOGXZReader(inPath) : + // new WPILOGReader(inPath)); + // if (outPath.endsWith(".wpilogxz")) { + // outPath = outPath.substring(0, outPath.length() - 2); + // } + // Logger.addDataReceiver(new WPILOGWriter(outPath)); + break; + } + + // Start AdvantageKit logger + Logger.start(); + + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(); } - /** - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics - * that you want ran during disabled, autonomous, teleoperated and utility. - * - *

This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - */ + /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. + // Optionally switch the thread to high priority to improve loop + // timing (see the template project documentation for details) + // Threads.setCurrentThreadPriority(true, 99); + + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled commands, running already-scheduled commands, removing + // finished or interrupted commands, and running subsystem periodic() methods. + // This must be called from the robot's periodic block in order for anything in + // the Command-based framework to work. CommandScheduler.getInstance().run(); + + // Return to non-RT thread priority (do not modify the first argument) + // Threads.setCurrentThreadPriority(false, 10); } - /** This function is called once each time the robot enters Disabled mode. */ + /** This function is called once when the robot is disabled. */ @Override public void disabledInit() {} + /** This function is called periodically when disabled. */ @Override public void disabledPeriodic() {} @@ -58,7 +97,7 @@ public void autonomousInit() { // schedule the autonomous command (example) if (autonomousCommand != null) { - CommandScheduler.getInstance().schedule(autonomousCommand); + autonomousCommand.schedule(); } } @@ -66,6 +105,7 @@ public void autonomousInit() { @Override public void autonomousPeriodic() {} + /** This function is called once when teleop is enabled. */ @Override public void teleopInit() { // This makes sure that the autonomous stops running when @@ -81,15 +121,14 @@ public void teleopInit() { @Override public void teleopPeriodic() {} - @Override - public void utilityInit() { - // Cancels all running commands at the start of utility mode. + /** This function is called once when test mode is enabled. */ + public void testInit() { + // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); } - /** This function is called periodically during utility mode. */ - @Override - public void utilityPeriodic() {} + /** This function is called periodically during test mode. */ + public void testPeriodic() {} /** This function is called once when the robot is first started up. */ @Override @@ -98,4 +137,4 @@ public void simulationInit() {} /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() {} -} +} \ No newline at end of file diff --git a/src/main/java/first/robot/RobotContainer.java b/src/main/java/first/robot/RobotContainer.java index ca7e3d6..9921c63 100644 --- a/src/main/java/first/robot/RobotContainer.java +++ b/src/main/java/first/robot/RobotContainer.java @@ -4,47 +4,141 @@ package first.robot; + +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.driverstation.GenericHID; +// import org.wpilib.wpilibj.XboxController; import org.wpilib.command2.Command; -import org.wpilib.command2.button.CommandGamepad; -import org.wpilib.command2.button.Trigger; -import first.robot.Constants.OperatorConstants; -import first.robot.commands.Autos; -import first.robot.commands.ExampleCommand; -import first.robot.subsystems.ExampleSubsystem; +import org.wpilib.command2.Commands; +// import org.wpilib.wpilibj2.command.button.CommandXboxController; +import first.robot.commands.DriveCommands; +import first.robot.subsystems.drive.Drive; +import first.robot.subsystems.drive.DriveConstants; +import first.robot.subsystems.drive.GyroIO; +import first.robot.subsystems.drive.GyroIOOnboardIMU; +import first.robot.subsystems.drive.ModuleIO; +import first.robot.subsystems.drive.ModuleIOSim; +import first.robot.subsystems.drive.ModuleIOTalonFX; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - * subsystems, commands, and trigger mappings) should be declared here. + * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here... - private final ExampleSubsystem exampleSubsystem = new ExampleSubsystem(); + // Subsystems + private Drive drive; + + // Controller + // private final CommandXboxController controller = new CommandXboxController(0); - private final CommandGamepad driverController = - new CommandGamepad(OperatorConstants.kDriverControllerPort); + // Dashboard inputs + private final LoggedDashboardChooser autoChooser; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - // Configure the trigger bindings - configureBindings(); + if (Constants.getMode() != Constants.Mode.REPLAY) { + switch (Constants.getRobot()) { + case DEVBOT: + // Real robot, instantiate hardware IO implementations + drive = + new Drive( + new GyroIOOnboardIMU(), + new ModuleIOTalonFX(DriveConstants.moduleConfigs[0]), + new ModuleIOTalonFX(DriveConstants.moduleConfigs[1]), + new ModuleIOTalonFX(DriveConstants.moduleConfigs[2]), + new ModuleIOTalonFX(DriveConstants.moduleConfigs[3])); + break; + + case SIMBOT: + // Sim robot, instantiate physics sim IO implementations + drive = + new Drive( + new GyroIO() {}, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + break; + + default: + // Replayed robot, disable IO implementations + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + break; + } + } + + // No-op implementations for replay + if (drive == null) { + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + } + + // Set up auto routines + autoChooser = new LoggedDashboardChooser<>("Auto Choices"); + + // Set up SysId routines + autoChooser.addDefaultOption( + "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive)); + autoChooser.addOption( + "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive)); + + // Configure the button bindings + configureButtonBindings(); } /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary - * predicate, or via the named factories in {@link org.wpilib.command2.button.CommandGenericHID}'s - * subclasses for {@link CommandGamepad Gamepad} gamepads or {@link - * org.wpilib.command2.button.CommandJoystick Flight joysticks}. + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * org.wpilib.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * org.wpilib.wpilibj2.command.button.JoystickButton}. */ - private void configureBindings() { - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - new Trigger(exampleSubsystem::exampleCondition).onTrue(new ExampleCommand(exampleSubsystem)); + private void configureButtonBindings() { + // Default command, normal field-relative drive + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + + // Lock to 0° when A button is held + controller + .a() + .whileTrue( + DriveCommands.joystickDriveAtAngle( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> new Rotation2d())); + + // Switch to X pattern when X button is pressed + controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - // Schedule `exampleMethodCommand` when the Gamepad's east face button is pressed, - // cancelling on release. - driverController.eastFace().whileTrue(exampleSubsystem.exampleMethodCommand()); + // Reset gyro to 0° when B button is pressed + controller + .b() + .onTrue( + Commands.runOnce( + () -> + drive.setPose( + new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + drive) + .ignoringDisable(true)); } /** @@ -53,7 +147,6 @@ private void configureBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // An example command will be run in autonomous - return Autos.exampleAuto(exampleSubsystem); + return autoChooser.get(); } -} +} \ No newline at end of file diff --git a/src/main/java/first/robot/commands/DriveCommands.java b/src/main/java/first/robot/commands/DriveCommands.java new file mode 100644 index 0000000..b400ded --- /dev/null +++ b/src/main/java/first/robot/commands/DriveCommands.java @@ -0,0 +1,284 @@ +package first.robot.commands; + +import org.wpilib.math.util.MathUtil; +import org.wpilib.math.controller.ProfiledPIDController; +import org.wpilib.math.filter.SlewRateLimiter; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.kinematics.ChassisVelocities; +import org.wpilib.math.trajectory.TrapezoidProfile; +import org.wpilib.math.util.Units; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.internal.DriverStationBackend; +import org.wpilib.driverstation.Alliance; +import org.wpilib.system.Timer; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import first.robot.subsystems.drive.Drive; +import first.robot.subsystems.drive.DriveConstants; + +public class DriveCommands { + private static final double DEADBAND = 0.1; + private static final double ANGLE_KP = 5.0; + private static final double ANGLE_KD = 0.4; + private static final double ANGLE_MAX_VELOCITY = 8.0; + private static final double ANGLE_MAX_ACCELERATION = 20.0; + private static final double FF_START_DELAY = 2.0; // Secs + private static final double FF_RAMP_RATE = 0.1; // Volts/Sec + private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec + private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + + private DriveCommands() {} + + private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), DEADBAND); + Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); + + // Square magnitude for more precise control + linearMagnitude = linearMagnitude * linearMagnitude; + + // Return new linear velocity + return new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + } + + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return Commands.run( + () -> { + // Get linear velocity + Translation2d linearVelocity = + getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + + // Apply rotation deadband + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + + // Square rotation value for more precise control + omega = Math.copySign(omega * omega, omega); + + // Convert to field relative speeds & send command + ChassisVelocities speeds = + new ChassisVelocities( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec()); + boolean isFlipped = + DriverStationBackend.getAlliance().isPresent() + && DriverStationBackend.getAlliance().get() == Alliance.RED; + drive.runVelocity( + speeds.toRobotRelative( + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive); + } + + /** + * Field relative drive command using joystick for linear control and PID for angular control. + * Possible use cases include snapping to an angle, aiming at a vision target, or controlling + * absolute rotation with a joystick. + */ + public static Command joystickDriveAtAngle( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + Supplier rotationSupplier) { + + // Create PID controller + ProfiledPIDController angleController = + new ProfiledPIDController( + ANGLE_KP, + 0.0, + ANGLE_KD, + new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + // Construct command + return Commands.run( + () -> { + // Get linear velocity + Translation2d linearVelocity = + getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + + // Calculate angular speed + double omega = + angleController.calculate( + drive.getRotation().getRadians(), rotationSupplier.get().getRadians()); + + // Convert to field relative speeds & send command + ChassisVelocities speeds = + new ChassisVelocities( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega); + boolean isFlipped = + DriverStationBackend.getAlliance().isPresent() + && DriverStationBackend.getAlliance().get() == Alliance.RED; + drive.runVelocity( + speeds.toRobotRelative( + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive) + + // Reset PID controller when command starts + .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); + } + + /** + * Measures the velocity feedforward constants for the drive motors. + * + *

This command should only be used in voltage control mode. + */ + public static Command feedforwardCharacterization(Drive drive) { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce( + () -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + + // Allow modules to orient + Commands.run( + () -> { + drive.runCharacterization(0.0); + }, + drive) + .withTimeout(FF_START_DELAY), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run( + () -> { + double voltage = timer.get() * FF_RAMP_RATE; + drive.runCharacterization(voltage); + velocitySamples.add(drive.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + }, + drive) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + System.out.println("********** Drive FF Characterization Results **********"); + System.out.println("\tkS: " + formatter.format(kS)); + System.out.println("\tkV: " + formatter.format(kV)); + })); + } + + /** Measures the robot's wheel radius by spinning in a circle. */ + public static Command wheelRadiusCharacterization(Drive drive) { + SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); + WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce( + () -> { + limiter.reset(0.0); + }), + + // Turn in place, accelerating up to full speed + Commands.run( + () -> { + double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); + drive.runVelocity(new ChassisVelocities(0.0, 0.0, speed)); + }, + drive)), + + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + + // Record starting measurement + Commands.runOnce( + () -> { + state.positions = drive.getWheelRadiusCharacterizationPositions(); + state.lastAngle = drive.getRotation(); + state.gyroDelta = 0.0; + }), + + // Update gyro delta + Commands.run( + () -> { + var rotation = drive.getRotation(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + }) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + double[] positions = drive.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = + (state.gyroDelta * DriveConstants.driveBaseRadius) / wheelDelta; + + NumberFormat formatter = new DecimalFormat("#0.000"); + System.out.println( + "********** Wheel Radius Characterization Results **********"); + System.out.println( + "\tWheel Delta: " + formatter.format(wheelDelta) + " radians"); + System.out.println( + "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians"); + System.out.println( + "\tWheel Radius: " + + formatter.format(wheelRadius) + + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + + " inches"); + }))); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = new Rotation2d(); + double gyroDelta = 0.0; + } +} diff --git a/src/main/java/first/robot/subsystems/drive/Drive.java b/src/main/java/first/robot/subsystems/drive/Drive.java index 0430f36..30abc24 100644 --- a/src/main/java/first/robot/subsystems/drive/Drive.java +++ b/src/main/java/first/robot/subsystems/drive/Drive.java @@ -20,6 +20,7 @@ import org.wpilib.math.numbers.N3; import org.wpilib.driverstation.Alert; import org.wpilib.driverstation.Alert.Level; +import first.robot.Constants.Mode; import org.wpilib.driverstation.DriverStation; import org.wpilib.system.Timer; import org.wpilib.command2.SubsystemBase; @@ -28,14 +29,15 @@ import first.robot.Constants; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import first.robot.subsystems.drive.GyroIO.GyroIOInputs; public class Drive extends SubsystemBase { static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final GyroIOInputs gyroInputs = new GyroIOInputs(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final Alert gyroDisconnectedAlert = - new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); + new Alert("Disconnected gyro, using kinematics as fallback.", Level.MEDIUM); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(DriveConstants.moduleTranslations); @@ -75,8 +77,8 @@ public void periodic() { // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleVelocity[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleVelocity[] {}); } // Calculate odometry @@ -110,15 +112,15 @@ public void periodic() { * * @param speeds Speeds in meters/sec */ - public void runVelocity(ChassisSpeeds speeds) { + public void runVelocity(ChassisVelocities speeds) { // Calculate module setpoints ChassisVelocities discreteSpeeds = speeds.discretize(Constants.loopPeriodSecs); - SwerveModuleVelocity[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.maxLinearSpeed); + SwerveModuleVelocity[] setpointStates = kinematics.toSwerveModuleVelocities(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelVelocities(setpointStates, DriveConstants.maxLinearSpeed); // Log unoptimized setpoints and setpoint speeds Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds); + Logger.recordOutput("SwerveChassisVelocities/Setpoints", discreteSpeeds); // Send setpoints to modules for (int i = 0; i < 4; i++) { @@ -138,7 +140,7 @@ public void runCharacterization(double output) { /** Stops the drive. */ public void stop() { - runVelocity(new ChassisSpeeds()); + runVelocity(new ChassisVelocities()); } /** @@ -156,10 +158,10 @@ public void stopWithX() { /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") - private SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; + private SwerveModuleVelocity[] getModuleVelocities() { + SwerveModuleVelocity[] states = new SwerveModuleVelocity[4]; for (int i = 0; i < 4; i++) { - states[i] = modules[i].getState(); + states[i] = modules[i].getVelocity(); } return states; } @@ -174,9 +176,9 @@ private SwerveModulePosition[] getModulePositions() { } /** Returns the measured chassis speeds of the robot. */ - @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") - private ChassisSpeeds getChassisSpeeds() { - return kinematics.toChassisSpeeds(getModuleStates()); + @AutoLogOutput(key = "SwerveChassisVelocities/Measured") + private ChassisVelocities getChassisVelocities() { + return kinematics.toChassisVelocities(getModuleVelocities()); } /** Returns the position of each module in radians. */ diff --git a/src/main/java/first/robot/subsystems/drive/DriveConstants.java b/src/main/java/first/robot/subsystems/drive/DriveConstants.java index 66c7e25..f38b482 100644 --- a/src/main/java/first/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/first/robot/subsystems/drive/DriveConstants.java @@ -12,7 +12,7 @@ import org.wpilib.math.util.Units; import lombok.Builder; import first.robot.Constants; -import org.littletonrobotics.frc2025.Constants.RobotType; +import first.robot.Constants.RobotType;; public class DriveConstants { public static final double trackWidthX = Units.inchesToMeters(20.75); diff --git a/src/main/java/first/robot/subsystems/drive/Module.java b/src/main/java/first/robot/subsystems/drive/Module.java index 0c33513..7114c07 100644 --- a/src/main/java/first/robot/subsystems/drive/Module.java +++ b/src/main/java/first/robot/subsystems/drive/Module.java @@ -7,14 +7,15 @@ package first.robot.subsystems.drive; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import org.wpilib.math.controller.SimpleMotorFeedforward; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.kinematics.SwerveModulePosition; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.math.util.Units; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.DriverStation; +import org.wpilib.driverstation.Alert; +import org.wpilib.driverstation.Alert.Level; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.internal.DriverStationBackend; import org.littletonrobotics.junction.Logger; public class Module { @@ -34,10 +35,10 @@ public Module(ModuleIO io, int index) { driveDisconnectedAlert = new Alert( "Disconnected drive motor on module " + Integer.toString(index) + ".", - AlertType.kError); + Level.MEDIUM); turnDisconnectedAlert = new Alert( - "Disconnected turn motor on module " + Integer.toString(index) + ".", AlertType.kError); + "Disconnected turn motor on module " + Integer.toString(index) + ".", Level.MEDIUM); } public void periodic() { @@ -49,19 +50,19 @@ public void periodic() { turnDisconnectedAlert.set(!inputs.turnConnected); // Coast when disabled - if (DriverStation.isDisabled()) { + if (DriverStationBackend.isDisabled()) { io.coast(); } } /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ - public void runSetpoint(SwerveModuleState state) { + public void runSetpoint(SwerveModuleVelocity state) { // Optimize velocity setpoint state.optimize(getAngle()); state.cosineScale(inputs.turnPosition); // Apply setpoints - double speedRadPerSec = state.speed / DriveConstants.wheelRadius; + double speedRadPerSec = state.velocity / DriveConstants.wheelRadius; io.runDriveVelocity(speedRadPerSec, ffModel.calculate(speedRadPerSec)); io.runTurnPosition(state.angle); } @@ -99,8 +100,8 @@ public SwerveModulePosition getPosition() { } /** Returns the module state (turn angle and drive velocity). */ - public SwerveModuleState getState() { - return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + public SwerveModuleVelocity getState() { + return new SwerveModuleVelocity(getVelocityMetersPerSec(), getAngle()); } /** Returns the module position in radians. */ diff --git a/src/main/java/first/robot/subsystems/drive/ModuleIO.java b/src/main/java/first/robot/subsystems/drive/ModuleIO.java index f44b490..9d3b8c5 100644 --- a/src/main/java/first/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/first/robot/subsystems/drive/ModuleIO.java @@ -5,9 +5,9 @@ // license that can be found in the LICENSE file at // the root directory of this project. -package org.littletonrobotics.frc2025.subsystems.drive; +package first.robot.subsystems.drive; -import edu.wpi.first.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { diff --git a/src/main/java/first/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/first/robot/subsystems/drive/ModuleIOSim.java index f7d6576..90e9bba 100644 --- a/src/main/java/first/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/first/robot/subsystems/drive/ModuleIOSim.java @@ -5,15 +5,15 @@ // license that can be found in the LICENSE file at // the root directory of this project. -package org.littletonrobotics.frc2025.subsystems.drive; +package first.robot.subsystems.drive; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import org.littletonrobotics.frc2025.Constants; +import org.wpilib.math.util.MathUtil; +import org.wpilib.math.controller.PIDController; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.system.DCMotor; +import org.wpilib.math.system.Models; +import org.wpilib.simulation.DCMotorSim; +import first.robot.Constants; /** * Physics sim implementation of module IO. The sim models are configured using a set of module @@ -25,12 +25,12 @@ public class ModuleIOSim implements ModuleIO { private final DCMotorSim driveSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem( + Models.singleJointedArmFromPhysicalConstants( driveMotorModel, 0.025, ModuleIOTalonFX.driveReduction), driveMotorModel); private final DCMotorSim turnSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(turnMotorModel, 0.004, ModuleIOTalonFX.turnReduction), + Models.singleJointedArmFromPhysicalConstants(turnMotorModel, 0.004, ModuleIOTalonFX.turnReduction), turnMotorModel); private boolean driveClosedLoop = false; @@ -61,8 +61,8 @@ public void updateInputs(ModuleIOInputs inputs) { } // Update simulation state - driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); - turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); + driveSim.setInputVoltage(Math.clamp(driveAppliedVolts, -12.0, 12.0)); + turnSim.setInputVoltage(Math.clamp(turnAppliedVolts, -12.0, 12.0)); driveSim.update(Constants.loopPeriodSecs); turnSim.update(Constants.loopPeriodSecs); diff --git a/src/main/java/first/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/first/robot/subsystems/drive/ModuleIOTalonFX.java index 09419cb..9847775 100644 --- a/src/main/java/first/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/first/robot/subsystems/drive/ModuleIOTalonFX.java @@ -5,11 +5,11 @@ // license that can be found in the LICENSE file at // the root directory of this project. -package org.littletonrobotics.frc2025.subsystems.drive; - -import static org.littletonrobotics.frc2025.util.PhoenixUtil.tryUntilOk; +package first.robot.subsystems.drive; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -21,15 +21,15 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.AnalogInput; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.util.Units; +import org.wpilib.units.measure.Angle; +import org.wpilib.units.measure.AngularVelocity; +import org.wpilib.units.measure.Current; +import org.wpilib.units.measure.Voltage; +import org.wpilib.hardware.discrete.AnalogInput; import java.util.function.Supplier; -import org.littletonrobotics.frc2025.Constants; +import first.robot.Constants; public class ModuleIOTalonFX implements ModuleIO { private static final double driveCurrentLimitAmps = 80; @@ -71,8 +71,8 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal turnTorqueCurrentAmps; public ModuleIOTalonFX(DriveConstants.ModuleConfig config) { - driveTalon = new TalonFX(config.driveMotorId(), "can_s0"); - turnTalon = new TalonFX(config.turnMotorId(), "can_s0"); + driveTalon = new TalonFX(config.driveMotorId(), CANBus.systemcore(0)); + turnTalon = new TalonFX(config.turnMotorId(), CANBus.systemcore(0)); encoder = new AnalogInput(config.encoderChannel()); encoderOffset = config.encoderOffset(); // Configure drive motor @@ -202,4 +202,11 @@ public void coast() { driveTalon.setControl(coast); turnTalon.setControl(coast); } + + public static void tryUntilOk(int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + var error = command.get(); + if (error.isOK()) break; + } + } } From ece12280553d0aff2e23ed0507d305b7040e4935 Mon Sep 17 00:00:00 2001 From: Glutinfree Date: Sat, 5 Sep 2026 17:02:25 -0700 Subject: [PATCH 4/5] Fixed issues with moduleIOInputsAutoLogged Added dependencies to build.gradle --- build.gradle | 7 +++++++ src/main/java/first/robot/RobotContainer.java | 4 ++-- src/main/java/first/robot/subsystems/drive/Drive.java | 2 +- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/build.gradle b/build.gradle index b4f32f5..fa0e69d 100644 --- a/build.gradle +++ b/build.gradle @@ -2,6 +2,7 @@ plugins { id "java" id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6" id "com.gradleup.shadow" version "9.3.0" + id("io.freefair.lombok") version "9.5.0" } java { @@ -77,6 +78,12 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" + + compileOnly("org.projectlombok:lombok:1.18.48") + annotationProcessor("org.projectlombok:lombok:1.18.48") } test { diff --git a/src/main/java/first/robot/RobotContainer.java b/src/main/java/first/robot/RobotContainer.java index 9921c63..4d5d33d 100644 --- a/src/main/java/first/robot/RobotContainer.java +++ b/src/main/java/first/robot/RobotContainer.java @@ -11,7 +11,7 @@ // import org.wpilib.wpilibj.XboxController; import org.wpilib.command2.Command; import org.wpilib.command2.Commands; -// import org.wpilib.wpilibj2.command.button.CommandXboxController; +import org.wpilib.driverstation.Gamepad; import first.robot.commands.DriveCommands; import first.robot.subsystems.drive.Drive; import first.robot.subsystems.drive.DriveConstants; @@ -33,7 +33,7 @@ public class RobotContainer { private Drive drive; // Controller - // private final CommandXboxController controller = new CommandXboxController(0); + private final Gamepad controller = new Gamepad(0); // Dashboard inputs private final LoggedDashboardChooser autoChooser; diff --git a/src/main/java/first/robot/subsystems/drive/Drive.java b/src/main/java/first/robot/subsystems/drive/Drive.java index 30abc24..7f9eb2a 100644 --- a/src/main/java/first/robot/subsystems/drive/Drive.java +++ b/src/main/java/first/robot/subsystems/drive/Drive.java @@ -34,7 +34,7 @@ public class Drive extends SubsystemBase { static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; - private final GyroIOInputs gyroInputs = new GyroIOInputs(); + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", Level.MEDIUM); From 9529966255813b3d9a2bb08919efb426788a2341 Mon Sep 17 00:00:00 2001 From: Anay Date: Tue, 8 Sep 2026 18:38:15 -0700 Subject: [PATCH 5/5] fixed swerves hopefully --- .gitignore | 1 + .wpilib/wpilib_preferences.json | 2 +- build.gradle | 44 +++--- gradlew | 0 settings.gradle | 23 ++- src/main/java/first/Main.java | 2 +- src/main/java/first/robot/Constants.java | 37 +++-- src/main/java/first/robot/Robot.java | 10 +- src/main/java/first/robot/RobotContainer.java | 95 +++++------ .../first/robot/commands/DriveCommands.java | 11 +- .../first/robot/subsystems/drive/Drive.java | 32 ++-- .../subsystems/drive/DriveConstants.java | 109 +++++++++++-- .../robot/subsystems/drive/GyroIOPigeon2.java | 46 ++++++ .../first/robot/subsystems/drive/Module.java | 64 +++++--- .../robot/subsystems/drive/ModuleIO.java | 8 +- .../robot/subsystems/drive/ModuleIOSim.java | 32 +++- .../subsystems/drive/ModuleIOTalonFX.java | 108 ++++++++++--- src/test/java/first/robot/SimTestFixture.java | 83 ++++++++++ .../robot/commands/JoystickDriveTest.java | 148 ++++++++++++++++++ .../robot/subsystems/drive/DriveSimTest.java | 142 +++++++++++++++++ vendordeps/AdvantageKit.json | 66 ++++---- vendordeps/CommandsV2.json | 6 +- vendordeps/PathplannerLibSystemCoreAlpha.json | 2 +- vendordeps/Phoenix6-26.50.0-alpha-1.json | 2 +- 24 files changed, 839 insertions(+), 234 deletions(-) mode change 100644 => 100755 gradlew create mode 100644 src/main/java/first/robot/subsystems/drive/GyroIOPigeon2.java create mode 100644 src/test/java/first/robot/SimTestFixture.java create mode 100644 src/test/java/first/robot/commands/JoystickDriveTest.java create mode 100644 src/test/java/first/robot/subsystems/drive/DriveSimTest.java diff --git a/.gitignore b/.gitignore index 34cbaac..134d1a6 100644 --- a/.gitignore +++ b/.gitignore @@ -171,6 +171,7 @@ out/ # Simulation GUI and other tools window save file networktables.json simgui.json +simgui-ds.json *-window.json # Simulation data log directory diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index bbfeccd..44dd67e 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2027_alpha5", + "projectYear": "2027_alpha7", "teamNumber": 1138 } \ No newline at end of file diff --git a/build.gradle b/build.gradle index fa0e69d..1972bb7 100644 --- a/build.gradle +++ b/build.gradle @@ -1,8 +1,7 @@ plugins { id "java" - id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6" - id "com.gradleup.shadow" version "9.3.0" - id("io.freefair.lombok") version "9.5.0" + id "application" + id "org.wpilib.GradleRIO" version "2027.0.0-alpha-7" } java { @@ -19,8 +18,6 @@ deploy { systemcore(getTargetTypeClass('SystemCore')) { // Team number is loaded either from the .wpilib/wpilib_preferences.json // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. team = project.wpilib.getTeamNumber() // Use the default systemcore host name. This must be called after setting team // as happens on the line above @@ -32,6 +29,9 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string wpilibJava(getArtifactTypeClass('WPILibJavaArtifact')) { + // Set to true to use debug including JNI, which will drastically impact + // performance. + debugJni = false } // Static files artifact @@ -48,9 +48,8 @@ deploy { def deployArtifact = deploy.targets.systemcore.artifacts.wpilibJava -// Set to true to use debug for all targets including JNI, which will drastically impact -// performance. -wpi.java.debugJni = false +// Set to true to use debug for simulation including JNI +wpi.java.runSimWithDebugJni = false // Set this to true to enable desktop support. def includeDesktopSupport = false @@ -78,12 +77,12 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - - def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) - annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" - compileOnly("org.projectlombok:lombok:1.18.48") - annotationProcessor("org.projectlombok:lombok:1.18.48") + // AdvantageKit's @AutoLog annotation processor. Version is read from the vendordep so the + // two can never drift apart. + def akitJson = new groovy.json.JsonSlurper() + .parseText(new File(projectDir, "vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" } test { @@ -95,23 +94,18 @@ test { wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() -// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat/shaded jar') -// in order to make them all available at runtime and merging service files to make JSON work. -// Also adding the manifest so WPILib knows where to look for our Robot Class. -shadowJar { - mergeServiceFiles() +application.mainClass = ROBOT_MAIN_CLASS + +deployArtifact.configureApplication(application) +wpi.java.configureApplication(application) +wpi.java.configureTestTasks(test) + +jar { from('src') { into 'backup/src' } from('vendordeps') { into 'backup/vendordeps' } from('build.gradle') { into 'backup' } - manifest org.wpilib.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) - duplicatesStrategy = DuplicatesStrategy.INCLUDE } -// Configure jar and deploy tasks -deployArtifact.jarTask = shadowJar -wpi.java.configureExecutableTasks(shadowJar) -wpi.java.configureTestTasks(test) - // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/settings.gradle b/settings.gradle index e2626a8..842a352 100644 --- a/settings.gradle +++ b/settings.gradle @@ -2,19 +2,28 @@ import org.gradle.internal.os.OperatingSystem pluginManagement { repositories { - String wpilibYear = '2027_alpha5' + String wpilibYear = '2027_alpha7' File wpilibHome - if (OperatingSystem.current().isWindows()) { + def os = OperatingSystem.current() + if (os.isWindows()) { String publicFolder = System.getenv('PUBLIC') if (publicFolder == null) { publicFolder = "C:\\Users\\Public" } - def homeRoot = new File(publicFolder, "wpilib") - wpilibHome = new File(homeRoot, wpilibYear) + wpilibHome = new File(new File(publicFolder, "wpilib"), wpilibYear) + } else if (os.isLinux()) { + String xdgDataHome = System.getenv('XDG_DATA_HOME') + if (xdgDataHome == null || xdgDataHome.trim().isEmpty() || !new File(xdgDataHome).isAbsolute()) { + xdgDataHome = new File(System.getProperty("user.home"), ".local/share").getPath() + } + wpilibHome = new File(new File(xdgDataHome, "wpilib"), wpilibYear) } else { - def userFolder = System.getProperty("user.home") - def homeRoot = new File(userFolder, "wpilib") - wpilibHome = new File(homeRoot, wpilibYear) + def userFolder = new File(System.getProperty("user.home")) + // The macOS installer has used both ~/.wpilib and ~/wpilib across alphas. + wpilibHome = new File(new File(userFolder, ".wpilib"), wpilibYear) + if (!wpilibHome.exists()) { + wpilibHome = new File(new File(userFolder, "wpilib"), wpilibYear) + } } def wpilibHomeMaven = new File(wpilibHome, 'maven') maven { diff --git a/src/main/java/first/Main.java b/src/main/java/first/Main.java index 7d287e7..b3a3127 100644 --- a/src/main/java/first/Main.java +++ b/src/main/java/first/Main.java @@ -20,6 +20,6 @@ private Main() {} *

If you change your main robot class, change the parameter type. */ public static void main(String... args) { - RobotBase.startRobot(first.robot.Robot.class); + RobotBase.startRobot(first.robot.Robot::new); } } diff --git a/src/main/java/first/robot/Constants.java b/src/main/java/first/robot/Constants.java index 3e8085f..af188da 100644 --- a/src/main/java/first/robot/Constants.java +++ b/src/main/java/first/robot/Constants.java @@ -4,35 +4,50 @@ package first.robot; -import org.wpilib.driverstation.Alert; -import org.wpilib.driverstation.Alert.Level; import org.wpilib.framework.RobotBase; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.Level; /** * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running - * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay" - * (log replay from a file). + * on SystemCore. Change the value of {@link #simMode} to switch between "sim" (physics sim) and + * "replay" (log replay from a file). */ public final class Constants { - public static final double loopPeriodSecs = 0.005; + /** + * Robot loop period. This is handed to {@code LoggedRobot} in {@link Robot}, so the value used + * for velocity discretization and Phoenix status frame rates always matches the real loop rate. + */ + public static final double loopPeriodSecs = 0.02; + + /** Which physical robot the code is running on. Selects hardware IDs. */ private static RobotType robotType = RobotType.DEVBOT; + + /** Enables tuning dashboard inputs. Must be false when merging. */ public static final boolean tuningMode = false; + /** Mode used when not running on real hardware. Set to REPLAY to replay a log instead. */ + public static final Mode simMode = Mode.SIM; + @SuppressWarnings("resource") public static RobotType getRobot() { if (!disableHAL && RobotBase.isReal() && robotType == RobotType.SIMBOT) { - new Alert("Invalid robot selected, using competition robot as default.", Level.MEDIUM) + new Alert( + "invalidRobotType", + "Invalid robot selected, using competition robot as default.", + Level.MEDIUM) .set(true); robotType = RobotType.DEVBOT; } return robotType; } + /** + * Returns the current runtime mode. Real hardware is always {@link Mode#REAL}; off-robot this + * follows {@link #simMode} so that the physics simulation actually runs by default. + */ public static Mode getMode() { - return switch (robotType) { - case DEVBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; - case SIMBOT -> Mode.SIM; - }; + return RobotBase.isReal() ? Mode.REAL : simMode; } public enum Mode { @@ -76,4 +91,6 @@ public static void main(String... args) { } } } + + private Constants() {} } diff --git a/src/main/java/first/robot/Robot.java b/src/main/java/first/robot/Robot.java index f2f89f8..422560d 100644 --- a/src/main/java/first/robot/Robot.java +++ b/src/main/java/first/robot/Robot.java @@ -23,7 +23,13 @@ public class Robot extends LoggedRobot { private RobotContainer robotContainer; public Robot() { + // Run the loop at the period the rest of the code assumes (see Constants.loopPeriodSecs). + super(Constants.loopPeriodSecs); + // Record build/runtime metadata so logs can be traced back to a configuration + Logger.recordMetadata("RobotType", Constants.getRobot().toString()); + Logger.recordMetadata("RuntimeMode", Constants.getMode().toString()); + Logger.recordMetadata("TuningMode", Boolean.toString(Constants.tuningMode)); // Set up data receivers & replay source switch (Constants.getMode()) { @@ -95,9 +101,9 @@ public void disabledPeriodic() {} public void autonomousInit() { autonomousCommand = robotContainer.getAutonomousCommand(); - // schedule the autonomous command (example) + // schedule the autonomous command (Command.schedule() was removed in 2027) if (autonomousCommand != null) { - autonomousCommand.schedule(); + CommandScheduler.getInstance().schedule(autonomousCommand); } } diff --git a/src/main/java/first/robot/RobotContainer.java b/src/main/java/first/robot/RobotContainer.java index 4d5d33d..e242915 100644 --- a/src/main/java/first/robot/RobotContainer.java +++ b/src/main/java/first/robot/RobotContainer.java @@ -4,23 +4,20 @@ package first.robot; - -import org.wpilib.math.geometry.Pose2d; -import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.driverstation.GenericHID; -// import org.wpilib.wpilibj.XboxController; -import org.wpilib.command2.Command; -import org.wpilib.command2.Commands; -import org.wpilib.driverstation.Gamepad; import first.robot.commands.DriveCommands; import first.robot.subsystems.drive.Drive; import first.robot.subsystems.drive.DriveConstants; import first.robot.subsystems.drive.GyroIO; -import first.robot.subsystems.drive.GyroIOOnboardIMU; +import first.robot.subsystems.drive.GyroIOPigeon2; import first.robot.subsystems.drive.ModuleIO; import first.robot.subsystems.drive.ModuleIOSim; import first.robot.subsystems.drive.ModuleIOTalonFX; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.LoggedNetworkChooser; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import org.wpilib.command2.button.CommandGamepad; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -30,31 +27,31 @@ */ public class RobotContainer { // Subsystems - private Drive drive; + private final Drive drive; - // Controller - private final Gamepad controller = new Gamepad(0); + // Controller. CommandGamepad uses controller-agnostic names: faceDown/faceRight/faceLeft/faceUp + // are A/B/X/Y on an Xbox pad. + private final CommandGamepad controller = new CommandGamepad(0); // Dashboard inputs - private final LoggedDashboardChooser autoChooser; + private final LoggedNetworkChooser autoChooser; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - if (Constants.getMode() != Constants.Mode.REPLAY) { - switch (Constants.getRobot()) { - case DEVBOT: + switch (Constants.getMode()) { + case REAL -> // Real robot, instantiate hardware IO implementations drive = new Drive( - new GyroIOOnboardIMU(), + new GyroIOPigeon2(), new ModuleIOTalonFX(DriveConstants.moduleConfigs[0]), new ModuleIOTalonFX(DriveConstants.moduleConfigs[1]), new ModuleIOTalonFX(DriveConstants.moduleConfigs[2]), new ModuleIOTalonFX(DriveConstants.moduleConfigs[3])); - break; - case SIMBOT: - // Sim robot, instantiate physics sim IO implementations + case SIM -> + // Sim robot, instantiate physics sim IO implementations. There is no Pigeon sim, so the + // heading comes from the module kinematics instead. drive = new Drive( new GyroIO() {}, @@ -62,9 +59,8 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); - break; - default: + default -> // Replayed robot, disable IO implementations drive = new Drive( @@ -73,42 +69,27 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - break; - } - } - - // No-op implementations for replay - if (drive == null) { - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); } // Set up auto routines - autoChooser = new LoggedDashboardChooser<>("Auto Choices"); + autoChooser = new LoggedNetworkChooser<>("/SmartDashboard/Auto Choices"); + autoChooser.addDefault("None", Commands.none()); - // Set up SysId routines - autoChooser.addDefaultOption( + // Set up characterization routines + autoChooser.add( "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive)); - autoChooser.addOption( + autoChooser.add( "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive)); // Configure the button bindings configureButtonBindings(); } - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * org.wpilib.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * org.wpilib.wpilibj2.command.button.JoystickButton}. - */ + /** Maps driver inputs to commands. */ private void configureButtonBindings() { - // Default command, normal field-relative drive + // Default command, normal field-relative drive. + // +X on the field is away from the driver station and +Y is to the left, so forward on the + // stick (which reads negative) maps to +X and left (also negative) maps to +Y. drive.setDefaultCommand( DriveCommands.joystickDrive( drive, @@ -116,27 +97,25 @@ private void configureButtonBindings() { () -> -controller.getLeftX(), () -> -controller.getRightX())); - // Lock to 0° when A button is held + // Lock to 0 degrees while A is held controller - .a() + .faceDown() .whileTrue( DriveCommands.joystickDriveAtAngle( drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), - () -> new Rotation2d())); + () -> Rotation2d.ZERO)); - // Switch to X pattern when X button is pressed - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + // Switch to X pattern when X is pressed + controller.faceLeft().onTrue(Commands.runOnce(drive::stopWithX, drive)); - // Reset gyro to 0° when B button is pressed + // Reset the gyro heading to 0 degrees when B is pressed controller - .b() + .faceRight() .onTrue( Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), Rotation2d.ZERO)), drive) .ignoringDisable(true)); } @@ -149,4 +128,4 @@ private void configureButtonBindings() { public Command getAutonomousCommand() { return autoChooser.get(); } -} \ No newline at end of file +} diff --git a/src/main/java/first/robot/commands/DriveCommands.java b/src/main/java/first/robot/commands/DriveCommands.java index b400ded..2a0c4b4 100644 --- a/src/main/java/first/robot/commands/DriveCommands.java +++ b/src/main/java/first/robot/commands/DriveCommands.java @@ -10,9 +10,8 @@ import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.math.trajectory.TrapezoidProfile; import org.wpilib.math.util.Units; -import org.wpilib.driverstation.DriverStation; -import org.wpilib.driverstation.internal.DriverStationBackend; import org.wpilib.driverstation.Alliance; +import org.wpilib.driverstation.MatchState; import org.wpilib.system.Timer; import org.wpilib.command2.Command; import org.wpilib.command2.Commands; @@ -78,9 +77,7 @@ public static Command joystickDrive( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega * drive.getMaxAngularSpeedRadPerSec()); - boolean isFlipped = - DriverStationBackend.getAlliance().isPresent() - && DriverStationBackend.getAlliance().get() == Alliance.RED; + boolean isFlipped = MatchState.getAlliance().orElse(Alliance.BLUE) == Alliance.RED; drive.runVelocity( speeds.toRobotRelative( isFlipped @@ -128,9 +125,7 @@ public static Command joystickDriveAtAngle( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega); - boolean isFlipped = - DriverStationBackend.getAlliance().isPresent() - && DriverStationBackend.getAlliance().get() == Alliance.RED; + boolean isFlipped = MatchState.getAlliance().orElse(Alliance.BLUE) == Alliance.RED; drive.runVelocity( speeds.toRobotRelative( isFlipped diff --git a/src/main/java/first/robot/subsystems/drive/Drive.java b/src/main/java/first/robot/subsystems/drive/Drive.java index 7f9eb2a..973ed2f 100644 --- a/src/main/java/first/robot/subsystems/drive/Drive.java +++ b/src/main/java/first/robot/subsystems/drive/Drive.java @@ -18,10 +18,10 @@ import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N3; -import org.wpilib.driverstation.Alert; -import org.wpilib.driverstation.Alert.Level; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.Level; import first.robot.Constants.Mode; -import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.RobotState; import org.wpilib.system.Timer; import org.wpilib.command2.SubsystemBase; import java.util.concurrent.locks.Lock; @@ -29,7 +29,6 @@ import first.robot.Constants; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import first.robot.subsystems.drive.GyroIO.GyroIOInputs; public class Drive extends SubsystemBase { static final Lock odometryLock = new ReentrantLock(); @@ -37,7 +36,8 @@ public class Drive extends SubsystemBase { private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final Alert gyroDisconnectedAlert = - new Alert("Disconnected gyro, using kinematics as fallback.", Level.MEDIUM); + new Alert( + "gyroDisconnected", "Disconnected gyro, using kinematics as fallback.", Level.MEDIUM); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(DriveConstants.moduleTranslations); @@ -76,7 +76,7 @@ public void periodic() { odometryLock.unlock(); // Log empty setpoint states when disabled - if (DriverStation.isDisabled()) { + if (RobotState.isDisabled()) { Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleVelocity[] {}); Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleVelocity[] {}); } @@ -115,20 +115,24 @@ public void periodic() { public void runVelocity(ChassisVelocities speeds) { // Calculate module setpoints ChassisVelocities discreteSpeeds = speeds.discretize(Constants.loopPeriodSecs); - SwerveModuleVelocity[] setpointStates = kinematics.toSwerveModuleVelocities(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelVelocities(setpointStates, DriveConstants.maxLinearSpeed); + // desaturateWheelVelocities returns a new array rather than mutating in place, so its result + // must be used or the speed limit is silently ignored. + SwerveModuleVelocity[] setpointStates = + SwerveDriveKinematics.desaturateWheelVelocities( + kinematics.toSwerveModuleVelocities(discreteSpeeds), DriveConstants.maxLinearSpeed); // Log unoptimized setpoints and setpoint speeds Logger.recordOutput("SwerveStates/Setpoints", setpointStates); Logger.recordOutput("SwerveChassisVelocities/Setpoints", discreteSpeeds); - // Send setpoints to modules + // Send setpoints to modules, collecting the optimized states they actually applied + SwerveModuleVelocity[] optimizedStates = new SwerveModuleVelocity[4]; for (int i = 0; i < 4; i++) { - modules[i].runSetpoint(setpointStates[i]); + optimizedStates[i] = modules[i].runSetpoint(setpointStates[i]); } - // Log optimized setpoints (runSetpoint mutates each state) - Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); + // Log optimized setpoints + Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedStates); } /** Runs the drive in a straight line with the specified drive output. */ @@ -150,7 +154,7 @@ public void stop() { public void stopWithX() { Rotation2d[] headings = new Rotation2d[4]; for (int i = 0; i < 4; i++) { - headings[i] = DriveConstants.moduleTranslations[i].getAngle(); + headings[i] = DriveConstants.moduleTranslations[i].getAngle().orElse(Rotation2d.ZERO); } kinematics.resetHeadings(headings); stop(); @@ -158,7 +162,7 @@ public void stopWithX() { /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") - private SwerveModuleVelocity[] getModuleVelocities() { + public SwerveModuleVelocity[] getModuleVelocities() { SwerveModuleVelocity[] states = new SwerveModuleVelocity[4]; for (int i = 0; i < 4; i++) { states[i] = modules[i].getVelocity(); diff --git a/src/main/java/first/robot/subsystems/drive/DriveConstants.java b/src/main/java/first/robot/subsystems/drive/DriveConstants.java index f38b482..3574613 100644 --- a/src/main/java/first/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/first/robot/subsystems/drive/DriveConstants.java @@ -7,32 +7,42 @@ package first.robot.subsystems.drive; +import first.robot.Constants; +import first.robot.Constants.RobotType; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.util.Units; -import lombok.Builder; -import first.robot.Constants; -import first.robot.Constants.RobotType;; public class DriveConstants { public static final double trackWidthX = Units.inchesToMeters(20.75); public static final double trackWidthY = Units.inchesToMeters(20.75); public static final double driveBaseRadius = Math.hypot(trackWidthX / 2, trackWidthY / 2); public static final double maxLinearSpeed = 4.69; - public static final double maxAngularSpeed = 4.69 / driveBaseRadius; + public static final double maxAngularSpeed = maxLinearSpeed / driveBaseRadius; public static final double maxLinearAcceleration = 22.0; - public static final double driveKs = 5.0; - public static final double driveKv = 0.0; - public static final double driveKp = 35.0; + // Drive/turn gains for the real robot. Both motors are run with torque-current FOC, so all of + // these are in amps (or amps per unit of error), NOT volts. + public static final double driveKs = 5.0; // Amps to overcome static friction + public static final double driveKv = 0.0; // Amps per rad/sec of viscous drag + public static final double driveKp = 35.0; // Amps per rot/sec of velocity error public static final double driveKd = 0.0; - public static final double turnKp = 4000.0; + public static final double turnKp = 4000.0; // Amps per rotation of position error public static final double turnKd = 50.0; + // Gains for the physics simulation. Simulation is voltage based, so these are in volts. + public static final double driveSimKs = 0.03; + public static final double driveSimKv = 0.13; + public static final double driveSimKp = 0.05; + public static final double driveSimKd = 0.0; + public static final double turnSimKp = 8.0; + public static final double turnSimKd = 0.0; + /** Includes bumpers! */ public static final double robotWidth = Units.inchesToMeters(28.0) + 2 * Units.inchesToMeters(2.0); + /** Module locations, in the order FL, FR, BL, BR. +x is forward, +y is left. */ public static final Translation2d[] moduleTranslations = { new Translation2d(trackWidthX / 2, trackWidthY / 2), new Translation2d(trackWidthX / 2, -trackWidthY / 2), @@ -42,12 +52,20 @@ public class DriveConstants { public static final double wheelRadius = Units.inchesToMeters(1.9413001940413326); + /** + * Per-module hardware configuration, in the order FL, FR, BL, BR. + * + *

{@code encoderOffset} is written into the CANcoder's {@code MagnetSensor.MagnetOffset}, so + * after calibration a module pointing straight forward must read 0. To recalibrate: point every + * wheel forward (bevel gears all facing the same way), set the offsets below to zero, deploy, + * then read {@code /Drive/ModuleN/turnAbsolutePosition} and negate each value here. + */ public static final ModuleConfig[] moduleConfigs = { // FL ModuleConfig.builder() .driveMotorId(12) .turnMotorId(9) - .encoderChannel(2) + .encoderId(2) .encoderOffset(Rotation2d.fromRadians(0.9022009671847623)) .turnInverted(true) .encoderInverted(false) @@ -56,7 +74,7 @@ public class DriveConstants { ModuleConfig.builder() .driveMotorId(2) .turnMotorId(10) - .encoderChannel(3) + .encoderId(3) .encoderOffset(Rotation2d.fromRadians(1.6663099495963458)) .turnInverted(true) .encoderInverted(false) @@ -65,7 +83,7 @@ public class DriveConstants { ModuleConfig.builder() .driveMotorId(15) .turnMotorId(11) - .encoderChannel(4) + .encoderId(4) .encoderOffset(Rotation2d.fromRadians(-0.09896592242077659)) .turnInverted(true) .encoderInverted(false) @@ -74,7 +92,7 @@ public class DriveConstants { ModuleConfig.builder() .driveMotorId(3) .turnMotorId(8) - .encoderChannel(5) + .encoderId(5) .encoderOffset(Rotation2d.fromRadians(-3.051832863487227)) .turnInverted(true) .encoderInverted(false) @@ -85,12 +103,73 @@ public static class PigeonConstants { public static final int id = Constants.getRobot() == RobotType.DEVBOT ? 3 : 30; } - @Builder + /** + * Configuration for a single swerve module. + * + * @param driveMotorId CAN id of the drive TalonFX + * @param turnMotorId CAN id of the turn TalonFX + * @param encoderId CAN id of the steer CANcoder + * @param encoderOffset Offset applied to the CANcoder so that forward reads zero + * @param turnInverted Whether the turn motor is inverted + * @param encoderInverted Whether the CANcoder counts clockwise-positive + */ public record ModuleConfig( int driveMotorId, int turnMotorId, - int encoderChannel, + int encoderId, Rotation2d encoderOffset, boolean turnInverted, - boolean encoderInverted) {} + boolean encoderInverted) { + + public static Builder builder() { + return new Builder(); + } + + /** Hand-written builder, so the project needs no annotation processor for this. */ + public static final class Builder { + private int driveMotorId; + private int turnMotorId; + private int encoderId; + private Rotation2d encoderOffset = Rotation2d.ZERO; + private boolean turnInverted; + private boolean encoderInverted; + + public Builder driveMotorId(int driveMotorId) { + this.driveMotorId = driveMotorId; + return this; + } + + public Builder turnMotorId(int turnMotorId) { + this.turnMotorId = turnMotorId; + return this; + } + + public Builder encoderId(int encoderId) { + this.encoderId = encoderId; + return this; + } + + public Builder encoderOffset(Rotation2d encoderOffset) { + this.encoderOffset = encoderOffset; + return this; + } + + public Builder turnInverted(boolean turnInverted) { + this.turnInverted = turnInverted; + return this; + } + + public Builder encoderInverted(boolean encoderInverted) { + this.encoderInverted = encoderInverted; + return this; + } + + public ModuleConfig build() { + return new ModuleConfig( + driveMotorId, turnMotorId, encoderId, encoderOffset, turnInverted, encoderInverted); + } + } + } + + private DriveConstants() {} } diff --git a/src/main/java/first/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/first/robot/subsystems/drive/GyroIOPigeon2.java new file mode 100644 index 0000000..9dc1215 --- /dev/null +++ b/src/main/java/first/robot/subsystems/drive/GyroIOPigeon2.java @@ -0,0 +1,46 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package first.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import first.robot.Constants; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.util.Units; +import org.wpilib.units.measure.Angle; +import org.wpilib.units.measure.AngularVelocity; + +/** IMU implementation for the CTRE Pigeon 2 on the CAN bus. */ +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 pigeon = + new Pigeon2(DriveConstants.PigeonConstants.id, CANBus.systemcore(0)); + + private final StatusSignal yaw = pigeon.getYaw(); + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon2() { + ModuleIOTalonFX.tryUntilOk( + 5, () -> pigeon.getConfigurator().apply(new Pigeon2Configuration(), 0.25)); + ModuleIOTalonFX.tryUntilOk(5, () -> pigeon.setYaw(0.0, 0.25)); + + // Yaw feeds odometry every loop; the rate signal is only used for logging. + yaw.setUpdateFrequency(1.0 / Constants.loopPeriodSecs); + yawVelocity.setUpdateFrequency(50.0); + pigeon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).isOK(); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + } +} diff --git a/src/main/java/first/robot/subsystems/drive/Module.java b/src/main/java/first/robot/subsystems/drive/Module.java index 7114c07..9335d41 100644 --- a/src/main/java/first/robot/subsystems/drive/Module.java +++ b/src/main/java/first/robot/subsystems/drive/Module.java @@ -7,70 +7,94 @@ package first.robot.subsystems.drive; +import first.robot.Constants; +import first.robot.Constants.Mode; +import org.littletonrobotics.junction.Logger; +import org.wpilib.driverstation.RobotState; import org.wpilib.math.controller.SimpleMotorFeedforward; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.kinematics.SwerveModulePosition; import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.math.util.Units; -import org.wpilib.driverstation.Alert; -import org.wpilib.driverstation.Alert.Level; -import org.wpilib.driverstation.DriverStation; -import org.wpilib.driverstation.internal.DriverStationBackend; -import org.littletonrobotics.junction.Logger; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.Level; public class Module { private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; - private SimpleMotorFeedforward ffModel = - new SimpleMotorFeedforward(DriveConstants.driveKs, DriveConstants.driveKv); + private final SimpleMotorFeedforward ffModel; private final Alert driveDisconnectedAlert; private final Alert turnDisconnectedAlert; + private final Alert turnEncoderDisconnectedAlert; public Module(ModuleIO io, int index) { this.io = io; this.index = index; + + // Simulation is voltage controlled, the real robot is torque-current controlled, so the + // feedforward constants differ by more than just tuning. + ffModel = + Constants.getMode() == Mode.SIM + ? new SimpleMotorFeedforward(DriveConstants.driveSimKs, DriveConstants.driveSimKv) + : new SimpleMotorFeedforward(DriveConstants.driveKs, DriveConstants.driveKv); + driveDisconnectedAlert = new Alert( - "Disconnected drive motor on module " + Integer.toString(index) + ".", + "driveDisconnected" + index, + "Disconnected drive motor on module " + index + ".", Level.MEDIUM); turnDisconnectedAlert = new Alert( - "Disconnected turn motor on module " + Integer.toString(index) + ".", Level.MEDIUM); + "turnDisconnected" + index, + "Disconnected turn motor on module " + index + ".", + Level.MEDIUM); + turnEncoderDisconnectedAlert = + new Alert( + "turnEncoderDisconnected" + index, + "Disconnected steer CANcoder on module " + index + ".", + Level.MEDIUM); } public void periodic() { io.updateInputs(inputs); - Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + Logger.processInputs("Drive/Module" + index, inputs); // Update alerts driveDisconnectedAlert.set(!inputs.driveConnected); turnDisconnectedAlert.set(!inputs.turnConnected); + turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); // Coast when disabled - if (DriverStationBackend.isDisabled()) { + if (RobotState.isDisabled()) { io.coast(); } } - /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ - public void runSetpoint(SwerveModuleVelocity state) { - // Optimize velocity setpoint - state.optimize(getAngle()); - state.cosineScale(inputs.turnPosition); + /** + * Runs the module with the specified setpoint state. + * + * @return the optimized state that was actually applied, for logging + */ + public SwerveModuleVelocity runSetpoint(SwerveModuleVelocity state) { + // As of 2027, optimize() and cosineScale() are pure - they return a new state instead of + // mutating in place, so their results must be used. + SwerveModuleVelocity optimized = state.optimize(getAngle()).cosineScale(getAngle()); // Apply setpoints - double speedRadPerSec = state.velocity / DriveConstants.wheelRadius; + double speedRadPerSec = optimized.velocity / DriveConstants.wheelRadius; io.runDriveVelocity(speedRadPerSec, ffModel.calculate(speedRadPerSec)); - io.runTurnPosition(state.angle); + io.runTurnPosition(optimized.angle); + + return optimized; } /** Runs the module with the specified output while controlling to zero degrees. */ public void runCharacterization(double output) { io.runDriveOpenLoop(output); - io.runTurnPosition(new Rotation2d()); + io.runTurnPosition(Rotation2d.ZERO); } /** Disables all outputs to motors. */ @@ -100,7 +124,7 @@ public SwerveModulePosition getPosition() { } /** Returns the module state (turn angle and drive velocity). */ - public SwerveModuleVelocity getState() { + public SwerveModuleVelocity getVelocity() { return new SwerveModuleVelocity(getVelocityMetersPerSec(), getAngle()); } diff --git a/src/main/java/first/robot/subsystems/drive/ModuleIO.java b/src/main/java/first/robot/subsystems/drive/ModuleIO.java index 9d3b8c5..c977ad3 100644 --- a/src/main/java/first/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/first/robot/subsystems/drive/ModuleIO.java @@ -7,8 +7,8 @@ package first.robot.subsystems.drive; -import org.wpilib.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; +import org.wpilib.math.geometry.Rotation2d; public interface ModuleIO { @AutoLog @@ -21,8 +21,14 @@ public static class ModuleIOInputs { public double driveTorqueCurrentAmps = 0.0; public boolean turnConnected = false; + public boolean turnEncoderConnected = false; + + /** Raw CANcoder reading, before the calibration offset. Used to find encoder offsets. */ public Rotation2d turnAbsolutePosition = new Rotation2d(); + + /** Calibrated module heading. Zero means the wheel points straight forward. */ public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnSupplyCurrentAmps = 0.0; diff --git a/src/main/java/first/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/first/robot/subsystems/drive/ModuleIOSim.java index 90e9bba..4bbdfde 100644 --- a/src/main/java/first/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/first/robot/subsystems/drive/ModuleIOSim.java @@ -7,17 +7,17 @@ package first.robot.subsystems.drive; -import org.wpilib.math.util.MathUtil; +import first.robot.Constants; import org.wpilib.math.controller.PIDController; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.Models; import org.wpilib.simulation.DCMotorSim; -import first.robot.Constants; /** - * Physics sim implementation of module IO. The sim models are configured using a set of module - * constants from Phoenix. Simulation is always based on voltage control. + * Physics sim implementation of module IO. Simulation is always based on voltage control, so it + * uses the {@code *SimK*} gains from {@link DriveConstants} rather than the torque-current gains + * used on the real robot. */ public class ModuleIOSim implements ModuleIO { private static final DCMotor driveMotorModel = DCMotor.getKrakenX60Foc(1); @@ -30,14 +30,17 @@ public class ModuleIOSim implements ModuleIO { driveMotorModel); private final DCMotorSim turnSim = new DCMotorSim( - Models.singleJointedArmFromPhysicalConstants(turnMotorModel, 0.004, ModuleIOTalonFX.turnReduction), + Models.singleJointedArmFromPhysicalConstants( + turnMotorModel, 0.004, ModuleIOTalonFX.turnReduction), turnMotorModel); private boolean driveClosedLoop = false; private boolean turnClosedLoop = false; - private PIDController driveController = new PIDController(0, 0, 0); - private PIDController turnController = new PIDController(0, 0, 0); - private double driveFFVolts = 0; + private final PIDController driveController = + new PIDController(DriveConstants.driveSimKp, 0.0, DriveConstants.driveSimKd); + private final PIDController turnController = + new PIDController(DriveConstants.turnSimKp, 0.0, DriveConstants.turnSimKd); + private double driveFFVolts = 0.0; private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; @@ -71,11 +74,16 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.driveVelocityRadPerSec = driveSim.getAngularVelocity(); inputs.driveAppliedVolts = driveAppliedVolts; inputs.driveSupplyCurrentAmps = Math.abs(driveSim.getCurrentDraw()); + inputs.driveTorqueCurrentAmps = driveSim.getCurrentDraw(); inputs.turnConnected = true; + inputs.turnEncoderConnected = true; inputs.turnPosition = new Rotation2d(turnSim.getAngularPosition()); inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPosition()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocity(); + inputs.turnAppliedVolts = turnAppliedVolts; inputs.turnSupplyCurrentAmps = Math.abs(turnSim.getCurrentDraw()); + inputs.turnTorqueCurrentAmps = turnSim.getCurrentDraw(); } @Override @@ -102,4 +110,12 @@ public void runTurnPosition(Rotation2d rotation) { turnClosedLoop = true; turnController.setSetpoint(rotation.getRadians()); } + + @Override + public void coast() { + driveClosedLoop = false; + turnClosedLoop = false; + driveAppliedVolts = 0.0; + turnAppliedVolts = 0.0; + } } diff --git a/src/main/java/first/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/first/robot/subsystems/drive/ModuleIOTalonFX.java index 9847775..7f721cd 100644 --- a/src/main/java/first/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/first/robot/subsystems/drive/ModuleIOTalonFX.java @@ -11,41 +11,57 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import first.robot.Constants; +import java.util.function.Supplier; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.util.Units; import org.wpilib.units.measure.Angle; import org.wpilib.units.measure.AngularVelocity; import org.wpilib.units.measure.Current; import org.wpilib.units.measure.Voltage; -import org.wpilib.hardware.discrete.AnalogInput; -import java.util.function.Supplier; -import first.robot.Constants; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.Level; +/** + * Module IO implementation for two TalonFX motors plus a CANcoder for absolute steer position. + * + *

The CANcoder is configured with the module's calibration offset, so its absolute position is + * already the true module heading. That value seeds the turn TalonFX's internal rotor position once + * at startup, after which the TalonFX closes the steer loop against its own (much faster) sensor. + */ public class ModuleIOTalonFX implements ModuleIO { private static final double driveCurrentLimitAmps = 80; private static final double turnCurrentLimitAmps = 40; + + /** SDS MK4i L2: 6.12:1 drive, 150/7:1 steer. */ public static final double driveReduction = (50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0); + public static final double turnReduction = (150.0 / 7.0); + private static final CANBus canBus = CANBus.systemcore(0); + // Hardware objects private final TalonFX driveTalon; private final TalonFX turnTalon; - private final AnalogInput encoder; + private final CANcoder cancoder; // Config private final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); private final TalonFXConfiguration turnConfig = new TalonFXConfiguration(); - private final Rotation2d encoderOffset; + private final CANcoderConfiguration encoderConfig = new CANcoderConfiguration(); // Control requests private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0).withUpdateFreqHz(0); @@ -63,18 +79,44 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal driveTorqueCurrentAmps; // Inputs from turn motor - private final Supplier turnAbsolutePosition; private final StatusSignal turnPosition; private final StatusSignal turnVelocity; private final StatusSignal turnAppliedVolts; private final StatusSignal turnSupplyCurrentAmps; private final StatusSignal turnTorqueCurrentAmps; + // Inputs from CANcoder + private final StatusSignal turnAbsolutePosition; + + private final Rotation2d encoderOffset; + private final Alert seedFailedAlert; + public ModuleIOTalonFX(DriveConstants.ModuleConfig config) { - driveTalon = new TalonFX(config.driveMotorId(), CANBus.systemcore(0)); - turnTalon = new TalonFX(config.turnMotorId(), CANBus.systemcore(0)); - encoder = new AnalogInput(config.encoderChannel()); + driveTalon = new TalonFX(config.driveMotorId(), canBus); + turnTalon = new TalonFX(config.turnMotorId(), canBus); + cancoder = new CANcoder(config.encoderId(), canBus); encoderOffset = config.encoderOffset(); + + seedFailedAlert = + new Alert( + "steerSeedFailed" + config.turnMotorId(), + "Steer CANcoder " + + config.encoderId() + + " never reported a position; that module's steering will be wrong. Check CAN" + + " wiring and power-cycle.", + Level.HIGH); + + // Configure the CANcoder. The calibration offset lives on the device, so absolute position is + // the module heading directly. + encoderConfig.MagnetSensor.MagnetOffset = encoderOffset.getRotations(); + encoderConfig.MagnetSensor.SensorDirection = + config.encoderInverted() + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + // Report in [-0.5, 0.5) rotations, matching Rotation2d's range. + encoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; + tryUntilOk(5, () -> cancoder.getConfigurator().apply(encoderConfig, 0.25)); + // Configure drive motor driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveConfig.Slot0 = @@ -104,30 +146,37 @@ public ModuleIOTalonFX(DriveConstants.ModuleConfig config) { : InvertedValue.CounterClockwise_Positive; tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); - // Configure absolute encoder and set position on turn talon - turnAbsolutePosition = - () -> - Rotation2d.fromRadians((double) encoder.getValue() / 3200 * 2.0 * Math.PI) - .plus(encoderOffset); - tryUntilOk(5, () -> turnTalon.setPosition(turnAbsolutePosition.get().getRotations(), 0.25)); - - // Create drive status signals + // Create status signals drivePosition = driveTalon.getPosition(); driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); driveSupplyCurrentAmps = driveTalon.getSupplyCurrent(); driveTorqueCurrentAmps = driveTalon.getTorqueCurrent(); - // Create turn status signals turnPosition = turnTalon.getPosition(); turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); turnSupplyCurrentAmps = turnTalon.getSupplyCurrent(); turnTorqueCurrentAmps = turnTalon.getTorqueCurrent(); + turnAbsolutePosition = cancoder.getAbsolutePosition(); + + // Seed the turn motor from the absolute encoder. This MUST succeed or the module's steering is + // permanently offset, so block briefly for a real CAN frame rather than trusting a default 0. + boolean seeded = false; + for (int i = 0; i < 5; i++) { + if (turnAbsolutePosition.waitForUpdate(0.25).getStatus().isOK()) { + if (turnTalon.setPosition(turnAbsolutePosition.getValueAsDouble(), 0.25).isOK()) { + seeded = true; + break; + } + } + } + seedFailedAlert.set(!seeded); + // Configure periodic frames BaseStatusSignal.setUpdateFrequencyForAll( - 1.0 / Constants.loopPeriodSecs, drivePosition, turnPosition); + 1.0 / Constants.loopPeriodSecs, drivePosition, turnPosition, turnAbsolutePosition); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, driveVelocity, @@ -138,11 +187,11 @@ public ModuleIOTalonFX(DriveConstants.ModuleConfig config) { turnAppliedVolts, turnSupplyCurrentAmps, turnTorqueCurrentAmps); - ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); + ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon, cancoder); } @Override - public void updateInputs(ModuleIO.ModuleIOInputs inputs) { + public void updateInputs(ModuleIOInputs inputs) { // Update drive inputs inputs.driveConnected = BaseStatusSignal.refreshAll( @@ -158,6 +207,7 @@ public void updateInputs(ModuleIO.ModuleIOInputs inputs) { inputs.driveSupplyCurrentAmps = driveSupplyCurrentAmps.getValueAsDouble(); inputs.driveTorqueCurrentAmps = driveTorqueCurrentAmps.getValueAsDouble(); + // Update turn inputs inputs.turnConnected = BaseStatusSignal.refreshAll( turnPosition, @@ -166,12 +216,17 @@ public void updateInputs(ModuleIO.ModuleIOInputs inputs) { turnSupplyCurrentAmps, turnTorqueCurrentAmps) .isOK(); - inputs.turnAbsolutePosition = turnAbsolutePosition.get().minus(encoderOffset); inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); inputs.turnSupplyCurrentAmps = turnSupplyCurrentAmps.getValueAsDouble(); inputs.turnTorqueCurrentAmps = turnTorqueCurrentAmps.getValueAsDouble(); + + // Update CANcoder inputs. Log the raw (uncalibrated) angle, since that is what is needed to + // work out a new encoder offset. + inputs.turnEncoderConnected = BaseStatusSignal.refreshAll(turnAbsolutePosition).isOK(); + inputs.turnAbsolutePosition = + Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()).minus(encoderOffset); } @Override @@ -203,10 +258,13 @@ public void coast() { turnTalon.setControl(coast); } - public static void tryUntilOk(int maxAttempts, Supplier command) { + /** Attempts a device call until it reports success. Returns whether it ever succeeded. */ + public static boolean tryUntilOk(int maxAttempts, Supplier command) { for (int i = 0; i < maxAttempts; i++) { - var error = command.get(); - if (error.isOK()) break; + if (command.get().isOK()) { + return true; + } } + return false; } } diff --git a/src/test/java/first/robot/SimTestFixture.java b/src/test/java/first/robot/SimTestFixture.java new file mode 100644 index 0000000..e6d5fed --- /dev/null +++ b/src/test/java/first/robot/SimTestFixture.java @@ -0,0 +1,83 @@ +package first.robot; + +import static org.junit.jupiter.api.Assertions.assertTrue; + +import first.robot.subsystems.drive.Drive; +import first.robot.subsystems.drive.DriveConstants; +import first.robot.subsystems.drive.GyroIO; +import first.robot.subsystems.drive.ModuleIOSim; +import org.littletonrobotics.junction.Logger; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.hal.RobotMode; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.kinematics.ChassisVelocities; +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.simulation.DriverStationSim; + +/** + * Shared simulation fixture for the drive tests. + * + *

The HAL, the AdvantageKit {@code Logger}, and {@code Alert} ids are all JVM-wide, so all test + * classes in this source set share one initialization and one {@link Drive} instance. + */ +public final class SimTestFixture { + private static final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics(DriveConstants.moduleTranslations); + + private static Drive drive; + + private SimTestFixture() {} + + /** Initializes the HAL, logger, and drive exactly once per JVM. */ + public static synchronized Drive drive() { + if (drive == null) { + assertTrue(HAL.initialize(), "HAL failed to initialize"); + DriverStationSim.setRobotMode(RobotMode.TELEOPERATED); + DriverStationSim.setEnabled(true); + DriverStationSim.setDsAttached(true); + DriverStationSim.notifyNewData(); + + // AdvantageKit refuses to start outside a LoggedRobot; this is its supported escape hatch + // for custom robot bases and tests. + Logger.AdvancedHooks.disableRobotBaseCheck(); + Logger.disableConsoleCapture(); + Logger.start(); + + drive = + new Drive( + new GyroIO() {}, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + } + return drive; + } + + /** Advances one AdvantageKit logging cycle so processInputs/recordOutput have a valid frame. */ + public static void tick() { + Logger.AdvancedHooks.invokePeriodicBeforeUser(); + Logger.AdvancedHooks.invokePeriodicAfterUser(0, 0); + } + + /** + * Brings the drive to rest and zeroes its estimated pose, so each test starts from a known state + * with the field frame and the robot frame aligned. + */ + public static void reset() { + Drive d = drive(); + for (int i = 0; i < 150; i++) { + tick(); + d.periodic(); + d.stop(); + } + tick(); + d.periodic(); + d.setPose(new Pose2d()); + } + + /** Robot-relative chassis velocity implied by the current module states. */ + public static ChassisVelocities measured() { + return kinematics.toChassisVelocities(drive().getModuleVelocities()); + } +} diff --git a/src/test/java/first/robot/commands/JoystickDriveTest.java b/src/test/java/first/robot/commands/JoystickDriveTest.java new file mode 100644 index 0000000..1aaaef3 --- /dev/null +++ b/src/test/java/first/robot/commands/JoystickDriveTest.java @@ -0,0 +1,148 @@ +package first.robot.commands; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import first.robot.SimTestFixture; +import first.robot.subsystems.drive.Drive; +import first.robot.subsystems.drive.DriveConstants; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.wpilib.command2.Command; +import org.wpilib.math.kinematics.ChassisVelocities; + +/** + * Verifies the driver-facing path: gamepad axis values -> {@code DriveCommands.joystickDrive} -> + * chassis motion. Stick values here are already negated the same way {@code RobotContainer} negates + * them, so {@code stickX = +1} means "left stick pushed fully forward". + * + *

{@link SimTestFixture#reset()} zeroes the estimated heading before each test, so the + * field-relative command frame and the robot-relative measurement frame line up. + */ +class JoystickDriveTest { + private Drive drive; + + // Mutable stick state, read by the command's suppliers. + private double stickX; + private double stickY; + private double stickOmega; + + @BeforeEach + void setUp() { + drive = SimTestFixture.drive(); + stickX = 0.0; + stickY = 0.0; + stickOmega = 0.0; + SimTestFixture.reset(); + } + + /** Runs the joystick drive command for the given number of robot loops. */ + private ChassisVelocities run(int loops) { + Command command = + DriveCommands.joystickDrive(drive, () -> stickX, () -> stickY, () -> stickOmega); + command.initialize(); + for (int i = 0; i < loops; i++) { + SimTestFixture.tick(); + drive.periodic(); + command.execute(); + } + SimTestFixture.tick(); + drive.periodic(); + return SimTestFixture.measured(); + } + + @Test + void stickForwardDrivesForward() { + stickX = 1.0; + ChassisVelocities measured = run(200); + assertTrue( + measured.vx > DriveConstants.maxLinearSpeed * 0.7, + "full forward stick should drive near max speed, got " + measured.vx); + assertEquals(0.0, measured.vy, 0.3, "should not drift sideways"); + assertEquals(0.0, measured.omega, 0.3, "should not rotate"); + } + + @Test + void stickBackDrivesBackward() { + stickX = -1.0; + ChassisVelocities measured = run(200); + assertTrue( + measured.vx < -DriveConstants.maxLinearSpeed * 0.7, + "full back stick should drive backward, got " + measured.vx); + assertEquals(0.0, measured.vy, 0.3, "should not drift sideways"); + } + + @Test + void stickLeftStrafesLeft() { + stickY = 1.0; + ChassisVelocities measured = run(200); + assertTrue( + measured.vy > DriveConstants.maxLinearSpeed * 0.7, + "full left stick should strafe left (+y), got " + measured.vy); + assertEquals(0.0, measured.vx, 0.3); + } + + @Test + void stickRightStrafesRight() { + stickY = -1.0; + ChassisVelocities measured = run(200); + assertTrue( + measured.vy < -DriveConstants.maxLinearSpeed * 0.7, + "full right stick should strafe right (-y), got " + measured.vy); + assertEquals(0.0, measured.vx, 0.3); + } + + @Test + void rightStickRotatesCounterClockwise() { + stickOmega = 1.0; + ChassisVelocities measured = run(200); + assertTrue( + measured.omega > drive.getMaxAngularSpeedRadPerSec() * 0.5, + "full rotation stick should spin CCW, got " + measured.omega); + } + + @Test + void rightStickRotatesClockwise() { + stickOmega = -1.0; + ChassisVelocities measured = run(200); + assertTrue( + measured.omega < -drive.getMaxAngularSpeedRadPerSec() * 0.5, + "negative rotation stick should spin CW, got " + measured.omega); + } + + /** Small stick noise must not move the robot. */ + @Test + void deadbandIgnoresSmallInputs() { + stickX = 0.05; + stickY = 0.05; + stickOmega = 0.05; + ChassisVelocities measured = run(150); + assertEquals(0.0, measured.vx, 0.1, "inside the deadband the robot should hold still"); + assertEquals(0.0, measured.vy, 0.1); + assertEquals(0.0, measured.omega, 0.1); + } + + /** Diagonal stick input should drive along that diagonal. */ + @Test + void diagonalStickDrivesDiagonally() { + stickX = 0.7071; + stickY = 0.7071; + ChassisVelocities measured = run(220); + assertTrue(measured.vx > 1.0, "expected forward motion, got " + measured.vx); + assertTrue(measured.vy > 1.0, "expected leftward motion, got " + measured.vy); + assertEquals( + measured.vx, measured.vy, 0.4, "a 45 degree stick should drive along the 45 degree line"); + } + + /** Half stick should be clearly slower than full stick (the squared response curve). */ + @Test + void halfStickIsSlowerThanFullStick() { + stickX = 0.5; + double half = run(200).vx; + SimTestFixture.reset(); + stickX = 1.0; + double full = run(200).vx; + assertTrue(half > 0.1, "half stick should still move the robot, got " + half); + assertTrue(half < full * 0.6, "half stick (" + half + ") should be well under full (" + full + ")"); + } +} diff --git a/src/test/java/first/robot/subsystems/drive/DriveSimTest.java b/src/test/java/first/robot/subsystems/drive/DriveSimTest.java new file mode 100644 index 0000000..b8d37d3 --- /dev/null +++ b/src/test/java/first/robot/subsystems/drive/DriveSimTest.java @@ -0,0 +1,142 @@ +package first.robot.subsystems.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import first.robot.Constants; +import first.robot.SimTestFixture; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.ChassisVelocities; +import org.wpilib.math.kinematics.SwerveModuleVelocity; + +/** + * End-to-end checks that the swerve stack actually drives: kinematics -> module optimization -> + * simulated motors -> measured chassis velocity. These drive robot-relative, so the measured + * velocity is directly comparable to the commanded one. + */ +class DriveSimTest { + private Drive drive; + + @BeforeEach + void setUp() { + drive = SimTestFixture.drive(); + SimTestFixture.reset(); + } + + /** Runs the drive at a fixed velocity for a while and returns the measured chassis velocity. */ + private ChassisVelocities settleAt(ChassisVelocities target, int loops) { + for (int i = 0; i < loops; i++) { + SimTestFixture.tick(); + drive.periodic(); + drive.runVelocity(target); + } + SimTestFixture.tick(); + drive.periodic(); + return SimTestFixture.measured(); + } + + @Test + void drivesStraightForward() { + ChassisVelocities measured = settleAt(new ChassisVelocities(2.0, 0.0, 0.0), 200); + assertEquals(2.0, measured.vx, 0.25, "forward velocity should track the setpoint"); + assertEquals(0.0, measured.vy, 0.25, "should not drift sideways"); + assertEquals(0.0, measured.omega, 0.25, "should not rotate"); + } + + @Test + void drivesSideways() { + ChassisVelocities measured = settleAt(new ChassisVelocities(0.0, 1.5, 0.0), 200); + assertEquals(0.0, measured.vx, 0.25); + assertEquals(1.5, measured.vy, 0.25, "strafe velocity should track the setpoint"); + assertEquals(0.0, measured.omega, 0.25); + } + + @Test + void spinsInPlace() { + ChassisVelocities measured = settleAt(new ChassisVelocities(0.0, 0.0, 2.0), 200); + assertEquals(0.0, measured.vx, 0.25); + assertEquals(0.0, measured.vy, 0.25); + assertEquals(2.0, measured.omega, 0.3, "angular velocity should track the setpoint"); + } + + @Test + void drivesDiagonallyWhileRotating() { + ChassisVelocities target = new ChassisVelocities(1.5, 1.0, 1.0); + ChassisVelocities measured = settleAt(target, 250); + assertEquals(target.vx, measured.vx, 0.3); + assertEquals(target.vy, measured.vy, 0.3); + assertEquals(target.omega, measured.omega, 0.3); + } + + /** + * A request beyond what the modules can do must be scaled down as a whole, not clipped per + * module. This is what the discarded {@code desaturateWheelVelocities} result used to break. + */ + @Test + void desaturatesOverspeedRequests() { + settleAt(new ChassisVelocities(DriveConstants.maxLinearSpeed * 3.0, 0.0, 0.0), 200); + for (SwerveModuleVelocity state : drive.getModuleVelocities()) { + assertTrue( + Math.abs(state.velocity) <= DriveConstants.maxLinearSpeed * 1.1, + "module speed " + + state.velocity + + " exceeded the max of " + + DriveConstants.maxLinearSpeed); + } + } + + /** + * Reversing direction must flip the wheel rather than steer the module 180 degrees. This is what + * the discarded {@code optimize()} result used to break. + */ + @Test + void reversingFlipsWheelInsteadOfSteering() { + settleAt(new ChassisVelocities(2.0, 0.0, 0.0), 200); + ChassisVelocities measured = settleAt(new ChassisVelocities(-2.0, 0.0, 0.0), 200); + + assertEquals(-2.0, measured.vx, 0.25, "should track the reversed setpoint"); + for (SwerveModuleVelocity state : drive.getModuleVelocities()) { + // Steering stayed near the forward/backward axis; the wheel spins backwards instead. + double angleFromForward = Math.abs(state.angle.getDegrees()); + assertTrue( + angleFromForward < 15.0 || angleFromForward > 165.0, + "module should not be steered sideways, was " + state.angle.getDegrees() + " deg"); + assertTrue( + Math.signum(state.velocity) * Math.cos(state.angle.getRadians()) < 0.0, + "module should be driving backwards"); + } + } + + /** The period used for discretization and sim integration must match the real robot loop. */ + @Test + void loopPeriodMatchesRobotPeriod() { + assertEquals( + org.littletonrobotics.junction.LoggedRobot.defaultPeriodSecs, + Constants.loopPeriodSecs, + 1e-9, + "Constants.loopPeriodSecs must match the period handed to LoggedRobot"); + } + + /** stopWithX must park the modules in an X and hold still. */ + @Test + void stopWithXHoldsStill() { + settleAt(new ChassisVelocities(2.0, 0.0, 0.0), 100); + drive.stopWithX(); + for (int i = 0; i < 100; i++) { + SimTestFixture.tick(); + drive.periodic(); + drive.stop(); + } + SimTestFixture.tick(); + drive.periodic(); + + for (SwerveModuleVelocity state : drive.getModuleVelocities()) { + assertEquals(0.0, state.velocity, 0.15, "modules should be stopped"); + } + // Module headings should form the X pattern (45 degrees off axis). + Rotation2d flHeading = drive.getModuleVelocities()[0].angle; + assertEquals(45.0, Math.abs(flHeading.getDegrees()), 12.0, "front-left should sit at 45 deg"); + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 177ee85..1cb9533 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,35 +1,35 @@ { - "fileName": "AdvantageKit.json", - "name": "AdvantageKit", - "version": "27.0.0-alpha-4", - "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "wpilibYear": "2027_alpha5", - "mavenUrls": [ - "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" - ], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.akit", - "artifactId": "akit-java", - "version": "27.0.0-alpha-4" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.akit", - "artifactId": "akit-wpilibio", - "version": "27.0.0-alpha-4", - "skipInvalidPlatforms": false, - "isJar": false, - "validPlatforms": [ - "linuxsystemcore", - "linuxx86-64", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [] + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "27.0.0-alpha-5", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "wpilibYear": "2027_alpha7", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "27.0.0-alpha-5" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "27.0.0-alpha-5", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxsystemcore", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] } \ No newline at end of file diff --git a/vendordeps/CommandsV2.json b/vendordeps/CommandsV2.json index 8358b09..ae2b96b 100644 --- a/vendordeps/CommandsV2.json +++ b/vendordeps/CommandsV2.json @@ -3,7 +3,7 @@ "name": "Commands V2", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "mavenUrls": [], "jsonUrl": "", "conflictsWith": [ @@ -33,11 +33,9 @@ "skipInvalidPlatforms": true, "binaryPlatforms": [ "linuxsystemcore", - "linuxathena", - "linuxarm32", "linuxarm64", "windowsx86-64", - "windowsx86", + "windowsarm64", "linuxx86-64", "osxuniversal" ] diff --git a/vendordeps/PathplannerLibSystemCoreAlpha.json b/vendordeps/PathplannerLibSystemCoreAlpha.json index 24d3c8c..5c3c6bb 100644 --- a/vendordeps/PathplannerLibSystemCoreAlpha.json +++ b/vendordeps/PathplannerLibSystemCoreAlpha.json @@ -3,7 +3,7 @@ "name": "PathplannerLib", "version": "2027.0.0-alpha-3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], diff --git a/vendordeps/Phoenix6-26.50.0-alpha-1.json b/vendordeps/Phoenix6-26.50.0-alpha-1.json index f7db60a..5b7f1dc 100644 --- a/vendordeps/Phoenix6-26.50.0-alpha-1.json +++ b/vendordeps/Phoenix6-26.50.0-alpha-1.json @@ -2,7 +2,7 @@ "fileName": "Phoenix6-26.50.0-alpha-1.json", "name": "CTRE-Phoenix (v6)", "version": "26.50.0-alpha-1", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/"