diff --git a/build.gradle b/build.gradle index c2b7dff..0b1c988 100644 --- a/build.gradle +++ b/build.gradle @@ -55,6 +55,8 @@ def includeDesktopSupport = true // Also defines JUnit 5. dependencies { annotationProcessor wpi.java.deps.wpilibAnnotations() + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -115,4 +117,4 @@ gversion { dateFormat = "yyyy-MM-dd HH:mm:ss z" timeZone = "America/New_York" // Use preferred time zone indent = " " -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c576762..9e466a2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,5 +1,7 @@ package frc.robot; +import frc.robot.commands.DriveCommands; +import frc.robot.constants.Constants; import frc.robot.constants.OperatorConstants; import frc.robot.constants.PVConstants; import frc.robot.subsystems.FlywheelSubsystem; @@ -8,12 +10,15 @@ import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.PDHSubsystem; import frc.robot.subsystems.PhotonVisionSubsystem; -import frc.robot.subsystems.SwerveSubsystem; import frc.robot.subsystems.TurretSubsystem; +import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.drive.GyroIO; +import frc.robot.subsystems.drive.GyroIOPigeon2; +import frc.robot.subsystems.drive.ModuleIO; +import frc.robot.subsystems.drive.ModuleIOSim; +import frc.robot.subsystems.drive.ModuleIOSpark; import frc.robot.utils.GameHelpers; -import swervelib.SwerveInputStream; - import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.RPM; @@ -31,21 +36,19 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class RobotContainer { - private final SwerveSubsystem drivebase = new SwerveSubsystem(); + private final DriveSubsystem drive; // While these cameras aren't referenced directly, they still need to be // initalized in the RobotContainer @SuppressWarnings("unused") - private final PhotonVisionSubsystem blueCam = new PhotonVisionSubsystem("Blue_cam", - PVConstants.A_ROBOT_TO_CAMERA, drivebase::addVisionMeasurement); + private final PhotonVisionSubsystem blueCam; @SuppressWarnings("unused") - private final PhotonVisionSubsystem redCam = new PhotonVisionSubsystem("Red_cam", - PVConstants.C_ROBOT_TO_CAMERA, drivebase::addVisionMeasurement); + private final PhotonVisionSubsystem redCam; @SuppressWarnings("unused") - private final PhotonVisionSubsystem yellowCam = new PhotonVisionSubsystem("Yellow_cam", - PVConstants.D_ROBOT_TO_CAMERA, drivebase::addVisionMeasurement); + private final PhotonVisionSubsystem yellowCam; private final GameHelpers gameHelpers; @@ -62,34 +65,71 @@ public class RobotContainer { OperatorConstants.kDriverControllerPort); private final Command driveFieldOrientedAngularVelocity; - private final SwerveInputStream driveAngularVelocity; private final LoggedDashboardChooser autoChooser; public RobotContainer() { - gameHelpers = new GameHelpers(drivebase::getPose, drivebase::getRobotVelocity); + switch (Constants.currentMode) { + case REAL: + // Real robot, instantiate hardware IO implementations + drive = new DriveSubsystem( + new GyroIOPigeon2(), + new ModuleIOSpark(0), + new ModuleIOSpark(1), + new ModuleIOSpark(2), + new ModuleIOSpark(3)); + break; + + case SIM: + // Sim robot, instantiate physics sim IO implementations + drive = new DriveSubsystem( + new GyroIO() { + }, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + break; + + default: + // Replayed robot, disable IO implementations + drive = new DriveSubsystem( + new GyroIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }); + break; + } + + blueCam = new PhotonVisionSubsystem("Blue_cam", + PVConstants.A_ROBOT_TO_CAMERA, drive::addVisionMeasurement); + redCam = new PhotonVisionSubsystem("Red_cam", + PVConstants.C_ROBOT_TO_CAMERA, drive::addVisionMeasurement); + yellowCam = new PhotonVisionSubsystem("Yellow_cam", + PVConstants.D_ROBOT_TO_CAMERA, drive::addVisionMeasurement); + + gameHelpers = new GameHelpers(drive::getPose, drive::getRobotVelocity); flywheel = new FlywheelSubsystem(gameHelpers::getShotParameters); - turret = new TurretSubsystem(drivebase::getPose, gameHelpers::getVirtualTargetFieldAngle); + turret = new TurretSubsystem(drive::getPose, gameHelpers::getVirtualTargetFieldAngle); configurePathPlannerCommands(); - drivebase.resetOdometry(new Pose2d(1, 1, Rotation2d.kZero)); + drive.resetOdometry(new Pose2d(1, 1, Rotation2d.kZero)); // Coast during setup so the robot can be pushed into position setMotorBrake(false); - driveAngularVelocity = SwerveInputStream - .of(drivebase.getSwerveDrive(), - () -> -driverController.getLeftY(), - () -> -driverController.getLeftX()) - .withControllerRotationAxis( - () -> -driverController.getRightX()) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true) - .aim(() -> new Pose2d(gameHelpers.getVirtualTargetTranslation(), Rotation2d.kZero)) - .aimHeadingOffset(Rotation2d.fromDegrees(180.0)) - .aimHeadingOffset(true) - .aimWhile(driverController.b()); - driveFieldOrientedAngularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); - drivebase.setDefaultCommand(driveFieldOrientedAngularVelocity); + driveFieldOrientedAngularVelocity = drive.driveCommand( + () -> -driverController.getLeftY(), + () -> -driverController.getLeftX(), + () -> -driverController.getRightX(), + driverController.b(), + gameHelpers::getVirtualTargetTranslation, + Rotation2d.k180deg); + drive.setDefaultCommand(driveFieldOrientedAngularVelocity); // Signal readiness to the driver so they know when to shoot leds.configureLEDs(() -> turret.isAutoTrackingEnabled() @@ -107,6 +147,21 @@ public RobotContainer() { .andThen(Commands.sequence( flywheel.stop(), indexer.stop(), intake.stop(), turret.stopAutoTracking()))); + // Set up SysId routines + autoChooser.addOption( + "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive)); + autoChooser.addOption( + "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive)); + autoChooser.addOption( + "Drive SysId (Quasistatic Forward)", + drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive SysId (Quasistatic Reverse)", + drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); configureBindings(); } @@ -160,7 +215,7 @@ private void configureBindings() { .whileTrue(indexer.feedBackwards()); // b is bound to swerve aiming above driverController.x() - .whileTrue(drivebase.lockWheels()); + .whileTrue(drive.lockWheels()); // Rumble confirms the toggle so the driver doesn't have to check the dashboard driverController.y() .onTrue(flywheel.toggleVaryingRPM() @@ -185,7 +240,7 @@ private void configureBindings() { } public void setMotorBrake(boolean brake) { - drivebase.setMotorBrake(brake); + drive.setMotorBrake(brake); } /** @@ -196,11 +251,7 @@ public void setMotorBrake(boolean brake) { * the heading lock was never updated during auto driving. */ private Command autoDriving(Command drivingCommand) { - return drivingCommand.beforeStarting(() -> { - driveAngularVelocity.get(); - }).finallyDo(interrupted -> { - driveAngularVelocity.get(); - }).withName("AutoDriving"); + return drivingCommand.finallyDo(interrupted -> drive.stop()).withName("AutoDriving"); } /** @@ -235,4 +286,4 @@ public Command getAutonomousCommand() { return Commands.none(); } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java new file mode 100644 index 0000000..9f3b0a4 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -0,0 +1,371 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.drive.DriveConstants; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class DriveCommands { + private static final double DEADBAND = 0.1; + private static final double ANGLE_KP = 5.0; + private static final double ANGLE_KD = 0.4; + private static final double ANGLE_MAX_VELOCITY = 8.0; + private static final double ANGLE_MAX_ACCELERATION = 20.0; + 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 Translation2d(linearMagnitude, linearDirection); + } + + /** + * Field relative drive command using two joysticks (controlling linear and + * angular velocities). + */ + public static Command joystickDrive( + DriveSubsystem 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 + ChassisSpeeds speeds = new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec()); + + boolean isFlipped = DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive); + } + + /** + * Field relative drive command using joystick for linear control and PID for + * angular control. + * Possible use cases include snapping to an angle, aiming at a vision target, + * or controlling + * absolute rotation with a joystick. + */ + public static Command joystickDriveAtAngle( + DriveSubsystem 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 + ChassisSpeeds speeds = new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega); + + boolean isFlipped = DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive) + + // Reset PID controller when command starts + .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); + } + + /** + * Field relative drive command that can switch between joystick rotation and + * PID angle control while scheduled. + */ + public static Command joystickDriveMaybeAtAngle( + DriveSubsystem drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier, + BooleanSupplier useAngleSupplier, + 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); + boolean[] wasUsingAngle = new boolean[] { false }; + + // Construct command + return Commands.run( + () -> { + // Get linear velocity + Translation2d linearVelocity = getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), + ySupplier.getAsDouble()); + + double omega; + boolean useAngle = useAngleSupplier.getAsBoolean(); + if (useAngle) { + if (!wasUsingAngle[0]) { + angleController.reset(drive.getRotation().getRadians()); + } + + // Calculate angular speed + omega = angleController.calculate( + drive.getRotation().getRadians(), rotationSupplier.get().getRadians()); + } else { + // Apply rotation deadband + omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + + // Square rotation value for more precise control + omega = Math.copySign(omega * omega, omega) * drive.getMaxAngularSpeedRadPerSec(); + } + wasUsingAngle[0] = useAngle; + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega); + + boolean isFlipped = DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive) + + // Reset PID controller when command starts + .beforeStarting(() -> { + angleController.reset(drive.getRotation().getRadians()); + wasUsingAngle[0] = false; + }); + } + + /** + * Measures the velocity feedforward constants for the drive motors. + * + *

+ * This command should only be used in voltage control mode. + */ + public static Command feedforwardCharacterization(DriveSubsystem 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(DriveSubsystem 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 ChassisSpeeds(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( + () -> { + Rotation2d 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 = Rotation2d.kZero; + double gyroDelta = 0.0; + } +} diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 1048ba2..875a24e 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -15,7 +15,10 @@ public class Constants { /** TimedRobot loop period — 20ms matches the default CAN frame rate. */ public static final Time LOOP_TIME = Seconds.of(0.02); + public static final double LOOP_TIME_SECONDS = 0.02; + public static final double MAX_SPEED = Units.feetToMeters(10); + /** Shots closer than this hit the hub rim; shots farther lose accuracy. */ public static final double MIN_SHOT_DISTANCE = 1.9; public static final double MAX_SHOT_DISTANCE = 4.02; @@ -23,7 +26,6 @@ public class Constants { // AKit public static final Mode simMode = Mode.SIM; public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; - public static enum Mode { /** Running on a real robot. */ REAL, diff --git a/src/main/java/frc/robot/constants/SwerveConstants.java b/src/main/java/frc/robot/constants/SwerveConstants.java deleted file mode 100644 index 0194f31..0000000 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ /dev/null @@ -1,122 +0,0 @@ -package frc.robot.constants; - -// Adapted from previous YAGSL config files -public class SwerveConstants { - // Pigeon2 (IMU) (`swervedrive.json`) - public static final class Pigeon { - public static final int ID = 0; - public static final boolean INVERTED = false; - } - - // `controllerproperties.json` - public static final class ControllerProperties { - public static final double DEADBAND = 0.5; - - public static final double kP = 0.4; - public static final double kI = 0.0; - public static final double kD = 0.01; - } - - // `modules/physicalproperties.json` - public static final class PhysicalProperties { - public static final int OPTIMAL_VOLTAGE = 12; - public static final double ROBOT_MASS = 135.0; - public static final double WHEEL_GRIP_COEFFICIENT_OF_FRICTION = 1.0; - - // `currentLimit` - public static final int DRIVE_CURRENT_LIMIT = 40; - public static final int ANGLE_CURRENT_LIMIT = 20; - - // `conversionFactors.angle` - public static final double ANGLE_GEAR_RATIO = 21.4285714286; - public static final double ANGLE_FACTOR = 0.0; - - // `conversionFactors.drive` - public static final double DRIVE_DIAMETER = 4.0; - public static final double DRIVE_GEAR_RATIO = 6.75; - public static final double DRIVE_FACTOR = 0.0; - - // `rampRate` - public static final double DRIVE_RAMP_RATE = 0.1; - public static final double ANGLE_RAMP_RATE = 0.1; - } - - // `modules/pidfproperties.json` - public static final class PIDFProperties { - // `drive` - public static final double DRIVE_kP = 0.020236; - public static final double DRIVE_kI = 0.0; - public static final double DRIVE_kD = 0.0; - - // `angle` - public static final double ANGLE_kP = 0.016664; - public static final double ANGLE_kI = 0.0; - public static final double ANGLE_kD = 1.4884; - public static final double ANGLE_kF = 0.0; - public static final double angle_kIZ = 0.0; - } - - // `modules/backleft.json` - public static final class BackLeftModule { - public static final double LOCATION_FRONT = -10.875; - public static final double LOCATION_LEFT = 10.875; - - public static final double ABSOLUTE_ENCODER_OFFSET = 148.380; - - public static final int DRIVE_ID = 8; // SparkMax Neo - public static final int ANGLE_ID = 7; // SparkMax Neo - public static final int ENCODER_ID = 4; // CanCoder - - public static final boolean DRIVE_INVERTED = true; - public static final boolean ANGLE_INVERTED = true; - public static final boolean ENCODER_INVERTED = false; - } - - // `modules/backright.json` - public static final class BackRightModule { - public static final double LOCATION_FRONT = -10.875; - public static final double LOCATION_LEFT = -10.875; - - public static final double ABSOLUTE_ENCODER_OFFSET = 271.270; - - public static final int DRIVE_ID = 6; // SparkMax Neo - public static final int ANGLE_ID = 5; // SparkMax Neo - public static final int ENCODER_ID = 3; // CanCoder - - public static final boolean DRIVE_INVERTED = true; - public static final boolean ANGLE_INVERTED = true; - public static final boolean ENCODER_INVERTED = false; - } - - // `modules/frontleft.json` - public static final class FrontLeftModule { - public static final double LOCATION_FRONT = 10.875; - public static final double LOCATION_LEFT = 10.875; - - public static final double ABSOLUTE_ENCODER_OFFSET = 250.64; - - public static final int DRIVE_ID = 2; // SparkMax Neo - public static final int ANGLE_ID = 1; // SparkMax Neo - public static final int ENCODER_ID = 1; // CanCoder - - public static final boolean DRIVE_INVERTED = true; - public static final boolean ANGLE_INVERTED = true; - public static final boolean ENCODER_INVERTED = false; - } - - // `modules/frontright.json` - public static final class FrontRightModule { - public static final double LOCATION_FRONT = 10.875; - public static final double LOCATION_LEFT = -10.875; - - public static final double ABSOLUTE_ENCODER_OFFSET = 296.333; - - public static final int DRIVE_ID = 4; // SparkMax Neo - public static final int ANGLE_ID = 3; // SparkMax Neo - public static final int ENCODER_ID = 2; // CanCoder - - public static final boolean DRIVE_INVERTED = true; - public static final boolean ANGLE_INVERTED = true; - public static final boolean ENCODER_INVERTED = false; - } -} diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java deleted file mode 100644 index 4d1a8be..0000000 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ /dev/null @@ -1,246 +0,0 @@ -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.Meter; - -import frc.robot.constants.Constants; -import frc.robot.utils.AKTimeLogger; - -import java.io.File; -import java.util.Optional; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.math.Matrix; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.Filesystem; - -import swervelib.parser.SwerveParser; -import swervelib.SwerveDrive; -import swervelib.SwerveDriveTest; -import swervelib.math.SwerveMath; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; - -import org.littletonrobotics.junction.Logger; - -public class SwerveSubsystem extends SubsystemBase { - private File directory = new File(Filesystem.getDeployDirectory(), "swerve"); - private SwerveDrive swerveDrive; - - public SwerveSubsystem() { - try { - swerveDrive = new SwerveParser(directory) - .createSwerveDrive(Constants.MAX_SPEED, - new Pose2d( - new Translation2d( - Meter.of(0), - Meter.of(0)), - Rotation2d.fromDegrees(0))); - } catch (Exception e) { - throw new RuntimeException(e); - } - - // Compensates for the delay between calculating and applying speeds, - // which otherwise causes the robot to arc instead of driving straight. - swerveDrive.setChassisDiscretization(true, 0.02); - swerveDrive.setAngularVelocityCompensation(true, true, 0.1); - swerveDrive.setHeadingCorrection(true); - - SmartDashboard.putData("ZeroGyro", zeroGyro().withName("Zero Gyro")); - - setupPathPlanner(); - } - - @Override - public void periodic() { - Logger.recordOutput(getName() + "/SwerveModuleStates", swerveDrive.getStates()); - Logger.recordOutput(getName() + "/Pose", getPose()); - Logger.recordOutput(getName() + "/RobotVelocity", getRobotVelocity()); - } - - @Override - public void simulationPeriodic() { - } - - public SwerveDrive getSwerveDrive() { - return swerveDrive; - } - - public Pose2d getPose() { - return swerveDrive.getPose(); - } - - public ChassisSpeeds getRobotVelocity() { - return swerveDrive.getRobotVelocity(); - } - - /** - * Fuses a vision pose measurement into the Kalman filter. - * Standard deviations control how much to trust vision vs. wheel odometry. - */ - public void addVisionMeasurement( - Pose3d visionMeasurement, - double timestampSeconds, - Matrix stdDevs) { - AKTimeLogger.startTiming("Timing/Vision/AddMeasurementSeconds"); - swerveDrive.addVisionMeasurement(visionMeasurement.toPose2d(), timestampSeconds, stdDevs); - AKTimeLogger.endTiming("Timing/Vision/AddMeasurementSeconds"); - } - - public void driveFieldOriented(ChassisSpeeds velocity) { - swerveDrive.driveFieldOriented(velocity); - } - - public Command driveFieldOriented(Supplier velocity) { - return run(() -> { - swerveDrive.driveFieldOriented(velocity.get()); - }).withName("SwerveDriveFieldOriented"); - } - - /** - * Drives with a target heading direction via the right stick, allowing - * simultaneous translation and heading control. - */ - public Command driveCommand( - DoubleSupplier translationX, - DoubleSupplier translationY, - DoubleSupplier headingX, - DoubleSupplier headingY) { - return run(() -> { - Translation2d scaledInputs = SwerveMath.scaleTranslation( - new Translation2d( - translationX.getAsDouble(), - translationY.getAsDouble()), - 0.8); - - driveFieldOriented( - swerveDrive.swerveController.getTargetSpeeds( - scaledInputs.getX(), - scaledInputs.getY(), - headingX.getAsDouble(), - headingY.getAsDouble(), - swerveDrive.getOdometryHeading().getRadians(), - swerveDrive.getMaximumChassisVelocity())); - }).withName("SwerveDriveWithHeading"); - } - - public void setMotorBrake(boolean brake) { - swerveDrive.setMotorIdleMode(brake); - } - - public Command zeroGyro() { - return Commands - .runOnce(() -> swerveDrive.zeroGyro()) - .andThen(Commands.waitSeconds(0.5)) - .withName("SwerveZeroGyro"); - } - - /** Locks the wheels in an X formation so the robot can't be pushed. */ - public Command lockWheels() { - return run(() -> swerveDrive.lockPose()) - .withName("SwerveLockWheels"); - } - - public Rotation2d getGyro() { - return swerveDrive.getYaw(); - } - - public void resetOdometry(Pose2d pose) { - swerveDrive.resetOdometry(pose); - } - - /** SysId characterization for the angle (steering) motors. */ - public Command getAngleCharacterizationCommand() { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setAngleSysIdRoutine( - new SysIdRoutine.Config(), this, swerveDrive), - 3, 6, 3); - } - - /** SysId characterization for the drive (wheel) motors. */ - public Command getDriveCharacterizationCommand() { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setDriveSysIdRoutine( - new SysIdRoutine.Config(), this, swerveDrive, - 12, false), - 3, 4, 1.5); - } - - public void setupPathPlanner() { - RobotConfig config; - try { - config = RobotConfig.fromGUISettings(); - final boolean enableFeedforward = true; - - AutoBuilder.configure( - swerveDrive::getPose, // Supplier for current robot pose - swerveDrive::resetOdometry, // Consumer to reset odometry - swerveDrive::getRobotVelocity, // Supplier for current robot velocity - (speedsRobotRelative, moduleFeedForwards) -> { - if (enableFeedforward) { - swerveDrive.drive( - speedsRobotRelative, - swerveDrive.kinematics.toSwerveModuleStates(speedsRobotRelative), - moduleFeedForwards.linearForces()); - } else { - swerveDrive.setChassisSpeeds(speedsRobotRelative); - } - }, - new PPHolonomicDriveController( - new PIDConstants(4.5, 0.0, 0.2), - new PIDConstants(4.5, 0.0, 0.2) - ), - config, - // PathPlanner needs to mirror paths for the red alliance - () -> { - Optional alliance = DriverStation.getAlliance(); - - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - - return false; - }, - this); - } catch (Exception e) { - DriverStation.reportError( - "Failed to setup PathPlanner: " + e.getMessage(), e.getStackTrace()); - } - } - - public Command getAutonomousCommand(String pathName) { - return new PathPlannerAuto(pathName); - } - - public Command pathfindThenFollowPath(String pathName, PathConstraints constraints) { - try { - PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); - return AutoBuilder.pathfindThenFollowPath(path, constraints); - } catch (Exception e) { - DriverStation.reportError( - "Unable to load path: " + pathName, e.getStackTrace()); - - return Commands.none(); - } - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java new file mode 100644 index 0000000..66249d6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -0,0 +1,113 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.RobotConfig; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; + +public class DriveConstants { + public static final double maxSpeedMetersPerSec = 4.8; + public static final double odometryFrequency = 100.0; // Hz + public static final double trackWidth = Units.inchesToMeters(21.75); + public static final double wheelBase = Units.inchesToMeters(21.75); + public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0); + public static final Translation2d[] moduleTranslations = new Translation2d[] { + new Translation2d(trackWidth / 2.0, wheelBase / 2.0), + new Translation2d(trackWidth / 2.0, -wheelBase / 2.0), + new Translation2d(-trackWidth / 2.0, wheelBase / 2.0), + new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0) + }; + + // CANcoder absolute offsets from the prior YAGSL configuration + public static final Rotation2d frontLeftZeroRotation = Rotation2d.fromDegrees(250.64); + public static final Rotation2d frontRightZeroRotation = Rotation2d.fromDegrees(296.333); + public static final Rotation2d backLeftZeroRotation = Rotation2d.fromDegrees(148.380); + public static final Rotation2d backRightZeroRotation = Rotation2d.fromDegrees(271.270); + + // Device CAN IDs + public static final int pigeonCanId = 0; + + public static final int frontLeftDriveCanId = 2; + public static final int frontRightDriveCanId = 4; + public static final int backLeftDriveCanId = 8; + public static final int backRightDriveCanId = 6; + + public static final int frontLeftTurnCanId = 1; + public static final int frontRightTurnCanId = 3; + public static final int backLeftTurnCanId = 7; + public static final int backRightTurnCanId = 5; + + public static final int frontLeftCancoderCanId = 1; + public static final int frontRightCancoderCanId = 2; + public static final int backLeftCancoderCanId = 4; + public static final int backRightCancoderCanId = 3; + + // Drive motor configuration + public static final boolean driveInverted = true; + public static final int driveMotorCurrentLimit = 40; + public static final double wheelRadiusMeters = Units.inchesToMeters(2.0); + public static final double driveMotorReduction = 6.75; + public static final DCMotor driveGearbox = DCMotor.getNEO(1); + + // Drive encoder configuration + public static final double driveEncoderPositionFactor = 2 * Math.PI / driveMotorReduction; // Rotor Rotations -> + // Wheel Radians + public static final double driveEncoderVelocityFactor = (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> + // Wheel Rad/Sec + + // Drive PID configuration + public static final double driveKp = 0.020236; + public static final double driveKd = 0.0; + public static final double driveKs = 0.0; + public static final double driveKv = 0.1; + public static final double driveSimP = 0.05; + public static final double driveSimD = 0.0; + public static final double driveSimKs = 0.0; + public static final double driveSimKv = 0.0789; + + // Turn motor configuration + public static final boolean turnInverted = true; + public static final int turnMotorCurrentLimit = 20; + public static final double turnMotorReduction = 21.4285714286; + public static final DCMotor turnGearbox = DCMotor.getNEO(1); + + // Turn encoder configuration + public static final boolean turnEncoderInverted = false; + public static final double turnEncoderPositionFactor = 2 * Math.PI / turnMotorReduction; // Rotor Rotations -> + // Module Radians + public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0 / turnMotorReduction; // Rotor RPM -> + // Module Rad/Sec + + // Turn PID configuration + public static final double turnKp = 0.016664; + public static final double turnKd = 1.4884; + public static final double turnSimP = 8.0; + public static final double turnSimD = 0.0; + public static final double turnPIDMinInput = 0; // Radians + public static final double turnPIDMaxInput = 2 * Math.PI; // Radians + + // PathPlanner configuration + public static final double robotMassKg = Units.lbsToKilograms(135.0); + public static final double robotMOI = 6.883; // Retained until measured for this chassis + public static final double wheelCOF = 1.0; + public static final RobotConfig ppConfig = new RobotConfig( + robotMassKg, + robotMOI, + new ModuleConfig( + wheelRadiusMeters, + maxSpeedMetersPerSec, + wheelCOF, + driveGearbox.withReduction(driveMotorReduction), + driveMotorCurrentLimit, + 1), + moduleTranslations); +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java new file mode 100644 index 0000000..f0ce49f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -0,0 +1,392 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.hal.FRCNetComm.tInstances; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; +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.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +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.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.commands.DriveCommands; +import frc.robot.constants.Constants; +import frc.robot.utils.LocalADStarAK; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class DriveSubsystem 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 SysIdRoutine sysId; + private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", + AlertType.kError); + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); + private Rotation2d rawGyroRotation = Rotation2d.kZero; + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, + lastModulePositions, Pose2d.kZero); + + public DriveSubsystem( + 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); + + // Usage reporting for swerve template + HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit); + + // Start odometry thread + SparkOdometryThread.getInstance().start(); + + // Configure AutoBuilder for PathPlanner + AutoBuilder.configure( + this::getPose, + this::setPose, + this::getChassisSpeeds, + this::runVelocity, + new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), + ppConfig, + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this); + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput("Odometry/Trajectory", activePath.toArray(new Pose2d[0])); + }); + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + + // Configure SysId + sysId = new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + } + + @Override + public void periodic() { + odometryLock.lock(); // Prevents odometry updates while reading data + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + + for (Module module : modules) { + module.periodic(); + } + + odometryLock.unlock(); + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (Module module : modules) { + module.stop(); + } + } + + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + // Update odometry + double[] sampleTimestamps = modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + + for (int i = 0; i < sampleCount; i++) { + // 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].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + + // Apply update + poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + } + + // Update gyro alert + gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Constants.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 = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, maxSpeedMetersPerSec); + + // Log unoptimized setpoints + 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] = moduleTranslations[i].getAngle(); + } + + kinematics.resetHeadings(headings); + stop(); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)) + .withTimeout(1.0) + .andThen(sysId.quasistatic(direction)); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); + } + + /** + * 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 rad/sec. */ + 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(); + } + + /** Returns the current robot-relative chassis speeds. */ + public ChassisSpeeds getRobotVelocity() { + return getChassisSpeeds(); + } + + /** Resets the current odometry pose. */ + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** Resets the current odometry pose. */ + public void resetOdometry(Pose2d pose) { + setPose(pose); + } + + /** Adds a new timestamped vision measurement. */ + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + poseEstimator.addVisionMeasurement( + visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); + } + + /** Adds a new timestamped vision measurement. */ + public void addVisionMeasurement( + Pose3d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + addVisionMeasurement(visionRobotPoseMeters.toPose2d(), timestampSeconds, visionMeasurementStdDevs); + } + + /** + * Drives field-relative with joystick rotation until the trigger is held, then + * locks heading toward the supplied target translation. + */ + public Command driveCommand( + DoubleSupplier translationX, + DoubleSupplier translationY, + DoubleSupplier omega, + BooleanSupplier aimAtTarget, + Supplier targetTranslationSupplier, + Rotation2d angleOffset) { + return DriveCommands.joystickDriveMaybeAtAngle( + this, + translationX, + translationY, + omega, + aimAtTarget, + () -> targetTranslationSupplier.get().minus(getPose().getTranslation()).getAngle().plus(angleOffset)) + .withName("DriveCommand"); + } + + /** Locks the modules in an X pattern while the command is scheduled. */ + public Command lockWheels() { + return run(this::stopWithX).withName("DriveLockWheels"); + } + + /** Sets every module to brake or coast mode. */ + public void setMotorBrake(boolean brake) { + for (Module module : modules) { + module.setBrakeMode(brake); + } + } + + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return maxSpeedMetersPerSec; + } + + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return maxSpeedMetersPerSec / driveBaseRadius; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java new file mode 100644 index 0000000..de6a1de --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -0,0 +1,25 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.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 = Rotation2d.kZero; + public double yawVelocityRadPerSec = 0.0; + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + } + + public default void updateInputs(GyroIOInputs inputs) { + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java new file mode 100644 index 0000000..c9ba49b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -0,0 +1,60 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +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 java.util.Queue; + +/** IO implementation for Pigeon 2. */ +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 pigeon = new Pigeon2(pigeonCanId); + private final StatusSignal yaw = pigeon.getYaw(); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon2() { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + + yaw.setUpdateFrequency(odometryFrequency); + yawVelocity.setUpdateFrequency(50.0); + + pigeon.optimizeBusUtilization(); + + yawTimestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); + StatusSignal yawClone = yaw.clone(); // Status signals are not thread-safe + yawPositionQueue = SparkOdometryThread.getInstance() + .registerSignal(() -> yawClone.refresh().getValueAsDouble()); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + + inputs.odometryYawTimestamps = yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryYawPositions = yawPositionQueue.stream() + .map((Double value) -> Rotation2d.fromDegrees(value)) + .toArray(Rotation2d[]::new); + + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java new file mode 100644 index 0000000..d27fb40 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -0,0 +1,139 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +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.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import org.littletonrobotics.junction.Logger; + +public class Module { + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + private final Alert driveDisconnectedAlert; + private final Alert turnDisconnectedAlert; + private final Alert turnAbsoluteDisconnectedAlert; + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + + 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); + turnAbsoluteDisconnectedAlert = new Alert( + "Disconnected turn CANCoder on module " + Integer.toString(index) + ".", AlertType.kError); + } + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // Calculate positions for odometry + int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together + odometryPositions = new SwerveModulePosition[sampleCount]; + + for (int i = 0; i < sampleCount; i++) { + double positionMeters = inputs.odometryDrivePositionsRad[i] * wheelRadiusMeters; + Rotation2d angle = inputs.odometryTurnPositions[i]; + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + + // Update alerts + driveDisconnectedAlert.set(!inputs.driveConnected); + turnDisconnectedAlert.set(!inputs.turnConnected); + turnAbsoluteDisconnectedAlert.set(!inputs.turnAbsoluteConnected); + } + + /** + * 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 + io.setDriveVelocity(state.speedMetersPerSecond / wheelRadiusMeters); + io.setTurnPosition(state.angle); + } + + /** + * Runs the module with the specified output while controlling to zero degrees. + */ + public void runCharacterization(double output) { + io.setDriveOpenLoop(output); + io.setTurnPosition(Rotation2d.kZero); + } + + /** Disables all outputs to motors. */ + public void stop() { + io.setDriveOpenLoop(0.0); + io.setTurnOpenLoop(0.0); + } + + /** Sets the module motors to brake or coast mode. */ + public void setBrakeMode(boolean enabled) { + io.setBrakeMode(enabled); + } + + /** 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 * wheelRadiusMeters; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * wheelRadiusMeters; + } + + /** 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 positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } + + /** Returns the module position in radians. */ + public double getWheelRadiusCharacterizationPosition() { + return inputs.drivePositionRad; + } + + /** Returns the module velocity in rad/sec. */ + public double getFFCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java new file mode 100644 index 0000000..109b451 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -0,0 +1,59 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.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 driveCurrentAmps = 0.0; + + public boolean turnConnected = false; + public Rotation2d turnPosition = Rotation2d.kZero; + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnCurrentAmps = 0.0; + + public boolean turnAbsoluteConnected = false; + public Rotation2d turnAbsolutePosition = Rotation2d.kZero; + + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** 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 setDriveOpenLoop(double output) { + } + + /** Run the turn motor at the specified open loop value. */ + public default void setTurnOpenLoop(double output) { + } + + /** Run the drive motor at the specified velocity. */ + public default void setDriveVelocity(double velocityRadPerSec) { + } + + /** Run the turn motor to the specified rotation. */ + public default void setTurnPosition(Rotation2d rotation) { + } + + /** Sets the module motors to brake or coast mode. */ + public default void setBrakeMode(boolean enabled) { + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java new file mode 100644 index 0000000..6c40beb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -0,0 +1,112 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +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.LinearSystemId; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +/** Physics sim implementation of module IO. */ +public class ModuleIOSim implements ModuleIO { + private final DCMotorSim driveSim; + private final DCMotorSim turnSim; + + private boolean driveClosedLoop = false; + private boolean turnClosedLoop = false; + private PIDController driveController = new PIDController(driveSimP, 0, driveSimD); + private PIDController turnController = new PIDController(turnSimP, 0, turnSimD); + private double driveFFVolts = 0.0; + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + public ModuleIOSim() { + // Create drive and turn sim models + driveSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(driveGearbox, 0.025, driveMotorReduction), + driveGearbox); + turnSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(turnGearbox, 0.004, turnMotorReduction), + turnGearbox); + + // 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.getAngularVelocityRadPerSec()); + } else { + driveController.reset(); + } + if (turnClosedLoop) { + turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad()); + } 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(0.02); + turnSim.update(0.02); + + // Update drive inputs + inputs.driveConnected = true; + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + + // Update turn inputs + inputs.turnConnected = true; + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + inputs.turnAbsoluteConnected = true; + inputs.turnAbsolutePosition = inputs.turnPosition; + + // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't + // matter) + inputs.odometryTimestamps = new double[] { Timer.getFPGATimestamp() }; + inputs.odometryDrivePositionsRad = new double[] { inputs.drivePositionRad }; + inputs.odometryTurnPositions = new Rotation2d[] { inputs.turnPosition }; + } + + @Override + public void setDriveOpenLoop(double output) { + driveClosedLoop = false; + driveAppliedVolts = output; + } + + @Override + public void setTurnOpenLoop(double output) { + turnClosedLoop = false; + turnAppliedVolts = output; + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + driveClosedLoop = true; + driveFFVolts = driveSimKs * Math.signum(velocityRadPerSec) + driveSimKv * velocityRadPerSec; + driveController.setSetpoint(velocityRadPerSec); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnClosedLoop = true; + turnController.setSetpoint(rotation.getRadians()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java new file mode 100644 index 0000000..330c61f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -0,0 +1,295 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.utils.SparkUtil.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import java.util.Queue; +import java.util.function.DoubleSupplier; + +/** + * Module IO implementation for Spark Max drive/turn controllers with a CANcoder + * steering reference. + */ +public class ModuleIOSpark implements ModuleIO { + private final Rotation2d zeroRotation; + + // Hardware objects + private final SparkMax driveSpark; + private final SparkMax turnSpark; + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnEncoder; + private final CANcoder turnAbsoluteEncoder; + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnAbsolutePositionOdometry; + + // Closed loop controllers + private final SparkClosedLoopController driveController; + private final SparkClosedLoopController turnController; + + // Queue inputs from odometry thread + private final Queue timestampQueue; + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private final Debouncer turnConnectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private final Debouncer turnAbsoluteConnectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + public ModuleIOSpark(int module) { + zeroRotation = switch (module) { + case 0 -> frontLeftZeroRotation; + case 1 -> frontRightZeroRotation; + case 2 -> backLeftZeroRotation; + case 3 -> backRightZeroRotation; + default -> Rotation2d.kZero; + }; + + driveSpark = new SparkMax( + switch (module) { + case 0 -> frontLeftDriveCanId; + case 1 -> frontRightDriveCanId; + case 2 -> backLeftDriveCanId; + case 3 -> backRightDriveCanId; + default -> 0; + }, + MotorType.kBrushless); + + turnSpark = new SparkMax( + switch (module) { + case 0 -> frontLeftTurnCanId; + case 1 -> frontRightTurnCanId; + case 2 -> backLeftTurnCanId; + case 3 -> backRightTurnCanId; + default -> 0; + }, + MotorType.kBrushless); + + turnAbsoluteEncoder = new CANcoder( + switch (module) { + case 0 -> frontLeftCancoderCanId; + case 1 -> frontRightCancoderCanId; + case 2 -> backLeftCancoderCanId; + case 3 -> backRightCancoderCanId; + default -> 0; + }); + + driveEncoder = driveSpark.getEncoder(); + turnEncoder = turnSpark.getEncoder(); + turnAbsolutePosition = turnAbsoluteEncoder.getAbsolutePosition(); + turnAbsolutePositionOdometry = turnAbsolutePosition.clone(); + driveController = driveSpark.getClosedLoopController(); + turnController = turnSpark.getClosedLoopController(); + + // Configure drive motor + SparkMaxConfig driveConfig = new SparkMaxConfig(); + driveConfig + .inverted(driveInverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(driveMotorCurrentLimit) + .voltageCompensation(12.0); + driveConfig.encoder + .positionConversionFactor(driveEncoderPositionFactor) + .velocityConversionFactor(driveEncoderVelocityFactor) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + driveConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(driveKp, 0.0, driveKd); + driveConfig.signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + + tryUntilOk( + driveSpark, + 5, + () -> driveSpark.configure( + driveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + tryUntilOk(driveSpark, 5, () -> driveEncoder.setPosition(0.0)); + + // Configure turn motor + SparkMaxConfig turnConfig = new SparkMaxConfig(); + turnConfig + .inverted(turnInverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(turnMotorCurrentLimit) + .voltageCompensation(12.0); + turnConfig.encoder + .inverted(turnEncoderInverted) + .positionConversionFactor(turnEncoderPositionFactor) + .velocityConversionFactor(turnEncoderVelocityFactor) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + turnConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .positionWrappingEnabled(true) + .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) + .pid(turnKp, 0.0, turnKd); + turnConfig.signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + + tryUntilOk( + turnSpark, + 5, + () -> turnSpark.configure( + turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + turnAbsolutePosition.setUpdateFrequency(odometryFrequency); + turnAbsoluteEncoder.optimizeBusUtilization(); + syncTurnEncoderWithAbsolute(); + + // Create odometry queues + timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); + drivePositionQueue = SparkOdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition); + turnPositionQueue = SparkOdometryThread.getInstance() + .registerSignal(() -> Rotation2d.fromRotations(turnAbsolutePositionOdometry.refresh().getValueAsDouble()) + .minus(zeroRotation) + .getRadians()); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Update drive inputs + sparkStickyFault = false; + + ifOk(driveSpark, driveEncoder::getPosition, (value) -> inputs.drivePositionRad = value); + ifOk(driveSpark, driveEncoder::getVelocity, (value) -> inputs.driveVelocityRadPerSec = value); + ifOk( + driveSpark, + new DoubleSupplier[] { driveSpark::getAppliedOutput, driveSpark::getBusVoltage }, + (values) -> inputs.driveAppliedVolts = values[0] * values[1]); + ifOk(driveSpark, driveSpark::getOutputCurrent, (value) -> inputs.driveCurrentAmps = value); + + inputs.driveConnected = driveConnectedDebounce.calculate(!sparkStickyFault); + + // Update turn inputs + sparkStickyFault = false; + + StatusCode turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + ifOk(turnSpark, turnEncoder::getPosition, (value) -> inputs.turnPosition = new Rotation2d(value)); + ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); + ifOk( + turnSpark, + new DoubleSupplier[] { turnSpark::getAppliedOutput, turnSpark::getBusVoltage }, + (values) -> inputs.turnAppliedVolts = values[0] * values[1]); + ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); + inputs.turnAbsolutePosition = getAbsoluteAngle(); + + inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault); + inputs.turnAbsoluteConnected = turnAbsoluteConnectedDebounce.calculate(turnEncoderStatus.equals(StatusCode.OK)); + + // Update odometry inputs + inputs.odometryTimestamps = timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryTurnPositions = turnPositionQueue.stream() + .map(Rotation2d::new) + .toArray(Rotation2d[]::new); + + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveOpenLoop(double output) { + driveSpark.setVoltage(output); + } + + @Override + public void setTurnOpenLoop(double output) { + turnSpark.setVoltage(output); + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + double ffVolts = driveKs * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; + driveController.setSetpoint( + velocityRadPerSec, + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + ffVolts, + ArbFFUnits.kVoltage); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + double setpoint = MathUtil.inputModulus( + rotation.getRadians(), turnPIDMinInput, turnPIDMaxInput); + turnController.setSetpoint(setpoint, ControlType.kPosition); + } + + @Override + public void setBrakeMode(boolean enabled) { + IdleMode idleMode = enabled ? IdleMode.kBrake : IdleMode.kCoast; + + SparkMaxConfig driveConfig = new SparkMaxConfig(); + driveConfig.idleMode(idleMode); + tryUntilOk( + driveSpark, + 5, + () -> driveSpark.configure( + driveConfig, + ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters)); + + SparkMaxConfig turnConfig = new SparkMaxConfig(); + turnConfig.idleMode(idleMode); + tryUntilOk( + turnSpark, + 5, + () -> turnSpark.configure( + turnConfig, + ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters)); + } + + private void syncTurnEncoderWithAbsolute() { + turnAbsolutePosition.refresh(); + tryUntilOk(turnSpark, 5, () -> turnEncoder.setPosition(getAbsoluteAngleRadians())); + } + + private Rotation2d getAbsoluteAngle() { + return Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()).minus(zeroRotation); + } + + private double getAbsoluteAngleRadians() { + return getAbsoluteAngle().getRadians(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java new file mode 100644 index 0000000..7a37a98 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java @@ -0,0 +1,141 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import com.revrobotics.REVLibError; +import com.revrobotics.spark.SparkBase; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.function.DoubleSupplier; + +/** + * Provides an interface for asynchronously reading high-frequency measurements + * to a set of queues. + * + *

+ * This version includes an overload for Spark signals, which checks for errors + * to ensure that + * all measurements in the sample are valid. + */ +public class SparkOdometryThread { + private final List sparks = new ArrayList<>(); + private final List sparkSignals = new ArrayList<>(); + private final List genericSignals = new ArrayList<>(); + private final List> sparkQueues = new ArrayList<>(); + private final List> genericQueues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + + private static SparkOdometryThread instance = null; + private Notifier notifier = new Notifier(this::run); + + public static SparkOdometryThread getInstance() { + if (instance == null) { + instance = new SparkOdometryThread(); + } + + return instance; + } + + private SparkOdometryThread() { + notifier.setName("OdometryThread"); + } + + public void start() { + if (timestampQueues.size() > 0) { + notifier.startPeriodic(1.0 / DriveConstants.odometryFrequency); + } + } + + /** Registers a Spark signal to be read from the thread. */ + public Queue registerSignal(SparkBase spark, DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + DriveSubsystem.odometryLock.lock(); + + try { + sparks.add(spark); + sparkSignals.add(signal); + sparkQueues.add(queue); + } finally { + DriveSubsystem.odometryLock.unlock(); + } + + return queue; + } + + /** Registers a generic signal to be read from the thread. */ + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + DriveSubsystem.odometryLock.lock(); + + try { + genericSignals.add(signal); + genericQueues.add(queue); + } finally { + DriveSubsystem.odometryLock.unlock(); + } + + return queue; + } + + /** Returns a new queue that returns timestamp values for each sample. */ + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + DriveSubsystem.odometryLock.lock(); + + try { + timestampQueues.add(queue); + } finally { + DriveSubsystem.odometryLock.unlock(); + } + + return queue; + } + + private void run() { + // Save new data to queues + DriveSubsystem.odometryLock.lock(); + + try { + // Get sample timestamp + double timestamp = RobotController.getFPGATime() / 1e6; + + // Read Spark values, mark invalid in case of error + double[] sparkValues = new double[sparkSignals.size()]; + boolean isValid = true; + + for (int i = 0; i < sparkSignals.size(); i++) { + sparkValues[i] = sparkSignals.get(i).getAsDouble(); + + if (sparks.get(i).getLastError() != REVLibError.kOk) { + isValid = false; + } + } + + // If valid, add values to queues + if (isValid) { + for (int i = 0; i < sparkSignals.size(); i++) { + sparkQueues.get(i).offer(sparkValues[i]); + } + + for (int i = 0; i < genericSignals.size(); i++) { + genericQueues.get(i).offer(genericSignals.get(i).getAsDouble()); + } + + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } + } finally { + DriveSubsystem.odometryLock.unlock(); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java deleted file mode 100644 index 3e87abd..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems.swerve; - -public class Swerve { - -} diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java deleted file mode 100644 index e75df91..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.robot.subsystems.swerve.gyro; - -import org.littletonrobotics.junction.AutoLog; - -// https://github.com/pittsfordrobotics/ChargedUp2023/blob/master/src/main/java/com/team3181/frc2023/subsystems/swerve/GyroIO.java -public interface GyroIO { - @AutoLog - class GyroIOInputs { - public boolean connected = false; - public double yawPositionRad = 0.0; - public double yawVelocityRadPerSec = 0.0; - public double pitchPositionRad = 0.0; - public double pitchVelocityRadPerSec = 0.0; - public double rollPositionRad = 0.0; - public double rollVelocityRadPerSec = 0.0; - } - - default void zeroGyro() {} - default void updateInputs(GyroIOInputs inputs) {} -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOPigeon.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOPigeon.java deleted file mode 100644 index 5dcfddd..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOPigeon.java +++ /dev/null @@ -1,53 +0,0 @@ -package frc.robot.subsystems.swerve.gyro; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.hardware.Pigeon2; - -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import frc.robot.constants.SwerveConstants; - -// https://github.com/pittsfordrobotics/ChargedUp2023/blob/master/src/main/java/com/team3181/frc2023/subsystems/swerve/GyroIOPigeon.java -public class GyroIOPigeon implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(SwerveConstants.Pigeon.ID); - private final Pigeon2Configuration config = new Pigeon2Configuration(); - - private final StatusSignal yaw = pigeon.getYaw(); - private final StatusSignal pitch = pigeon.getPitch(); - private final StatusSignal roll = pigeon.getRoll(); - private final StatusSignal rollVelocity = pigeon.getAngularVelocityXWorld(); - private final StatusSignal pitchVelocity = pigeon.getAngularVelocityYWorld(); - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - - public GyroIOPigeon() { - config.MountPose.MountPoseRoll = 0; - pigeon.getConfigurator().apply(config); - pigeon.setYaw(0); - } - - @Override - public void zeroGyro() { - pigeon.setYaw(0); - } - - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll( - yaw, - pitch, - roll, - rollVelocity, - pitchVelocity, - yawVelocity).isOK(); - - inputs.yawPositionRad = Units.degreesToRadians(yaw.getValueAsDouble()); // ccw+ - inputs.pitchPositionRad = Units.degreesToRadians(-pitch.getValueAsDouble()); // up+ down- - inputs.rollPositionRad = Units.degreesToRadians(roll.getValueAsDouble()); // cw+ - inputs.rollVelocityRadPerSec = Units.degreesToRadians(rollVelocity.getValueAsDouble()); // cw+ - inputs.pitchVelocityRadPerSec = Units.degreesToRadians(-pitchVelocity.getValueAsDouble()); // up+ down- - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); // ccw+ - } -} diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java deleted file mode 100644 index 432def3..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot.subsystems.swerve.gyro; - -// https://github.com/pittsfordrobotics/ChargedUp2023/blob/master/src/main/java/com/team3181/frc2023/subsystems/swerve/GyroIOSim.java -public class GyroIOSim implements GyroIO { - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = false; - inputs.yawPositionRad = 0; - inputs.yawVelocityRadPerSec = 0; - inputs.pitchPositionRad = 0; - inputs.rollPositionRad = 0; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIO.java deleted file mode 100644 index 7369118..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIO.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.subsystems.swerve.module; - -import org.littletonrobotics.junction.AutoLog; - -// https://github.com/pittsfordrobotics/ChargedUp2023/blob/master/src/main/java/com/team3181/frc2023/subsystems/swerve/SwerveModuleIO.java -public interface ModuleIO { - @AutoLog - class SwerveModuleIOInputs { - public double drivePositionMeters = 0.0; - public double driveVelocityMetersPerSec = 0.0; - public double driveAppliedVolts = 0.0; - public double driveCurrentAmps = 0.0; - public double driveTempCelsius = 0.0; - - public double steerAbsolutePositionRad = 0.0; - public double steerOffsetAbsolutePositionRad = 0.0; - public double steerAbsoluteVelocityRadPerSec = 0.0; - public double steerAppliedVolts = 0.0; - public double steerCurrentAmps = 0.0; - public double steerTempCelsius = 0.0; - } - - default void updateInputs(SwerveModuleIOInputs inputs) {} - - default void setDriveVoltage(double voltage) {} - - default void setSteerVoltage(double voltage) {} - - default void setModuleState(BetterSwerveModuleState state, boolean isOpenLoop) {} - - default void stopMotors() {} - - default void setDriveBrakeMode(boolean enable) {} - - default void setSteerBrakeMode(boolean enable) {} -} diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java deleted file mode 100644 index dfc949c..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems.swerve.module; - -public class ModuleIOSim { - -} diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSparkMax.java deleted file mode 100644 index 759dda7..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSparkMax.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems.swerve.module; - -public class ModuleIOSparkMax { - -} diff --git a/src/main/java/frc/robot/utils/LocalADStarAK.java b/src/main/java/frc/robot/utils/LocalADStarAK.java new file mode 100644 index 0000000..8f3de25 --- /dev/null +++ b/src/main/java/frc/robot/utils/LocalADStarAK.java @@ -0,0 +1,167 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.utils; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.LocalADStar; +import com.pathplanner.lib.pathfinding.Pathfinder; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +// NOTE: This file is available at +// https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d + +public class LocalADStarAK implements Pathfinder { + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was + * retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the + * pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an + * obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle + * it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. + * Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to + * change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + List pathPoints = new ArrayList<>(); + + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add( + new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} diff --git a/src/main/java/frc/robot/utils/SparkUtil.java b/src/main/java/frc/robot/utils/SparkUtil.java new file mode 100644 index 0000000..019f7ce --- /dev/null +++ b/src/main/java/frc/robot/utils/SparkUtil.java @@ -0,0 +1,61 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.utils; + +import com.revrobotics.REVLibError; +import com.revrobotics.spark.SparkBase; +import java.util.function.Consumer; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class SparkUtil { + /** Stores whether any error was has been detected by other utility methods. */ + public static boolean sparkStickyFault = false; + + /** Processes a value from a Spark only if the value is valid. */ + public static void ifOk(SparkBase spark, DoubleSupplier supplier, DoubleConsumer consumer) { + double value = supplier.getAsDouble(); + + if (spark.getLastError() == REVLibError.kOk) { + consumer.accept(value); + } else { + sparkStickyFault = true; + } + } + + /** Processes a value from a Spark only if the value is valid. */ + public static void ifOk( + SparkBase spark, DoubleSupplier[] suppliers, Consumer consumer) { + double[] values = new double[suppliers.length]; + + for (int i = 0; i < suppliers.length; i++) { + values[i] = suppliers[i].getAsDouble(); + + if (spark.getLastError() != REVLibError.kOk) { + sparkStickyFault = true; + return; + } + } + + consumer.accept(values); + } + + /** Attempts to run the command until no error is produced. */ + public static void tryUntilOk(SparkBase spark, int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + REVLibError error = command.get(); + + if (error == REVLibError.kOk) { + break; + } else { + sparkStickyFault = true; + } + } + } +} \ No newline at end of file diff --git a/vendordeps/ReduxLib-2026.1.2.json b/vendordeps/ReduxLib-2026.1.2.json deleted file mode 100644 index 46e64b2..0000000 --- a/vendordeps/ReduxLib-2026.1.2.json +++ /dev/null @@ -1,69 +0,0 @@ -{ - "fileName": "ReduxLib-2026.1.2.json", - "name": "ReduxLib", - "version": "2026.1.2", - "frcYear": "2026", - "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", - "mavenUrls": [ - "https://maven.reduxrobotics.com/" - ], - "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2026.json", - "javaDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-java", - "version": "2026.1.2" - } - ], - "jniDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-fifo", - "version": "2026.1.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-cpp", - "version": "2026.1.2", - "libName": "ReduxLib", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - }, - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-fifo", - "version": "2026.1.2", - "libName": "reduxfifo", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/yagsl-2026.4.1.json b/vendordeps/yagsl-2026.4.1.json deleted file mode 100644 index cb6aa20..0000000 --- a/vendordeps/yagsl-2026.4.1.json +++ /dev/null @@ -1,46 +0,0 @@ -{ - "fileName": "yagsl-2026.4.1.json", - "name": "YAGSL", - "version": "2026.4.1", - "frcYear": "2026", - "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", - "mavenUrls": [ - "https://yet-another-software-suite.github.io/YAGSL/releases/", - "https://repo1.maven.org/maven2" - ], - "jsonUrl": "https://yet-another-software-suite.github.io/YAGSL/yagsl.json", - "javaDependencies": [ - { - "groupId": "swervelib", - "artifactId": "YAGSL-java", - "version": "2026.4.1" - }, - { - "groupId": "org.dyn4j", - "artifactId": "dyn4j", - "version": "5.0.2" - } - ], - "requires": [ - { - "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", - "errorMessage": "ReduxLib is required!", - "offlineFileName": "ReduxLib.json", - "onlineUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2026.json" - }, - { - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "errorMessage": "REVLib is required!", - "offlineFileName": "REVLib.json", - "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json" - }, - { - "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", - "errorMessage": "ThriftyLib is required!", - "offlineFileName": "ThriftyLib.json", - "onlineUrl": "https://docs.home.thethriftybot.com/ThriftyLib-2026.json" - } - ], - "jniDependencies": [], - "cppDependencies": [] -} \ No newline at end of file