From e3cb4d77ffa61b3850868943f4b6be19aae1f003 Mon Sep 17 00:00:00 2001 From: Scaratech Date: Fri, 17 Apr 2026 07:58:56 -0400 Subject: [PATCH 1/5] feat: replace `SwerveSubsystem.java` and YAGSL with custom AK-compatable swerve --- build.gradle | 4 +- src/main/java/frc/robot/RobotContainer.java | 35 +- .../java/frc/robot/constants/Constants.java | 4 +- .../frc/robot/constants/SwerveConstants.java | 94 ++-- .../frc/robot/subsystems/SwerveSubsystem.java | 246 ----------- .../frc/robot/subsystems/swerve/Swerve.java | 400 +++++++++++++++++- .../robot/subsystems/swerve/gyro/GyroIO.java | 7 +- .../{GyroIOPigeon.java => GyroIOPigeon2.java} | 4 +- .../subsystems/swerve/gyro/GyroIOSim.java | 22 +- .../subsystems/swerve/module/ModuleIO.java | 40 +- .../subsystems/swerve/module/ModuleIOSim.java | 75 +++- .../swerve/module/ModuleIOSparkMax.java | 232 +++++++++- src/main/java/frc/robot/utils/SparkUtil.java | 53 +++ vendordeps/ReduxLib-2026.1.2.json | 69 --- vendordeps/yagsl-2026.4.1.json | 46 -- 15 files changed, 879 insertions(+), 452 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/SwerveSubsystem.java rename src/main/java/frc/robot/subsystems/swerve/gyro/{GyroIOPigeon.java => GyroIOPigeon2.java} (96%) create mode 100644 src/main/java/frc/robot/utils/SparkUtil.java delete mode 100644 vendordeps/ReduxLib-2026.1.2.json delete mode 100644 vendordeps/yagsl-2026.4.1.json 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..4f1156e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,12 +8,10 @@ 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.swerve.Swerve; import frc.robot.utils.GameHelpers; -import swervelib.SwerveInputStream; - import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.RPM; @@ -33,7 +31,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class RobotContainer { - private final SwerveSubsystem drivebase = new SwerveSubsystem(); + private final Swerve drivebase = new Swerve(); // While these cameras aren't referenced directly, they still need to be // initalized in the RobotContainer @@ -62,7 +60,6 @@ public class RobotContainer { OperatorConstants.kDriverControllerPort); private final Command driveFieldOrientedAngularVelocity; - private final SwerveInputStream driveAngularVelocity; private final LoggedDashboardChooser autoChooser; public RobotContainer() { @@ -75,20 +72,12 @@ public RobotContainer() { drivebase.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); + driveFieldOrientedAngularVelocity = drivebase.driveCommand( + () -> -driverController.getLeftY(), + () -> -driverController.getLeftX(), + () -> -driverController.getRightX(), + driverController.b(), + () -> new Pose2d(gameHelpers.getVirtualTargetTranslation(), Rotation2d.kZero)); drivebase.setDefaultCommand(driveFieldOrientedAngularVelocity); // Signal readiness to the driver so they know when to shoot @@ -196,11 +185,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 -> drivebase.stop()).withName("AutoDriving"); } /** @@ -235,4 +220,4 @@ public Command getAutonomousCommand() { return Commands.none(); } } -} \ No newline at end of file +} 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 index 0194f31..3f3b294 100644 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/constants/SwerveConstants.java @@ -1,27 +1,52 @@ package frc.robot.constants; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.util.Units; + // Adapted from previous YAGSL config files public class SwerveConstants { - // Pigeon2 (IMU) (`swervedrive.json`) + public static final double MAX_LINEAR_SPEED_METERS_PER_SEC = Constants.MAX_SPEED; + public static final double MAX_ANGULAR_SPEED_RAD_PER_SEC = 2.0 * Math.PI; + + public static final double DRIVE_WHEEL_RADIUS_METERS = Units.inchesToMeters( + PhysicalProperties.DRIVE_WHEEL_DIAMETER_INCHES) / 2.0; + public static final double DRIVE_POSITION_FACTOR_METERS_PER_ROTATION = 2.0 * Math.PI * DRIVE_WHEEL_RADIUS_METERS + / PhysicalProperties.DRIVE_GEAR_RATIO; + public static final double DRIVE_VELOCITY_FACTOR_METERS_PER_SEC_PER_RPM = DRIVE_POSITION_FACTOR_METERS_PER_ROTATION + / 60.0; + public static final double ANGLE_POSITION_FACTOR_RAD_PER_ROTATION = 2.0 * Math.PI + / PhysicalProperties.ANGLE_GEAR_RATIO; + public static final double ANGLE_VELOCITY_FACTOR_RAD_PER_SEC_PER_RPM = ANGLE_POSITION_FACTOR_RAD_PER_ROTATION + / 60.0; + + public static final Translation2d FRONT_LEFT_LOCATION = new Translation2d( + Units.inchesToMeters(FrontLeftModule.LOCATION_FRONT_INCHES), + Units.inchesToMeters(FrontLeftModule.LOCATION_LEFT_INCHES)); + public static final Translation2d FRONT_RIGHT_LOCATION = new Translation2d( + Units.inchesToMeters(FrontRightModule.LOCATION_FRONT_INCHES), + Units.inchesToMeters(FrontRightModule.LOCATION_LEFT_INCHES)); + public static final Translation2d BACK_LEFT_LOCATION = new Translation2d( + Units.inchesToMeters(BackLeftModule.LOCATION_FRONT_INCHES), + Units.inchesToMeters(BackLeftModule.LOCATION_LEFT_INCHES)); + public static final Translation2d BACK_RIGHT_LOCATION = new Translation2d( + Units.inchesToMeters(BackRightModule.LOCATION_FRONT_INCHES), + Units.inchesToMeters(BackRightModule.LOCATION_LEFT_INCHES)); + + public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics( + FRONT_LEFT_LOCATION, + FRONT_RIGHT_LOCATION, + BACK_LEFT_LOCATION, + BACK_RIGHT_LOCATION); + + // Pigeon2 (IMU/Gyro) (`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; + public static final int OPTIMAL_VOLTAGE_VOLTS = 12; // `currentLimit` public static final int DRIVE_CURRENT_LIMIT = 40; @@ -29,16 +54,14 @@ public static final class PhysicalProperties { // `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_WHEEL_DIAMETER_INCHES = 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; + public static final double DRIVE_RAMP_RATE_SECONDS = 0.1; + public static final double ANGLE_RAMP_RATE_SECONDS = 0.1; } // `modules/pidfproperties.json` @@ -52,16 +75,14 @@ public static final class PIDFProperties { 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 LOCATION_FRONT_INCHES = -10.875; + public static final double LOCATION_LEFT_INCHES = 10.875; - public static final double ABSOLUTE_ENCODER_OFFSET = 148.380; + public static final double ABSOLUTE_ENCODER_OFFSET_DEGREES = 148.380; public static final int DRIVE_ID = 8; // SparkMax Neo public static final int ANGLE_ID = 7; // SparkMax Neo @@ -74,10 +95,10 @@ public static final class BackLeftModule { // `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 LOCATION_FRONT_INCHES = -10.875; + public static final double LOCATION_LEFT_INCHES = -10.875; - public static final double ABSOLUTE_ENCODER_OFFSET = 271.270; + public static final double ABSOLUTE_ENCODER_OFFSET_DEGREES = 271.270; public static final int DRIVE_ID = 6; // SparkMax Neo public static final int ANGLE_ID = 5; // SparkMax Neo @@ -90,10 +111,10 @@ public static final class BackRightModule { // `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 LOCATION_FRONT_INCHES = 10.875; + public static final double LOCATION_LEFT_INCHES = 10.875; - public static final double ABSOLUTE_ENCODER_OFFSET = 250.64; + public static final double ABSOLUTE_ENCODER_OFFSET_DEGREES = 250.64; public static final int DRIVE_ID = 2; // SparkMax Neo public static final int ANGLE_ID = 1; // SparkMax Neo @@ -106,10 +127,10 @@ public static final class FrontLeftModule { // `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 LOCATION_FRONT_INCHES = 10.875; + public static final double LOCATION_LEFT_INCHES = -10.875; - public static final double ABSOLUTE_ENCODER_OFFSET = 296.333; + public static final double ABSOLUTE_ENCODER_OFFSET_DEGREES = 296.333; public static final int DRIVE_ID = 4; // SparkMax Neo public static final int ANGLE_ID = 3; // SparkMax Neo @@ -119,4 +140,9 @@ public static final class FrontRightModule { public static final boolean ANGLE_INVERTED = true; public static final boolean ENCODER_INVERTED = false; } + + public static final class Simulation { + public static final double kP = 0.05; + public static final double kD = 0.0; + } } 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/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index 3e87abd..d57a917 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -1,5 +1,401 @@ package frc.robot.subsystems.swerve; -public class Swerve { - +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 edu.wpi.first.math.Matrix; +import edu.wpi.first.math.controller.PIDController; +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.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.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.constants.OperatorConstants; +import frc.robot.constants.SwerveConstants; +import frc.robot.subsystems.swerve.gyro.GyroIO; +import frc.robot.subsystems.swerve.gyro.GyroIOInputsAutoLogged; +import frc.robot.subsystems.swerve.gyro.GyroIOPigeon2; +import frc.robot.subsystems.swerve.gyro.GyroIOSim; +import frc.robot.subsystems.swerve.module.ModuleIO; +import frc.robot.subsystems.swerve.module.ModuleIOInputsAutoLogged; +import frc.robot.subsystems.swerve.module.ModuleIOSim; +import frc.robot.subsystems.swerve.module.ModuleIOSparkMax; +import frc.robot.utils.AKTimeLogger; +import java.util.Optional; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +public class Swerve extends SubsystemBase { + private static final int FRONT_LEFT = 0; + private static final int FRONT_RIGHT = 1; + private static final int BACK_LEFT = 2; + private static final int BACK_RIGHT = 3; + + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules; + private final SwerveDrivePoseEstimator poseEstimator; + private final PIDController headingController = new PIDController(5.0, 0.0, 0.1); + + private ChassisSpeeds robotRelativeSetpoint = new ChassisSpeeds(); + private Rotation2d rawGyroRotation = Rotation2d.kZero; + + public Swerve() { + switch (Constants.currentMode) { + case REAL: + gyroIO = new GyroIOPigeon2(); + + modules = new Module[] { + new Module("FrontLeft", new ModuleIOSparkMax( + SwerveConstants.FrontLeftModule.DRIVE_ID, + SwerveConstants.FrontLeftModule.ANGLE_ID, + SwerveConstants.FrontLeftModule.ENCODER_ID, + SwerveConstants.FrontLeftModule.ABSOLUTE_ENCODER_OFFSET_DEGREES, + SwerveConstants.FrontLeftModule.DRIVE_INVERTED, + SwerveConstants.FrontLeftModule.ANGLE_INVERTED, + SwerveConstants.FrontLeftModule.ENCODER_INVERTED)), + new Module("FrontRight", new ModuleIOSparkMax( + SwerveConstants.FrontRightModule.DRIVE_ID, + SwerveConstants.FrontRightModule.ANGLE_ID, + SwerveConstants.FrontRightModule.ENCODER_ID, + SwerveConstants.FrontRightModule.ABSOLUTE_ENCODER_OFFSET_DEGREES, + SwerveConstants.FrontRightModule.DRIVE_INVERTED, + SwerveConstants.FrontRightModule.ANGLE_INVERTED, + SwerveConstants.FrontRightModule.ENCODER_INVERTED)), + new Module("BackLeft", new ModuleIOSparkMax( + SwerveConstants.BackLeftModule.DRIVE_ID, + SwerveConstants.BackLeftModule.ANGLE_ID, + SwerveConstants.BackLeftModule.ENCODER_ID, + SwerveConstants.BackLeftModule.ABSOLUTE_ENCODER_OFFSET_DEGREES, + SwerveConstants.BackLeftModule.DRIVE_INVERTED, + SwerveConstants.BackLeftModule.ANGLE_INVERTED, + SwerveConstants.BackLeftModule.ENCODER_INVERTED)), + new Module("BackRight", new ModuleIOSparkMax( + SwerveConstants.BackRightModule.DRIVE_ID, + SwerveConstants.BackRightModule.ANGLE_ID, + SwerveConstants.BackRightModule.ENCODER_ID, + SwerveConstants.BackRightModule.ABSOLUTE_ENCODER_OFFSET_DEGREES, + SwerveConstants.BackRightModule.DRIVE_INVERTED, + SwerveConstants.BackRightModule.ANGLE_INVERTED, + SwerveConstants.BackRightModule.ENCODER_INVERTED)) + }; + + break; + case SIM: + gyroIO = new GyroIOSim(); + + modules = new Module[] { + new Module("FrontLeft", new ModuleIOSim()), + new Module("FrontRight", new ModuleIOSim()), + new Module("BackLeft", new ModuleIOSim()), + new Module("BackRight", new ModuleIOSim()) + }; + + break; + case REPLAY: + default: + gyroIO = new GyroIO() { + }; + + modules = new Module[] { + new Module("FrontLeft", new ModuleIO() { + }), + new Module("FrontRight", new ModuleIO() { + }), + new Module("BackLeft", new ModuleIO() { + }), + new Module("BackRight", new ModuleIO() { + }) + }; + + break; + } + + headingController.enableContinuousInput(-Math.PI, Math.PI); + + poseEstimator = new SwerveDrivePoseEstimator( + SwerveConstants.KINEMATICS, + rawGyroRotation, + getModulePositions(), + Pose2d.kZero); + + SmartDashboard.putData("ZeroGyro", zeroGyro().withName("Zero Gyro")); + setupPathPlanner(); + } + + @Override + public void periodic() { + if (gyroIO instanceof GyroIOSim gyroSim) { + rawGyroRotation = rawGyroRotation.plus( + Rotation2d.fromRadians(robotRelativeSetpoint.omegaRadiansPerSecond * Constants.LOOP_TIME_SECONDS)); + gyroSim.setYaw(rawGyroRotation.getRadians(), robotRelativeSetpoint.omegaRadiansPerSecond); + } + + gyroIO.updateInputs(gyroInputs); + Logger.processInputs(getName() + "/Gyro", gyroInputs); + + for (Module module : modules) { + module.updateInputs(); + } + + rawGyroRotation = Rotation2d.fromRadians(gyroInputs.yawPositionRad); + poseEstimator.update(rawGyroRotation, getModulePositions()); + + Logger.recordOutput(getName() + "/Pose", getPose()); + Logger.recordOutput(getName() + "/RobotVelocity", getRobotVelocity()); + Logger.recordOutput(getName() + "/Setpoint", robotRelativeSetpoint); + Logger.recordOutput(getName() + "/MeasuredStates", getModuleStates()); + } + + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + public ChassisSpeeds getRobotVelocity() { + return SwerveConstants.KINEMATICS.toChassisSpeeds(getModuleStates()); + } + + public Rotation2d getGyro() { + return rawGyroRotation; + } + + public void addVisionMeasurement( + Pose3d visionMeasurement, + double timestampSeconds, + Matrix stdDevs) { + AKTimeLogger.startTiming("Timing/Vision/AddMeasurementSeconds"); + poseEstimator.addVisionMeasurement(visionMeasurement.toPose2d(), timestampSeconds, stdDevs); + AKTimeLogger.endTiming("Timing/Vision/AddMeasurementSeconds"); + } + + public void resetOdometry(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + public void driveFieldOriented(ChassisSpeeds fieldRelativeSpeeds) { + ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( + fieldRelativeSpeeds.vxMetersPerSecond, + fieldRelativeSpeeds.vyMetersPerSecond, + fieldRelativeSpeeds.omegaRadiansPerSecond, + getPose().getRotation()); + + driveRobotRelative(speeds); + } + + public Command driveFieldOriented(Supplier fieldRelativeSpeeds) { + return run(() -> driveFieldOriented(fieldRelativeSpeeds.get())) + .withName("SwerveDriveFieldOriented"); + } + + public Command driveCommand( + DoubleSupplier translationX, + DoubleSupplier translationY, + DoubleSupplier rotationX, + BooleanSupplier aim, + Supplier aimTarget) { + return run(() -> { + double x = modifyAxis(translationX.getAsDouble()) * SwerveConstants.MAX_LINEAR_SPEED_METERS_PER_SEC; + double y = modifyAxis(translationY.getAsDouble()) * SwerveConstants.MAX_LINEAR_SPEED_METERS_PER_SEC; + + Translation2d scaledTranslation = new Translation2d(x, y).times(0.8); + + if (shouldFlipForAlliance()) { + scaledTranslation = scaledTranslation.times(-1.0); + } + + double omega; + if (aim.getAsBoolean()) { + Translation2d toTarget = aimTarget.get().getTranslation().minus(getPose().getTranslation()); + Rotation2d targetHeading = toTarget.getAngle().plus(Rotation2d.fromDegrees(180.0)); + + omega = headingController.calculate(getPose().getRotation().getRadians(), targetHeading.getRadians()); + } else { + omega = -modifyAxis(rotationX.getAsDouble()) * SwerveConstants.MAX_ANGULAR_SPEED_RAD_PER_SEC; + } + + driveFieldOriented(new ChassisSpeeds(scaledTranslation.getX(), scaledTranslation.getY(), omega)); + }).withName("SwerveDriveTeleop"); + } + + public void driveRobotRelative(ChassisSpeeds speeds) { + robotRelativeSetpoint = ChassisSpeeds.discretize(speeds, Constants.LOOP_TIME_SECONDS); + SwerveModuleState[] states = SwerveConstants.KINEMATICS.toSwerveModuleStates(robotRelativeSetpoint); + + setModuleStates(states); + } + + public void setMotorBrake(boolean brake) { + for (Module module : modules) { + module.setBrakeMode(brake); + } + } + + public Command zeroGyro() { + return Commands.runOnce(() -> { + gyroIO.zeroGyro(); + rawGyroRotation = Rotation2d.kZero; + resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.kZero)); + }, this).withName("SwerveZeroGyro"); + } + + public Command lockWheels() { + return run(() -> setModuleStates(new SwerveModuleState[] { + new SwerveModuleState(0.0, SwerveConstants.FRONT_LEFT_LOCATION.getAngle()), + new SwerveModuleState(0.0, SwerveConstants.FRONT_RIGHT_LOCATION.getAngle()), + new SwerveModuleState(0.0, SwerveConstants.BACK_LEFT_LOCATION.getAngle()), + new SwerveModuleState(0.0, SwerveConstants.BACK_RIGHT_LOCATION.getAngle()) + })).withName("SwerveLockWheels"); + } + + public void stop() { + robotRelativeSetpoint = new ChassisSpeeds(); + + for (Module module : modules) { + module.stop(); + } + } + + public void setupPathPlanner() { + try { + RobotConfig config = RobotConfig.fromGUISettings(); + + AutoBuilder.configure( + this::getPose, + this::resetOdometry, + this::getRobotVelocity, + (speedsRobotRelative, moduleFeedForwards) -> driveRobotRelative(speedsRobotRelative), + new PPHolonomicDriveController( + new PIDConstants(4.5, 0.0, 0.2), + new PIDConstants(4.5, 0.0, 0.2)), + config, + () -> shouldFlipForAlliance(), + 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(); + } + } + + private void setModuleStates(SwerveModuleState[] states) { + SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.MAX_LINEAR_SPEED_METERS_PER_SEC); + + for (int i = 0; i < modules.length; i++) { + modules[i].setDesiredState(states[i]); + } + } + + private SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[] { + modules[FRONT_LEFT].getPosition(), + modules[FRONT_RIGHT].getPosition(), + modules[BACK_LEFT].getPosition(), + modules[BACK_RIGHT].getPosition() + }; + } + + private SwerveModuleState[] getModuleStates() { + return new SwerveModuleState[] { + modules[FRONT_LEFT].getState(), + modules[FRONT_RIGHT].getState(), + modules[BACK_LEFT].getState(), + modules[BACK_RIGHT].getState() + }; + } + + private static double modifyAxis(double value) { + double deadbanded = Math.abs(value) > OperatorConstants.DEADBAND ? value : 0.0; + return Math.copySign(deadbanded * deadbanded, deadbanded); + } + + private static boolean shouldFlipForAlliance() { + Optional alliance = DriverStation.getAlliance(); + return alliance.isPresent() && alliance.get() == Alliance.Red; + } + + private static class Module { + private final String name; + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + + Module(String name, ModuleIO io) { + this.name = name; + this.io = io; + } + + void updateInputs() { + io.updateInputs(inputs); + Logger.processInputs("Swerve/Module" + name, inputs); + } + + SwerveModulePosition getPosition() { + return new SwerveModulePosition( + inputs.drivePositionMeters, + Rotation2d.fromRadians(inputs.anglePositionRad)); + } + + SwerveModuleState getState() { + return new SwerveModuleState( + inputs.driveVelocityMetersPerSec, + Rotation2d.fromRadians(inputs.anglePositionRad)); + } + + void setDesiredState(SwerveModuleState state) { + SwerveModuleState optimizedState = optimize(state, Rotation2d.fromRadians(inputs.anglePositionRad)); + io.setDesiredState(optimizedState); + } + + void setBrakeMode(boolean brake) { + io.setBrakeMode(brake); + } + + void stop() { + io.stop(); + } + + private static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) { + Rotation2d delta = desiredState.angle.minus(currentAngle); + + if (Math.abs(delta.getDegrees()) > 90.0) { + return new SwerveModuleState( + -desiredState.speedMetersPerSecond, + desiredState.angle.plus(Rotation2d.fromDegrees(180.0))); + } + + return desiredState; + } + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java index e75df91..a8107b4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java @@ -15,6 +15,9 @@ class GyroIOInputs { public double rollVelocityRadPerSec = 0.0; } - default void zeroGyro() {} - default void updateInputs(GyroIOInputs inputs) {} + 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/GyroIOPigeon2.java similarity index 96% rename from src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOPigeon.java rename to src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOPigeon2.java index 5dcfddd..dc26468 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOPigeon.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOPigeon2.java @@ -11,7 +11,7 @@ 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 { +public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon = new Pigeon2(SwerveConstants.Pigeon.ID); private final Pigeon2Configuration config = new Pigeon2Configuration(); @@ -22,7 +22,7 @@ public class GyroIOPigeon implements GyroIO { private final StatusSignal pitchVelocity = pigeon.getAngularVelocityYWorld(); private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - public GyroIOPigeon() { + public GyroIOPigeon2() { config.MountPose.MountPoseRoll = 0; pigeon.getConfigurator().apply(config); pigeon.setYaw(0); diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java index 432def3..237160f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java @@ -2,12 +2,26 @@ // https://github.com/pittsfordrobotics/ChargedUp2023/blob/master/src/main/java/com/team3181/frc2023/subsystems/swerve/GyroIOSim.java public class GyroIOSim implements GyroIO { + private double yawPositionRad = 0.0; + private double yawVelocityRadPerSec = 0.0; + @Override public void updateInputs(GyroIOInputs inputs) { - inputs.connected = false; - inputs.yawPositionRad = 0; - inputs.yawVelocityRadPerSec = 0; + inputs.connected = true; + inputs.yawPositionRad = yawPositionRad; + inputs.yawVelocityRadPerSec = yawVelocityRadPerSec; inputs.pitchPositionRad = 0; inputs.rollPositionRad = 0; } -} \ No newline at end of file + + @Override + public void zeroGyro() { + yawPositionRad = 0.0; + yawVelocityRadPerSec = 0.0; + } + + public void setYaw(double yawPositionRad, double yawVelocityRadPerSec) { + this.yawPositionRad = yawPositionRad; + this.yawVelocityRadPerSec = yawVelocityRadPerSec; + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIO.java index 7369118..74fa7e0 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIO.java @@ -1,36 +1,44 @@ package frc.robot.subsystems.swerve.module; +import edu.wpi.first.math.kinematics.SwerveModuleState; 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 { + class ModuleIOInputs { + public boolean driveConnected = false; + public boolean angleConnected = false; + public boolean encoderConnected = false; + 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; + public double anglePositionRad = 0.0; + public double angleVelocityRadPerSec = 0.0; + public double angleAbsolutePositionRad = 0.0; + public double angleAppliedVolts = 0.0; + public double angleCurrentAmps = 0.0; + public double angleTempCelsius = 0.0; } - default void updateInputs(SwerveModuleIOInputs inputs) {} + default void updateInputs(ModuleIOInputs inputs) { + } - default void setDriveVoltage(double voltage) {} + default void setDesiredState(SwerveModuleState state) { + } - default void setSteerVoltage(double voltage) {} - - default void setModuleState(BetterSwerveModuleState state, boolean isOpenLoop) {} + default void setDriveVoltage(double volts) { + } - default void stopMotors() {} + default void setAngleVoltage(double volts) { + } - default void setDriveBrakeMode(boolean enable) {} + default void setBrakeMode(boolean brake) { + } - default void setSteerBrakeMode(boolean enable) {} + default void stop() { + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java index dfc949c..ccc78c4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java @@ -1,5 +1,76 @@ package frc.robot.subsystems.swerve.module; -public class ModuleIOSim { - +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.kinematics.SwerveModuleState; +import frc.robot.constants.Constants; +import frc.robot.constants.SwerveConstants; + +public class ModuleIOSim implements ModuleIO { + private final PIDController angleController = new PIDController( + SwerveConstants.Simulation.kP, + 0.0, + SwerveConstants.Simulation.kD); + + private double drivePositionMeters = 0.0; + private double driveVelocityMetersPerSec = 0.0; + private double anglePositionRad = 0.0; + private double angleVelocityRadPerSec = 0.0; + private double driveAppliedVolts = 0.0; + private double angleAppliedVolts = 0.0; + + public ModuleIOSim() { + angleController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + drivePositionMeters += driveVelocityMetersPerSec * Constants.LOOP_TIME_SECONDS; + angleVelocityRadPerSec = angleAppliedVolts; + anglePositionRad += angleVelocityRadPerSec * Constants.LOOP_TIME_SECONDS; + + inputs.driveConnected = true; + inputs.angleConnected = true; + inputs.encoderConnected = true; + inputs.drivePositionMeters = drivePositionMeters; + inputs.driveVelocityMetersPerSec = driveVelocityMetersPerSec; + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = Math.abs(driveAppliedVolts) * 2.0; + inputs.anglePositionRad = anglePositionRad; + inputs.angleVelocityRadPerSec = angleVelocityRadPerSec; + inputs.angleAbsolutePositionRad = anglePositionRad; + inputs.angleAppliedVolts = angleAppliedVolts; + inputs.angleCurrentAmps = Math.abs(angleAppliedVolts); + } + + @Override + public void setDesiredState(SwerveModuleState state) { + driveVelocityMetersPerSec = state.speedMetersPerSecond; + + driveAppliedVolts = MathUtil.clamp( + state.speedMetersPerSecond / SwerveConstants.MAX_LINEAR_SPEED_METERS_PER_SEC * 12.0, + -12.0, + 12.0); + angleAppliedVolts = MathUtil.clamp( + angleController.calculate(anglePositionRad, state.angle.getRadians()), + -12.0, + 12.0); + } + + @Override + public void setDriveVoltage(double volts) { + driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + driveVelocityMetersPerSec = driveAppliedVolts / 12.0 * SwerveConstants.MAX_LINEAR_SPEED_METERS_PER_SEC; + } + + @Override + public void setAngleVoltage(double volts) { + angleAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + } + + @Override + public void stop() { + setDesiredState(new SwerveModuleState(0.0, new Rotation2d(anglePositionRad))); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSparkMax.java index 759dda7..0fc6a15 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSparkMax.java @@ -1,5 +1,233 @@ package frc.robot.subsystems.swerve.module; -public class ModuleIOSparkMax { - +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +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.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import frc.robot.constants.SwerveConstants; +import frc.robot.utils.SparkUtil; + +public class ModuleIOSparkMax implements ModuleIO { + private final SparkMax driveMotor; + private final SparkMax angleMotor; + private final CANcoder absoluteEncoder; + + private final RelativeEncoder driveEncoder; + private final RelativeEncoder angleEncoder; + private final SparkClosedLoopController driveController; + private final SparkClosedLoopController angleController; + + private final StatusSignal absolutePosition; + + public ModuleIOSparkMax( + int driveId, + int angleId, + int encoderId, + double absoluteEncoderOffsetDegrees, + boolean driveInverted, + boolean angleInverted, + boolean encoderInverted) { + driveMotor = new SparkMax(driveId, MotorType.kBrushless); + angleMotor = new SparkMax(angleId, MotorType.kBrushless); + + absoluteEncoder = new CANcoder(encoderId); + + driveEncoder = driveMotor.getEncoder(); + angleEncoder = angleMotor.getEncoder(); + + driveController = driveMotor.getClosedLoopController(); + angleController = angleMotor.getClosedLoopController(); + + absolutePosition = absoluteEncoder.getAbsolutePosition(); + + CANcoderConfiguration encoderConfig = new CANcoderConfiguration(); + encoderConfig.MagnetSensor.SensorDirection = encoderInverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + encoderConfig.MagnetSensor.MagnetOffset = -absoluteEncoderOffsetDegrees / 360.0; + encoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; + absoluteEncoder.getConfigurator().apply(encoderConfig); + + SparkMaxConfig driveConfig = new SparkMaxConfig(); + driveConfig + .inverted(driveInverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(SwerveConstants.PhysicalProperties.DRIVE_CURRENT_LIMIT) + .voltageCompensation(SwerveConstants.PhysicalProperties.OPTIMAL_VOLTAGE_VOLTS) + .openLoopRampRate(SwerveConstants.PhysicalProperties.DRIVE_RAMP_RATE_SECONDS) + .closedLoopRampRate(SwerveConstants.PhysicalProperties.DRIVE_RAMP_RATE_SECONDS); + driveConfig.encoder + .positionConversionFactor(SwerveConstants.DRIVE_POSITION_FACTOR_METERS_PER_ROTATION) + .velocityConversionFactor(SwerveConstants.DRIVE_VELOCITY_FACTOR_METERS_PER_SEC_PER_RPM); + driveConfig.closedLoop.pid( + SwerveConstants.PIDFProperties.DRIVE_kP, + SwerveConstants.PIDFProperties.DRIVE_kI, + SwerveConstants.PIDFProperties.DRIVE_kD); + + SparkMaxConfig angleConfig = new SparkMaxConfig(); + angleConfig + .inverted(angleInverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(SwerveConstants.PhysicalProperties.ANGLE_CURRENT_LIMIT) + .voltageCompensation(SwerveConstants.PhysicalProperties.OPTIMAL_VOLTAGE_VOLTS) + .openLoopRampRate(SwerveConstants.PhysicalProperties.ANGLE_RAMP_RATE_SECONDS) + .closedLoopRampRate(SwerveConstants.PhysicalProperties.ANGLE_RAMP_RATE_SECONDS); + angleConfig.encoder + .positionConversionFactor(SwerveConstants.ANGLE_POSITION_FACTOR_RAD_PER_ROTATION) + .velocityConversionFactor(SwerveConstants.ANGLE_VELOCITY_FACTOR_RAD_PER_SEC_PER_RPM); + angleConfig.closedLoop + .pid( + SwerveConstants.PIDFProperties.ANGLE_kP, + SwerveConstants.PIDFProperties.ANGLE_kI, + SwerveConstants.PIDFProperties.ANGLE_kD) + .positionWrappingEnabled(true) + .positionWrappingInputRange(-Math.PI, Math.PI); + + SparkUtil.tryUntilOk( + driveMotor, + 5, + () -> driveMotor.configure(driveConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters)); + SparkUtil.tryUntilOk( + angleMotor, + 5, + () -> angleMotor.configure(angleConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters)); + + syncAngleEncoder(); + } + + private void syncAngleEncoder() { + BaseStatusSignal.refreshAll(absolutePosition); + + SparkUtil.tryUntilOk( + angleMotor, + 5, + () -> angleEncoder.setPosition( + Units.rotationsToRadians(absolutePosition.getValueAsDouble()))); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.encoderConnected = BaseStatusSignal.refreshAll(absolutePosition).isOK(); + inputs.angleAbsolutePositionRad = Units.rotationsToRadians(absolutePosition.getValueAsDouble()); + + SparkUtil.sparkStickyFault = false; + SparkUtil.ifOk( + driveMotor, + new java.util.function.DoubleSupplier[] { + driveEncoder::getPosition, + driveEncoder::getVelocity, + driveMotor::getAppliedOutput, + driveMotor::getBusVoltage, + driveMotor::getOutputCurrent, + driveMotor::getMotorTemperature + }, + values -> { + inputs.drivePositionMeters = values[0]; + inputs.driveVelocityMetersPerSec = values[1]; + inputs.driveAppliedVolts = values[2] * values[3]; + inputs.driveCurrentAmps = values[4]; + inputs.driveTempCelsius = values[5]; + }); + inputs.driveConnected = !SparkUtil.sparkStickyFault; + + SparkUtil.sparkStickyFault = false; + SparkUtil.ifOk( + angleMotor, + new java.util.function.DoubleSupplier[] { + angleEncoder::getPosition, + angleEncoder::getVelocity, + angleMotor::getAppliedOutput, + angleMotor::getBusVoltage, + angleMotor::getOutputCurrent, + angleMotor::getMotorTemperature + }, + values -> { + inputs.anglePositionRad = values[0]; + inputs.angleVelocityRadPerSec = values[1]; + inputs.angleAppliedVolts = values[2] * values[3]; + inputs.angleCurrentAmps = values[4]; + inputs.angleTempCelsius = values[5]; + }); + inputs.angleConnected = !SparkUtil.sparkStickyFault; + } + + @Override + public void setDesiredState(SwerveModuleState state) { + if (Math.abs(state.speedMetersPerSecond) < 0.01) { + driveMotor.stopMotor(); + angleController.setSetpoint( + state.angle.getRadians(), + ControlType.kPosition, + ClosedLoopSlot.kSlot0); + + return; + } + + driveController.setSetpoint( + state.speedMetersPerSecond, + ControlType.kVelocity, + ClosedLoopSlot.kSlot0); + angleController.setSetpoint( + state.angle.getRadians(), + ControlType.kPosition, + ClosedLoopSlot.kSlot0); + } + + @Override + public void setDriveVoltage(double volts) { + driveMotor.setVoltage(volts); + } + + @Override + public void setAngleVoltage(double volts) { + angleMotor.setVoltage(volts); + } + + @Override + public void setBrakeMode(boolean brake) { + SparkMaxConfig driveConfig = new SparkMaxConfig(); + SparkMaxConfig angleConfig = new SparkMaxConfig(); + + driveConfig.idleMode(brake ? IdleMode.kBrake : IdleMode.kCoast); + angleConfig.idleMode(brake ? IdleMode.kBrake : IdleMode.kCoast); + + SparkUtil.tryUntilOk( + driveMotor, + 5, + () -> driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, + PersistMode.kPersistParameters)); + SparkUtil.tryUntilOk( + angleMotor, + 5, + () -> angleMotor.configure(angleConfig, ResetMode.kNoResetSafeParameters, + PersistMode.kPersistParameters)); + } + + @Override + public void stop() { + driveMotor.stopMotor(); + angleMotor.stopMotor(); + } + + public Rotation2d getAbsoluteAngle() { + BaseStatusSignal.refreshAll(absolutePosition); + return Rotation2d.fromRadians(Units.rotationsToRadians(absolutePosition.getValueAsDouble())); + } } 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..f54e62c --- /dev/null +++ b/src/main/java/frc/robot/utils/SparkUtil.java @@ -0,0 +1,53 @@ +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 From bcbc70352eba4095fe5f79dbb109cab5de761e54 Mon Sep 17 00:00:00 2001 From: Scaratech Date: Fri, 24 Apr 2026 08:03:36 -0400 Subject: [PATCH 2/5] feat: refactor entire thing (steal example project) --- src/main/java/frc/robot/RobotContainer.java | 97 ++++- .../frc/robot/commands/DriveCommands.java | 301 +++++++++++++ .../frc/robot/constants/SwerveConstants.java | 148 ------- .../subsystems/drive/DriveConstants.java | 106 +++++ .../subsystems/drive/DriveSubsystem.java | 401 ++++++++++++++++++ .../frc/robot/subsystems/drive/GyroIO.java | 25 ++ .../robot/subsystems/drive/GyroIOPigeon2.java | 60 +++ .../frc/robot/subsystems/drive/Module.java | 135 ++++++ .../frc/robot/subsystems/drive/ModuleIO.java | 56 +++ .../robot/subsystems/drive/ModuleIOSim.java | 110 +++++ .../robot/subsystems/drive/ModuleIOSpark.java | 259 +++++++++++ .../subsystems/drive/SparkOdometryThread.java | 141 ++++++ .../frc/robot/subsystems/swerve/Swerve.java | 401 ------------------ .../robot/subsystems/swerve/gyro/GyroIO.java | 23 - .../subsystems/swerve/gyro/GyroIOPigeon2.java | 53 --- .../subsystems/swerve/gyro/GyroIOSim.java | 27 -- .../subsystems/swerve/module/ModuleIO.java | 44 -- .../subsystems/swerve/module/ModuleIOSim.java | 76 ---- .../swerve/module/ModuleIOSparkMax.java | 233 ---------- .../java/frc/robot/utils/LocalADStarAK.java | 167 ++++++++ src/main/java/frc/robot/utils/SparkUtil.java | 10 +- 21 files changed, 1851 insertions(+), 1022 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DriveCommands.java delete mode 100644 src/main/java/frc/robot/constants/SwerveConstants.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveConstants.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Module.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java create mode 100644 src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/Swerve.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOPigeon2.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/module/ModuleIO.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSparkMax.java create mode 100644 src/main/java/frc/robot/utils/LocalADStarAK.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4f1156e..a3c8809 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; @@ -9,7 +11,12 @@ import frc.robot.subsystems.PDHSubsystem; import frc.robot.subsystems.PhotonVisionSubsystem; import frc.robot.subsystems.TurretSubsystem; -import frc.robot.subsystems.swerve.Swerve; +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 static edu.wpi.first.units.Units.Degrees; @@ -29,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 Swerve drivebase = new Swerve(); + 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; @@ -63,22 +68,67 @@ public class RobotContainer { 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); - driveFieldOrientedAngularVelocity = drivebase.driveCommand( + driveFieldOrientedAngularVelocity = drive.driveCommand( () -> -driverController.getLeftY(), () -> -driverController.getLeftX(), () -> -driverController.getRightX(), driverController.b(), () -> new Pose2d(gameHelpers.getVirtualTargetTranslation(), Rotation2d.kZero)); - drivebase.setDefaultCommand(driveFieldOrientedAngularVelocity); + drive.setDefaultCommand(driveFieldOrientedAngularVelocity); // Signal readiness to the driver so they know when to shoot leds.configureLEDs(() -> turret.isAutoTrackingEnabled() @@ -96,6 +146,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(); } @@ -149,7 +214,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() @@ -174,7 +239,7 @@ private void configureBindings() { } public void setMotorBrake(boolean brake) { - drivebase.setMotorBrake(brake); + drive.setMotorBrake(brake); } /** @@ -185,7 +250,7 @@ public void setMotorBrake(boolean brake) { * the heading lock was never updated during auto driving. */ private Command autoDriving(Command drivingCommand) { - return drivingCommand.finallyDo(interrupted -> drivebase.stop()).withName("AutoDriving"); + return drivingCommand.finallyDo(interrupted -> drive.stop()).withName("AutoDriving"); } /** 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..4e5cbfa --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -0,0 +1,301 @@ +// 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.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +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.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 Pose2d(Translation2d.kZero, linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, Rotation2d.kZero)) + .getTranslation(); + } + + /** + * 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())); + } + + /** + * 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( + () -> { + var rotation = drive.getRotation(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + }) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + double[] positions = drive.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + + double wheelRadius = (state.gyroDelta * DriveConstants.driveBaseRadius) + / wheelDelta; + + NumberFormat formatter = new DecimalFormat("#0.000"); + System.out.println( + "********** Wheel Radius Characterization Results **********"); + System.out.println( + "\tWheel Delta: " + formatter.format(wheelDelta) + " radians"); + System.out.println( + "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians"); + System.out.println( + "\tWheel Radius: " + + formatter.format(wheelRadius) + + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + + " inches"); + }))); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = Rotation2d.kZero; + double gyroDelta = 0.0; + } +} 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 3f3b294..0000000 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ /dev/null @@ -1,148 +0,0 @@ -package frc.robot.constants; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.util.Units; - -// Adapted from previous YAGSL config files -public class SwerveConstants { - public static final double MAX_LINEAR_SPEED_METERS_PER_SEC = Constants.MAX_SPEED; - public static final double MAX_ANGULAR_SPEED_RAD_PER_SEC = 2.0 * Math.PI; - - public static final double DRIVE_WHEEL_RADIUS_METERS = Units.inchesToMeters( - PhysicalProperties.DRIVE_WHEEL_DIAMETER_INCHES) / 2.0; - public static final double DRIVE_POSITION_FACTOR_METERS_PER_ROTATION = 2.0 * Math.PI * DRIVE_WHEEL_RADIUS_METERS - / PhysicalProperties.DRIVE_GEAR_RATIO; - public static final double DRIVE_VELOCITY_FACTOR_METERS_PER_SEC_PER_RPM = DRIVE_POSITION_FACTOR_METERS_PER_ROTATION - / 60.0; - public static final double ANGLE_POSITION_FACTOR_RAD_PER_ROTATION = 2.0 * Math.PI - / PhysicalProperties.ANGLE_GEAR_RATIO; - public static final double ANGLE_VELOCITY_FACTOR_RAD_PER_SEC_PER_RPM = ANGLE_POSITION_FACTOR_RAD_PER_ROTATION - / 60.0; - - public static final Translation2d FRONT_LEFT_LOCATION = new Translation2d( - Units.inchesToMeters(FrontLeftModule.LOCATION_FRONT_INCHES), - Units.inchesToMeters(FrontLeftModule.LOCATION_LEFT_INCHES)); - public static final Translation2d FRONT_RIGHT_LOCATION = new Translation2d( - Units.inchesToMeters(FrontRightModule.LOCATION_FRONT_INCHES), - Units.inchesToMeters(FrontRightModule.LOCATION_LEFT_INCHES)); - public static final Translation2d BACK_LEFT_LOCATION = new Translation2d( - Units.inchesToMeters(BackLeftModule.LOCATION_FRONT_INCHES), - Units.inchesToMeters(BackLeftModule.LOCATION_LEFT_INCHES)); - public static final Translation2d BACK_RIGHT_LOCATION = new Translation2d( - Units.inchesToMeters(BackRightModule.LOCATION_FRONT_INCHES), - Units.inchesToMeters(BackRightModule.LOCATION_LEFT_INCHES)); - - public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics( - FRONT_LEFT_LOCATION, - FRONT_RIGHT_LOCATION, - BACK_LEFT_LOCATION, - BACK_RIGHT_LOCATION); - - // Pigeon2 (IMU/Gyro) (`swervedrive.json`) - public static final class Pigeon { - public static final int ID = 0; - } - - // `modules/physicalproperties.json` - public static final class PhysicalProperties { - public static final int OPTIMAL_VOLTAGE_VOLTS = 12; - - // `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; - - // `conversionFactors.drive` - public static final double DRIVE_WHEEL_DIAMETER_INCHES = 4.0; - public static final double DRIVE_GEAR_RATIO = 6.75; - - // `rampRate` - public static final double DRIVE_RAMP_RATE_SECONDS = 0.1; - public static final double ANGLE_RAMP_RATE_SECONDS = 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; - } - - // `modules/backleft.json` - public static final class BackLeftModule { - public static final double LOCATION_FRONT_INCHES = -10.875; - public static final double LOCATION_LEFT_INCHES = 10.875; - - public static final double ABSOLUTE_ENCODER_OFFSET_DEGREES = 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_INCHES = -10.875; - public static final double LOCATION_LEFT_INCHES = -10.875; - - public static final double ABSOLUTE_ENCODER_OFFSET_DEGREES = 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_INCHES = 10.875; - public static final double LOCATION_LEFT_INCHES = 10.875; - - public static final double ABSOLUTE_ENCODER_OFFSET_DEGREES = 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_INCHES = 10.875; - public static final double LOCATION_LEFT_INCHES = -10.875; - - public static final double ABSOLUTE_ENCODER_OFFSET_DEGREES = 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; - } - - public static final class Simulation { - public static final double kP = 0.05; - public static final double kD = 0.0; - } -} 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..f177402 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -0,0 +1,106 @@ +// 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(26.5); + public static final double wheelBase = Units.inchesToMeters(26.5); + public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0); + public static final Translation2d[] moduleTranslations = new Translation2d[] { + new Translation2d(trackWidth / 2.0, wheelBase / 2.0), + new Translation2d(trackWidth / 2.0, -wheelBase / 2.0), + new Translation2d(-trackWidth / 2.0, wheelBase / 2.0), + new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0) + }; + + // Zeroed rotation values for each module, see setup instructions + public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.0); + public static final Rotation2d frontRightZeroRotation = new Rotation2d(0.0); + public static final Rotation2d backLeftZeroRotation = new Rotation2d(0.0); + public static final Rotation2d backRightZeroRotation = new Rotation2d(0.0); + + // Device CAN IDs + public static final int pigeonCanId = 9; + + public static final int frontLeftDriveCanId = 1; + public static final int backLeftDriveCanId = 3; + public static final int frontRightDriveCanId = 5; + public static final int backRightDriveCanId = 7; + + public static final int frontLeftTurnCanId = 2; + public static final int backLeftTurnCanId = 4; + public static final int frontRightTurnCanId = 6; + public static final int backRightTurnCanId = 8; + + // Drive motor configuration + public static final int driveMotorCurrentLimit = 50; + public static final double wheelRadiusMeters = Units.inchesToMeters(1.5); + public static final double driveMotorReduction = (45.0 * 22.0) / (14.0 * 15.0); // MAXSwerve with 14 pinion teeth + // and 22 spur teeth + public static final DCMotor driveGearbox = DCMotor.getNeoVortex(1); + + // Drive encoder configuration + public static final double driveEncoderPositionFactor = 2 * Math.PI / driveMotorReduction; // Rotor Rotations -> + // Wheel Radians + public static final double driveEncoderVelocityFactor = (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> + // Wheel Rad/Sec + + // Drive PID configuration + public static final double driveKp = 0.0; + public static final double driveKd = 0.0; + public static final double driveKs = 0.0; + public static final double driveKv = 0.1; + public static final double driveSimP = 0.05; + public static final double driveSimD = 0.0; + public static final double driveSimKs = 0.0; + public static final double driveSimKv = 0.0789; + + // Turn motor configuration + public static final boolean turnInverted = false; + public static final int turnMotorCurrentLimit = 20; + public static final double turnMotorReduction = 9424.0 / 203.0; + public static final DCMotor turnGearbox = DCMotor.getNeo550(1); + + // Turn encoder configuration + public static final boolean turnEncoderInverted = true; + public static final double turnEncoderPositionFactor = 2 * Math.PI; // Rotations -> Radians + public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // RPM -> Rad/Sec + + // Turn PID configuration + public static final double turnKp = 2.0; + public static final double turnKd = 0.0; + public static final double turnSimP = 8.0; + public static final double turnSimD = 0.0; + public static final double turnPIDMinInput = 0; // Radians + public static final double turnPIDMaxInput = 2 * Math.PI; // Radians + + // PathPlanner configuration + public static final double robotMassKg = 74.088; + public static final double robotMOI = 6.883; + public static final double wheelCOF = 1.2; + public static final RobotConfig ppConfig = new RobotConfig( + robotMassKg, + robotMOI, + new ModuleConfig( + wheelRadiusMeters, + maxSpeedMetersPerSec, + wheelCOF, + driveGearbox.withReduction(driveMotorReduction), + driveMotorCurrentLimit, + 1), + moduleTranslations); +} \ No newline at end of file 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..d503e40 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -0,0 +1,401 @@ +// 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.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.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +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 (var 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 pose translation. + */ + public Command driveCommand( + DoubleSupplier translationX, + DoubleSupplier translationY, + DoubleSupplier omega, + BooleanSupplier aimAtTarget, + Supplier targetPoseSupplier) { + Command manualDrive = DriveCommands.joystickDrive(this, translationX, translationY, omega); + Command targetLockDrive = DriveCommands.joystickDriveAtAngle( + this, + translationX, + translationY, + () -> targetPoseSupplier.get().getTranslation().minus(getPose().getTranslation()).getAngle()); + return Commands.either(targetLockDrive, manualDrive, aimAtTarget).withName("DriveCommand"); + } + + /** Convenience overload for WPILib triggers. */ + public Command driveCommand( + DoubleSupplier translationX, + DoubleSupplier translationY, + DoubleSupplier omega, + Trigger aimAtTarget, + Supplier targetPoseSupplier) { + return driveCommand(translationX, translationY, omega, aimAtTarget::getAsBoolean, targetPoseSupplier); + } + + /** 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..621d9c8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -0,0 +1,135 @@ +// 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 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); + } + + 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); + } + + /** + * 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..fcf3791 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -0,0 +1,56 @@ +// 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 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..d9892ea --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -0,0 +1,110 @@ +// 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()); + + // 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..7b8d614 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -0,0 +1,259 @@ +// 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.revrobotics.AbsoluteEncoder; +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; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; +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 java.util.Queue; +import java.util.function.DoubleSupplier; + +/** + * Module IO implementation for Spark Flex drive motor controller, Spark Max + * turn motor controller, + * and duty cycle absolute encoder. + */ +public class ModuleIOSpark implements ModuleIO { + private final Rotation2d zeroRotation; + + // Hardware objects + private final SparkBase driveSpark; + private final SparkBase turnSpark; + private final RelativeEncoder driveEncoder; + private final AbsoluteEncoder turnEncoder; + + // 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); + + public ModuleIOSpark(int module) { + zeroRotation = switch (module) { + case 0 -> frontLeftZeroRotation; + case 1 -> frontRightZeroRotation; + case 2 -> backLeftZeroRotation; + case 3 -> backRightZeroRotation; + default -> Rotation2d.kZero; + }; + + driveSpark = new SparkFlex( + 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); + + driveEncoder = driveSpark.getEncoder(); + turnEncoder = turnSpark.getAbsoluteEncoder(); + driveController = driveSpark.getClosedLoopController(); + turnController = turnSpark.getClosedLoopController(); + + // Configure drive motor + SparkFlexConfig driveConfig = new SparkFlexConfig(); + driveConfig + .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.absoluteEncoder + .inverted(turnEncoderInverted) + .positionConversionFactor(turnEncoderPositionFactor) + .velocityConversionFactor(turnEncoderVelocityFactor) + .averageDepth(2); + turnConfig.closedLoop + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .positionWrappingEnabled(true) + .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) + .pid(turnKp, 0.0, turnKd); + turnConfig.signals + .absoluteEncoderPositionAlwaysOn(true) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .absoluteEncoderVelocityAlwaysOn(true) + .absoluteEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + + tryUntilOk( + turnSpark, + 5, + () -> turnSpark.configure( + turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + + // Create odometry queues + timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); + drivePositionQueue = SparkOdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition); + turnPositionQueue = SparkOdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition); + } + + @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; + + ifOk( + turnSpark, + turnEncoder::getPosition, + (value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation)); + 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.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault); + + // 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((Double value) -> new Rotation2d(value).minus(zeroRotation)) + .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.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); + turnController.setSetpoint(setpoint, ControlType.kPosition); + } + + @Override + public void setBrakeMode(boolean enabled) { + IdleMode idleMode = enabled ? IdleMode.kBrake : IdleMode.kCoast; + + SparkFlexConfig driveConfig = new SparkFlexConfig(); + 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)); + } +} 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 d57a917..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ /dev/null @@ -1,401 +0,0 @@ -package frc.robot.subsystems.swerve; - -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 edu.wpi.first.math.Matrix; -import edu.wpi.first.math.controller.PIDController; -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.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.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.Constants; -import frc.robot.constants.OperatorConstants; -import frc.robot.constants.SwerveConstants; -import frc.robot.subsystems.swerve.gyro.GyroIO; -import frc.robot.subsystems.swerve.gyro.GyroIOInputsAutoLogged; -import frc.robot.subsystems.swerve.gyro.GyroIOPigeon2; -import frc.robot.subsystems.swerve.gyro.GyroIOSim; -import frc.robot.subsystems.swerve.module.ModuleIO; -import frc.robot.subsystems.swerve.module.ModuleIOInputsAutoLogged; -import frc.robot.subsystems.swerve.module.ModuleIOSim; -import frc.robot.subsystems.swerve.module.ModuleIOSparkMax; -import frc.robot.utils.AKTimeLogger; -import java.util.Optional; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; - -public class Swerve extends SubsystemBase { - private static final int FRONT_LEFT = 0; - private static final int FRONT_RIGHT = 1; - private static final int BACK_LEFT = 2; - private static final int BACK_RIGHT = 3; - - private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - private final Module[] modules; - private final SwerveDrivePoseEstimator poseEstimator; - private final PIDController headingController = new PIDController(5.0, 0.0, 0.1); - - private ChassisSpeeds robotRelativeSetpoint = new ChassisSpeeds(); - private Rotation2d rawGyroRotation = Rotation2d.kZero; - - public Swerve() { - switch (Constants.currentMode) { - case REAL: - gyroIO = new GyroIOPigeon2(); - - modules = new Module[] { - new Module("FrontLeft", new ModuleIOSparkMax( - SwerveConstants.FrontLeftModule.DRIVE_ID, - SwerveConstants.FrontLeftModule.ANGLE_ID, - SwerveConstants.FrontLeftModule.ENCODER_ID, - SwerveConstants.FrontLeftModule.ABSOLUTE_ENCODER_OFFSET_DEGREES, - SwerveConstants.FrontLeftModule.DRIVE_INVERTED, - SwerveConstants.FrontLeftModule.ANGLE_INVERTED, - SwerveConstants.FrontLeftModule.ENCODER_INVERTED)), - new Module("FrontRight", new ModuleIOSparkMax( - SwerveConstants.FrontRightModule.DRIVE_ID, - SwerveConstants.FrontRightModule.ANGLE_ID, - SwerveConstants.FrontRightModule.ENCODER_ID, - SwerveConstants.FrontRightModule.ABSOLUTE_ENCODER_OFFSET_DEGREES, - SwerveConstants.FrontRightModule.DRIVE_INVERTED, - SwerveConstants.FrontRightModule.ANGLE_INVERTED, - SwerveConstants.FrontRightModule.ENCODER_INVERTED)), - new Module("BackLeft", new ModuleIOSparkMax( - SwerveConstants.BackLeftModule.DRIVE_ID, - SwerveConstants.BackLeftModule.ANGLE_ID, - SwerveConstants.BackLeftModule.ENCODER_ID, - SwerveConstants.BackLeftModule.ABSOLUTE_ENCODER_OFFSET_DEGREES, - SwerveConstants.BackLeftModule.DRIVE_INVERTED, - SwerveConstants.BackLeftModule.ANGLE_INVERTED, - SwerveConstants.BackLeftModule.ENCODER_INVERTED)), - new Module("BackRight", new ModuleIOSparkMax( - SwerveConstants.BackRightModule.DRIVE_ID, - SwerveConstants.BackRightModule.ANGLE_ID, - SwerveConstants.BackRightModule.ENCODER_ID, - SwerveConstants.BackRightModule.ABSOLUTE_ENCODER_OFFSET_DEGREES, - SwerveConstants.BackRightModule.DRIVE_INVERTED, - SwerveConstants.BackRightModule.ANGLE_INVERTED, - SwerveConstants.BackRightModule.ENCODER_INVERTED)) - }; - - break; - case SIM: - gyroIO = new GyroIOSim(); - - modules = new Module[] { - new Module("FrontLeft", new ModuleIOSim()), - new Module("FrontRight", new ModuleIOSim()), - new Module("BackLeft", new ModuleIOSim()), - new Module("BackRight", new ModuleIOSim()) - }; - - break; - case REPLAY: - default: - gyroIO = new GyroIO() { - }; - - modules = new Module[] { - new Module("FrontLeft", new ModuleIO() { - }), - new Module("FrontRight", new ModuleIO() { - }), - new Module("BackLeft", new ModuleIO() { - }), - new Module("BackRight", new ModuleIO() { - }) - }; - - break; - } - - headingController.enableContinuousInput(-Math.PI, Math.PI); - - poseEstimator = new SwerveDrivePoseEstimator( - SwerveConstants.KINEMATICS, - rawGyroRotation, - getModulePositions(), - Pose2d.kZero); - - SmartDashboard.putData("ZeroGyro", zeroGyro().withName("Zero Gyro")); - setupPathPlanner(); - } - - @Override - public void periodic() { - if (gyroIO instanceof GyroIOSim gyroSim) { - rawGyroRotation = rawGyroRotation.plus( - Rotation2d.fromRadians(robotRelativeSetpoint.omegaRadiansPerSecond * Constants.LOOP_TIME_SECONDS)); - gyroSim.setYaw(rawGyroRotation.getRadians(), robotRelativeSetpoint.omegaRadiansPerSecond); - } - - gyroIO.updateInputs(gyroInputs); - Logger.processInputs(getName() + "/Gyro", gyroInputs); - - for (Module module : modules) { - module.updateInputs(); - } - - rawGyroRotation = Rotation2d.fromRadians(gyroInputs.yawPositionRad); - poseEstimator.update(rawGyroRotation, getModulePositions()); - - Logger.recordOutput(getName() + "/Pose", getPose()); - Logger.recordOutput(getName() + "/RobotVelocity", getRobotVelocity()); - Logger.recordOutput(getName() + "/Setpoint", robotRelativeSetpoint); - Logger.recordOutput(getName() + "/MeasuredStates", getModuleStates()); - } - - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - public ChassisSpeeds getRobotVelocity() { - return SwerveConstants.KINEMATICS.toChassisSpeeds(getModuleStates()); - } - - public Rotation2d getGyro() { - return rawGyroRotation; - } - - public void addVisionMeasurement( - Pose3d visionMeasurement, - double timestampSeconds, - Matrix stdDevs) { - AKTimeLogger.startTiming("Timing/Vision/AddMeasurementSeconds"); - poseEstimator.addVisionMeasurement(visionMeasurement.toPose2d(), timestampSeconds, stdDevs); - AKTimeLogger.endTiming("Timing/Vision/AddMeasurementSeconds"); - } - - public void resetOdometry(Pose2d pose) { - poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); - } - - public void driveFieldOriented(ChassisSpeeds fieldRelativeSpeeds) { - ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - fieldRelativeSpeeds.vxMetersPerSecond, - fieldRelativeSpeeds.vyMetersPerSecond, - fieldRelativeSpeeds.omegaRadiansPerSecond, - getPose().getRotation()); - - driveRobotRelative(speeds); - } - - public Command driveFieldOriented(Supplier fieldRelativeSpeeds) { - return run(() -> driveFieldOriented(fieldRelativeSpeeds.get())) - .withName("SwerveDriveFieldOriented"); - } - - public Command driveCommand( - DoubleSupplier translationX, - DoubleSupplier translationY, - DoubleSupplier rotationX, - BooleanSupplier aim, - Supplier aimTarget) { - return run(() -> { - double x = modifyAxis(translationX.getAsDouble()) * SwerveConstants.MAX_LINEAR_SPEED_METERS_PER_SEC; - double y = modifyAxis(translationY.getAsDouble()) * SwerveConstants.MAX_LINEAR_SPEED_METERS_PER_SEC; - - Translation2d scaledTranslation = new Translation2d(x, y).times(0.8); - - if (shouldFlipForAlliance()) { - scaledTranslation = scaledTranslation.times(-1.0); - } - - double omega; - if (aim.getAsBoolean()) { - Translation2d toTarget = aimTarget.get().getTranslation().minus(getPose().getTranslation()); - Rotation2d targetHeading = toTarget.getAngle().plus(Rotation2d.fromDegrees(180.0)); - - omega = headingController.calculate(getPose().getRotation().getRadians(), targetHeading.getRadians()); - } else { - omega = -modifyAxis(rotationX.getAsDouble()) * SwerveConstants.MAX_ANGULAR_SPEED_RAD_PER_SEC; - } - - driveFieldOriented(new ChassisSpeeds(scaledTranslation.getX(), scaledTranslation.getY(), omega)); - }).withName("SwerveDriveTeleop"); - } - - public void driveRobotRelative(ChassisSpeeds speeds) { - robotRelativeSetpoint = ChassisSpeeds.discretize(speeds, Constants.LOOP_TIME_SECONDS); - SwerveModuleState[] states = SwerveConstants.KINEMATICS.toSwerveModuleStates(robotRelativeSetpoint); - - setModuleStates(states); - } - - public void setMotorBrake(boolean brake) { - for (Module module : modules) { - module.setBrakeMode(brake); - } - } - - public Command zeroGyro() { - return Commands.runOnce(() -> { - gyroIO.zeroGyro(); - rawGyroRotation = Rotation2d.kZero; - resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.kZero)); - }, this).withName("SwerveZeroGyro"); - } - - public Command lockWheels() { - return run(() -> setModuleStates(new SwerveModuleState[] { - new SwerveModuleState(0.0, SwerveConstants.FRONT_LEFT_LOCATION.getAngle()), - new SwerveModuleState(0.0, SwerveConstants.FRONT_RIGHT_LOCATION.getAngle()), - new SwerveModuleState(0.0, SwerveConstants.BACK_LEFT_LOCATION.getAngle()), - new SwerveModuleState(0.0, SwerveConstants.BACK_RIGHT_LOCATION.getAngle()) - })).withName("SwerveLockWheels"); - } - - public void stop() { - robotRelativeSetpoint = new ChassisSpeeds(); - - for (Module module : modules) { - module.stop(); - } - } - - public void setupPathPlanner() { - try { - RobotConfig config = RobotConfig.fromGUISettings(); - - AutoBuilder.configure( - this::getPose, - this::resetOdometry, - this::getRobotVelocity, - (speedsRobotRelative, moduleFeedForwards) -> driveRobotRelative(speedsRobotRelative), - new PPHolonomicDriveController( - new PIDConstants(4.5, 0.0, 0.2), - new PIDConstants(4.5, 0.0, 0.2)), - config, - () -> shouldFlipForAlliance(), - 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(); - } - } - - private void setModuleStates(SwerveModuleState[] states) { - SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.MAX_LINEAR_SPEED_METERS_PER_SEC); - - for (int i = 0; i < modules.length; i++) { - modules[i].setDesiredState(states[i]); - } - } - - private SwerveModulePosition[] getModulePositions() { - return new SwerveModulePosition[] { - modules[FRONT_LEFT].getPosition(), - modules[FRONT_RIGHT].getPosition(), - modules[BACK_LEFT].getPosition(), - modules[BACK_RIGHT].getPosition() - }; - } - - private SwerveModuleState[] getModuleStates() { - return new SwerveModuleState[] { - modules[FRONT_LEFT].getState(), - modules[FRONT_RIGHT].getState(), - modules[BACK_LEFT].getState(), - modules[BACK_RIGHT].getState() - }; - } - - private static double modifyAxis(double value) { - double deadbanded = Math.abs(value) > OperatorConstants.DEADBAND ? value : 0.0; - return Math.copySign(deadbanded * deadbanded, deadbanded); - } - - private static boolean shouldFlipForAlliance() { - Optional alliance = DriverStation.getAlliance(); - return alliance.isPresent() && alliance.get() == Alliance.Red; - } - - private static class Module { - private final String name; - private final ModuleIO io; - private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - - Module(String name, ModuleIO io) { - this.name = name; - this.io = io; - } - - void updateInputs() { - io.updateInputs(inputs); - Logger.processInputs("Swerve/Module" + name, inputs); - } - - SwerveModulePosition getPosition() { - return new SwerveModulePosition( - inputs.drivePositionMeters, - Rotation2d.fromRadians(inputs.anglePositionRad)); - } - - SwerveModuleState getState() { - return new SwerveModuleState( - inputs.driveVelocityMetersPerSec, - Rotation2d.fromRadians(inputs.anglePositionRad)); - } - - void setDesiredState(SwerveModuleState state) { - SwerveModuleState optimizedState = optimize(state, Rotation2d.fromRadians(inputs.anglePositionRad)); - io.setDesiredState(optimizedState); - } - - void setBrakeMode(boolean brake) { - io.setBrakeMode(brake); - } - - void stop() { - io.stop(); - } - - private static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) { - Rotation2d delta = desiredState.angle.minus(currentAngle); - - if (Math.abs(delta.getDegrees()) > 90.0) { - return new SwerveModuleState( - -desiredState.speedMetersPerSecond, - desiredState.angle.plus(Rotation2d.fromDegrees(180.0))); - } - - return desiredState; - } - } -} 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 a8107b4..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java +++ /dev/null @@ -1,23 +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/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOPigeon2.java deleted file mode 100644 index dc26468..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOPigeon2.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 GyroIOPigeon2 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 GyroIOPigeon2() { - 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 237160f..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java +++ /dev/null @@ -1,27 +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 { - private double yawPositionRad = 0.0; - private double yawVelocityRadPerSec = 0.0; - - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = true; - inputs.yawPositionRad = yawPositionRad; - inputs.yawVelocityRadPerSec = yawVelocityRadPerSec; - inputs.pitchPositionRad = 0; - inputs.rollPositionRad = 0; - } - - @Override - public void zeroGyro() { - yawPositionRad = 0.0; - yawVelocityRadPerSec = 0.0; - } - - public void setYaw(double yawPositionRad, double yawVelocityRadPerSec) { - this.yawPositionRad = yawPositionRad; - this.yawVelocityRadPerSec = yawVelocityRadPerSec; - } -} 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 74fa7e0..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIO.java +++ /dev/null @@ -1,44 +0,0 @@ -package frc.robot.subsystems.swerve.module; - -import edu.wpi.first.math.kinematics.SwerveModuleState; -import org.littletonrobotics.junction.AutoLog; - -public interface ModuleIO { - @AutoLog - class ModuleIOInputs { - public boolean driveConnected = false; - public boolean angleConnected = false; - public boolean encoderConnected = false; - - 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 anglePositionRad = 0.0; - public double angleVelocityRadPerSec = 0.0; - public double angleAbsolutePositionRad = 0.0; - public double angleAppliedVolts = 0.0; - public double angleCurrentAmps = 0.0; - public double angleTempCelsius = 0.0; - } - - default void updateInputs(ModuleIOInputs inputs) { - } - - default void setDesiredState(SwerveModuleState state) { - } - - default void setDriveVoltage(double volts) { - } - - default void setAngleVoltage(double volts) { - } - - default void setBrakeMode(boolean brake) { - } - - default void stop() { - } -} 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 ccc78c4..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java +++ /dev/null @@ -1,76 +0,0 @@ -package frc.robot.subsystems.swerve.module; - -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.kinematics.SwerveModuleState; -import frc.robot.constants.Constants; -import frc.robot.constants.SwerveConstants; - -public class ModuleIOSim implements ModuleIO { - private final PIDController angleController = new PIDController( - SwerveConstants.Simulation.kP, - 0.0, - SwerveConstants.Simulation.kD); - - private double drivePositionMeters = 0.0; - private double driveVelocityMetersPerSec = 0.0; - private double anglePositionRad = 0.0; - private double angleVelocityRadPerSec = 0.0; - private double driveAppliedVolts = 0.0; - private double angleAppliedVolts = 0.0; - - public ModuleIOSim() { - angleController.enableContinuousInput(-Math.PI, Math.PI); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - drivePositionMeters += driveVelocityMetersPerSec * Constants.LOOP_TIME_SECONDS; - angleVelocityRadPerSec = angleAppliedVolts; - anglePositionRad += angleVelocityRadPerSec * Constants.LOOP_TIME_SECONDS; - - inputs.driveConnected = true; - inputs.angleConnected = true; - inputs.encoderConnected = true; - inputs.drivePositionMeters = drivePositionMeters; - inputs.driveVelocityMetersPerSec = driveVelocityMetersPerSec; - inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = Math.abs(driveAppliedVolts) * 2.0; - inputs.anglePositionRad = anglePositionRad; - inputs.angleVelocityRadPerSec = angleVelocityRadPerSec; - inputs.angleAbsolutePositionRad = anglePositionRad; - inputs.angleAppliedVolts = angleAppliedVolts; - inputs.angleCurrentAmps = Math.abs(angleAppliedVolts); - } - - @Override - public void setDesiredState(SwerveModuleState state) { - driveVelocityMetersPerSec = state.speedMetersPerSecond; - - driveAppliedVolts = MathUtil.clamp( - state.speedMetersPerSecond / SwerveConstants.MAX_LINEAR_SPEED_METERS_PER_SEC * 12.0, - -12.0, - 12.0); - angleAppliedVolts = MathUtil.clamp( - angleController.calculate(anglePositionRad, state.angle.getRadians()), - -12.0, - 12.0); - } - - @Override - public void setDriveVoltage(double volts) { - driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - driveVelocityMetersPerSec = driveAppliedVolts / 12.0 * SwerveConstants.MAX_LINEAR_SPEED_METERS_PER_SEC; - } - - @Override - public void setAngleVoltage(double volts) { - angleAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - } - - @Override - public void stop() { - setDesiredState(new SwerveModuleState(0.0, new Rotation2d(anglePositionRad))); - } -} 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 0fc6a15..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSparkMax.java +++ /dev/null @@ -1,233 +0,0 @@ -package frc.robot.subsystems.swerve.module; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; -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.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import frc.robot.constants.SwerveConstants; -import frc.robot.utils.SparkUtil; - -public class ModuleIOSparkMax implements ModuleIO { - private final SparkMax driveMotor; - private final SparkMax angleMotor; - private final CANcoder absoluteEncoder; - - private final RelativeEncoder driveEncoder; - private final RelativeEncoder angleEncoder; - private final SparkClosedLoopController driveController; - private final SparkClosedLoopController angleController; - - private final StatusSignal absolutePosition; - - public ModuleIOSparkMax( - int driveId, - int angleId, - int encoderId, - double absoluteEncoderOffsetDegrees, - boolean driveInverted, - boolean angleInverted, - boolean encoderInverted) { - driveMotor = new SparkMax(driveId, MotorType.kBrushless); - angleMotor = new SparkMax(angleId, MotorType.kBrushless); - - absoluteEncoder = new CANcoder(encoderId); - - driveEncoder = driveMotor.getEncoder(); - angleEncoder = angleMotor.getEncoder(); - - driveController = driveMotor.getClosedLoopController(); - angleController = angleMotor.getClosedLoopController(); - - absolutePosition = absoluteEncoder.getAbsolutePosition(); - - CANcoderConfiguration encoderConfig = new CANcoderConfiguration(); - encoderConfig.MagnetSensor.SensorDirection = encoderInverted - ? SensorDirectionValue.Clockwise_Positive - : SensorDirectionValue.CounterClockwise_Positive; - encoderConfig.MagnetSensor.MagnetOffset = -absoluteEncoderOffsetDegrees / 360.0; - encoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; - absoluteEncoder.getConfigurator().apply(encoderConfig); - - SparkMaxConfig driveConfig = new SparkMaxConfig(); - driveConfig - .inverted(driveInverted) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(SwerveConstants.PhysicalProperties.DRIVE_CURRENT_LIMIT) - .voltageCompensation(SwerveConstants.PhysicalProperties.OPTIMAL_VOLTAGE_VOLTS) - .openLoopRampRate(SwerveConstants.PhysicalProperties.DRIVE_RAMP_RATE_SECONDS) - .closedLoopRampRate(SwerveConstants.PhysicalProperties.DRIVE_RAMP_RATE_SECONDS); - driveConfig.encoder - .positionConversionFactor(SwerveConstants.DRIVE_POSITION_FACTOR_METERS_PER_ROTATION) - .velocityConversionFactor(SwerveConstants.DRIVE_VELOCITY_FACTOR_METERS_PER_SEC_PER_RPM); - driveConfig.closedLoop.pid( - SwerveConstants.PIDFProperties.DRIVE_kP, - SwerveConstants.PIDFProperties.DRIVE_kI, - SwerveConstants.PIDFProperties.DRIVE_kD); - - SparkMaxConfig angleConfig = new SparkMaxConfig(); - angleConfig - .inverted(angleInverted) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(SwerveConstants.PhysicalProperties.ANGLE_CURRENT_LIMIT) - .voltageCompensation(SwerveConstants.PhysicalProperties.OPTIMAL_VOLTAGE_VOLTS) - .openLoopRampRate(SwerveConstants.PhysicalProperties.ANGLE_RAMP_RATE_SECONDS) - .closedLoopRampRate(SwerveConstants.PhysicalProperties.ANGLE_RAMP_RATE_SECONDS); - angleConfig.encoder - .positionConversionFactor(SwerveConstants.ANGLE_POSITION_FACTOR_RAD_PER_ROTATION) - .velocityConversionFactor(SwerveConstants.ANGLE_VELOCITY_FACTOR_RAD_PER_SEC_PER_RPM); - angleConfig.closedLoop - .pid( - SwerveConstants.PIDFProperties.ANGLE_kP, - SwerveConstants.PIDFProperties.ANGLE_kI, - SwerveConstants.PIDFProperties.ANGLE_kD) - .positionWrappingEnabled(true) - .positionWrappingInputRange(-Math.PI, Math.PI); - - SparkUtil.tryUntilOk( - driveMotor, - 5, - () -> driveMotor.configure(driveConfig, ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters)); - SparkUtil.tryUntilOk( - angleMotor, - 5, - () -> angleMotor.configure(angleConfig, ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters)); - - syncAngleEncoder(); - } - - private void syncAngleEncoder() { - BaseStatusSignal.refreshAll(absolutePosition); - - SparkUtil.tryUntilOk( - angleMotor, - 5, - () -> angleEncoder.setPosition( - Units.rotationsToRadians(absolutePosition.getValueAsDouble()))); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.encoderConnected = BaseStatusSignal.refreshAll(absolutePosition).isOK(); - inputs.angleAbsolutePositionRad = Units.rotationsToRadians(absolutePosition.getValueAsDouble()); - - SparkUtil.sparkStickyFault = false; - SparkUtil.ifOk( - driveMotor, - new java.util.function.DoubleSupplier[] { - driveEncoder::getPosition, - driveEncoder::getVelocity, - driveMotor::getAppliedOutput, - driveMotor::getBusVoltage, - driveMotor::getOutputCurrent, - driveMotor::getMotorTemperature - }, - values -> { - inputs.drivePositionMeters = values[0]; - inputs.driveVelocityMetersPerSec = values[1]; - inputs.driveAppliedVolts = values[2] * values[3]; - inputs.driveCurrentAmps = values[4]; - inputs.driveTempCelsius = values[5]; - }); - inputs.driveConnected = !SparkUtil.sparkStickyFault; - - SparkUtil.sparkStickyFault = false; - SparkUtil.ifOk( - angleMotor, - new java.util.function.DoubleSupplier[] { - angleEncoder::getPosition, - angleEncoder::getVelocity, - angleMotor::getAppliedOutput, - angleMotor::getBusVoltage, - angleMotor::getOutputCurrent, - angleMotor::getMotorTemperature - }, - values -> { - inputs.anglePositionRad = values[0]; - inputs.angleVelocityRadPerSec = values[1]; - inputs.angleAppliedVolts = values[2] * values[3]; - inputs.angleCurrentAmps = values[4]; - inputs.angleTempCelsius = values[5]; - }); - inputs.angleConnected = !SparkUtil.sparkStickyFault; - } - - @Override - public void setDesiredState(SwerveModuleState state) { - if (Math.abs(state.speedMetersPerSecond) < 0.01) { - driveMotor.stopMotor(); - angleController.setSetpoint( - state.angle.getRadians(), - ControlType.kPosition, - ClosedLoopSlot.kSlot0); - - return; - } - - driveController.setSetpoint( - state.speedMetersPerSecond, - ControlType.kVelocity, - ClosedLoopSlot.kSlot0); - angleController.setSetpoint( - state.angle.getRadians(), - ControlType.kPosition, - ClosedLoopSlot.kSlot0); - } - - @Override - public void setDriveVoltage(double volts) { - driveMotor.setVoltage(volts); - } - - @Override - public void setAngleVoltage(double volts) { - angleMotor.setVoltage(volts); - } - - @Override - public void setBrakeMode(boolean brake) { - SparkMaxConfig driveConfig = new SparkMaxConfig(); - SparkMaxConfig angleConfig = new SparkMaxConfig(); - - driveConfig.idleMode(brake ? IdleMode.kBrake : IdleMode.kCoast); - angleConfig.idleMode(brake ? IdleMode.kBrake : IdleMode.kCoast); - - SparkUtil.tryUntilOk( - driveMotor, - 5, - () -> driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, - PersistMode.kPersistParameters)); - SparkUtil.tryUntilOk( - angleMotor, - 5, - () -> angleMotor.configure(angleConfig, ResetMode.kNoResetSafeParameters, - PersistMode.kPersistParameters)); - } - - @Override - public void stop() { - driveMotor.stopMotor(); - angleMotor.stopMotor(); - } - - public Rotation2d getAbsoluteAngle() { - BaseStatusSignal.refreshAll(absolutePosition); - return Rotation2d.fromRadians(Units.rotationsToRadians(absolutePosition.getValueAsDouble())); - } -} 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 index f54e62c..019f7ce 100644 --- a/src/main/java/frc/robot/utils/SparkUtil.java +++ b/src/main/java/frc/robot/utils/SparkUtil.java @@ -1,3 +1,10 @@ +// 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; @@ -23,7 +30,8 @@ public static void ifOk(SparkBase spark, DoubleSupplier supplier, DoubleConsumer } /** Processes a value from a Spark only if the value is valid. */ - public static void ifOk(SparkBase spark, DoubleSupplier[] suppliers, Consumer consumer) { + public static void ifOk( + SparkBase spark, DoubleSupplier[] suppliers, Consumer consumer) { double[] values = new double[suppliers.length]; for (int i = 0; i < suppliers.length; i++) { From faa7984d40b4a0154c85756add76e5e1bf369c70 Mon Sep 17 00:00:00 2001 From: Scaratech Date: Fri, 24 Apr 2026 08:09:01 -0400 Subject: [PATCH 3/5] feat: constants + spark max --- .../frc/robot/commands/DriveCommands.java | 2 +- .../subsystems/drive/DriveConstants.java | 75 +++++++++-------- .../subsystems/drive/DriveSubsystem.java | 2 +- .../robot/subsystems/drive/ModuleIOSpark.java | 84 ++++++++++++------- 4 files changed, 98 insertions(+), 65 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 4e5cbfa..2ab2eb9 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -259,7 +259,7 @@ public static Command wheelRadiusCharacterization(DriveSubsystem drive) { // Update gyro delta Commands.run( () -> { - var rotation = drive.getRotation(); + Rotation2d rotation = drive.getRotation(); state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); state.lastAngle = rotation; }) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index f177402..66249d6 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -17,8 +17,8 @@ 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(26.5); - public static final double wheelBase = Units.inchesToMeters(26.5); + 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), @@ -27,31 +27,36 @@ public class DriveConstants { new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0) }; - // Zeroed rotation values for each module, see setup instructions - public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.0); - public static final Rotation2d frontRightZeroRotation = new Rotation2d(0.0); - public static final Rotation2d backLeftZeroRotation = new Rotation2d(0.0); - public static final Rotation2d backRightZeroRotation = new Rotation2d(0.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 = 9; + public static final int pigeonCanId = 0; - public static final int frontLeftDriveCanId = 1; - public static final int backLeftDriveCanId = 3; - public static final int frontRightDriveCanId = 5; - public static final int backRightDriveCanId = 7; + 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 = 2; - public static final int backLeftTurnCanId = 4; - public static final int frontRightTurnCanId = 6; - public static final int backRightTurnCanId = 8; + 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 int driveMotorCurrentLimit = 50; - public static final double wheelRadiusMeters = Units.inchesToMeters(1.5); - public static final double driveMotorReduction = (45.0 * 22.0) / (14.0 * 15.0); // MAXSwerve with 14 pinion teeth - // and 22 spur teeth - public static final DCMotor driveGearbox = DCMotor.getNeoVortex(1); + 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 -> @@ -60,7 +65,7 @@ public class DriveConstants { // Wheel Rad/Sec // Drive PID configuration - public static final double driveKp = 0.0; + 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; @@ -70,28 +75,30 @@ public class DriveConstants { public static final double driveSimKv = 0.0789; // Turn motor configuration - public static final boolean turnInverted = false; + public static final boolean turnInverted = true; public static final int turnMotorCurrentLimit = 20; - public static final double turnMotorReduction = 9424.0 / 203.0; - public static final DCMotor turnGearbox = DCMotor.getNeo550(1); + public static final double turnMotorReduction = 21.4285714286; + public static final DCMotor turnGearbox = DCMotor.getNEO(1); // Turn encoder configuration - public static final boolean turnEncoderInverted = true; - public static final double turnEncoderPositionFactor = 2 * Math.PI; // Rotations -> Radians - public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // RPM -> Rad/Sec + 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 = 2.0; - public static final double turnKd = 0.0; + 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 = 74.088; - public static final double robotMOI = 6.883; - public static final double wheelCOF = 1.2; + 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, @@ -103,4 +110,4 @@ public class DriveConstants { driveMotorCurrentLimit, 1), moduleTranslations); -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index d503e40..4e654a1 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -136,7 +136,7 @@ public void periodic() { // Stop moving when disabled if (DriverStation.isDisabled()) { - for (var module : modules) { + for (Module module : modules) { module.stop(); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 7b8d614..a60264e 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -10,41 +10,43 @@ import static frc.robot.subsystems.drive.DriveConstants.*; import static frc.robot.utils.SparkUtil.*; -import com.revrobotics.AbsoluteEncoder; +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; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; -import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkFlexConfig; 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 Flex drive motor controller, Spark Max - * turn motor controller, - * and duty cycle absolute encoder. + * 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 SparkBase driveSpark; - private final SparkBase turnSpark; + private final SparkMax driveSpark; + private final SparkMax turnSpark; private final RelativeEncoder driveEncoder; - private final AbsoluteEncoder turnEncoder; + private final RelativeEncoder turnEncoder; + private final CANcoder turnAbsoluteEncoder; + private final StatusSignal turnAbsolutePosition; // Closed loop controllers private final SparkClosedLoopController driveController; @@ -68,7 +70,7 @@ public ModuleIOSpark(int module) { default -> Rotation2d.kZero; }; - driveSpark = new SparkFlex( + driveSpark = new SparkMax( switch (module) { case 0 -> frontLeftDriveCanId; case 1 -> frontRightDriveCanId; @@ -88,14 +90,25 @@ public ModuleIOSpark(int module) { }, 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.getAbsoluteEncoder(); + turnEncoder = turnSpark.getEncoder(); + turnAbsolutePosition = turnAbsoluteEncoder.getAbsolutePosition(); driveController = driveSpark.getClosedLoopController(); turnController = turnSpark.getClosedLoopController(); // Configure drive motor - SparkFlexConfig driveConfig = new SparkFlexConfig(); + SparkMaxConfig driveConfig = new SparkMaxConfig(); driveConfig + .inverted(driveInverted) .idleMode(IdleMode.kBrake) .smartCurrentLimit(driveMotorCurrentLimit) .voltageCompensation(12.0); @@ -130,21 +143,22 @@ public ModuleIOSpark(int module) { .idleMode(IdleMode.kBrake) .smartCurrentLimit(turnMotorCurrentLimit) .voltageCompensation(12.0); - turnConfig.absoluteEncoder + turnConfig.encoder .inverted(turnEncoderInverted) .positionConversionFactor(turnEncoderPositionFactor) .velocityConversionFactor(turnEncoderVelocityFactor) - .averageDepth(2); + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); turnConfig.closedLoop - .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) .pid(turnKp, 0.0, turnKd); turnConfig.signals - .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) - .absoluteEncoderVelocityAlwaysOn(true) - .absoluteEncoderVelocityPeriodMs(20) + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) .outputCurrentPeriodMs(20); @@ -154,11 +168,12 @@ public ModuleIOSpark(int module) { 5, () -> turnSpark.configure( turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + syncTurnEncoderWithAbsolute(); // Create odometry queues timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); drivePositionQueue = SparkOdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition); - turnPositionQueue = SparkOdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition); + turnPositionQueue = SparkOdometryThread.getInstance().registerSignal(this::getAbsoluteAngleRadians); } @Override @@ -179,10 +194,8 @@ public void updateInputs(ModuleIOInputs inputs) { // Update turn inputs sparkStickyFault = false; - ifOk( - turnSpark, - turnEncoder::getPosition, - (value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation)); + StatusCode turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + inputs.turnPosition = getAbsoluteAngle(); ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); ifOk( turnSpark, @@ -190,13 +203,13 @@ public void updateInputs(ModuleIOInputs inputs) { (values) -> inputs.turnAppliedVolts = values[0] * values[1]); ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); - inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault); + inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault && 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((Double value) -> new Rotation2d(value).minus(zeroRotation)) + .map(Rotation2d::new) .toArray(Rotation2d[]::new); timestampQueue.clear(); @@ -228,7 +241,7 @@ public void setDriveVelocity(double velocityRadPerSec) { @Override public void setTurnPosition(Rotation2d rotation) { double setpoint = MathUtil.inputModulus( - rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); + rotation.getRadians(), turnPIDMinInput, turnPIDMaxInput); turnController.setSetpoint(setpoint, ControlType.kPosition); } @@ -236,7 +249,7 @@ public void setTurnPosition(Rotation2d rotation) { public void setBrakeMode(boolean enabled) { IdleMode idleMode = enabled ? IdleMode.kBrake : IdleMode.kCoast; - SparkFlexConfig driveConfig = new SparkFlexConfig(); + SparkMaxConfig driveConfig = new SparkMaxConfig(); driveConfig.idleMode(idleMode); tryUntilOk( driveSpark, @@ -256,4 +269,17 @@ public void setBrakeMode(boolean enabled) { 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(); + } } From 983ef3f01ad9c98b86273fc62a565d33f8823e1b Mon Sep 17 00:00:00 2001 From: Scaratech Date: Mon, 27 Apr 2026 07:32:56 -0400 Subject: [PATCH 4/5] fix: requested changes --- src/main/java/frc/robot/RobotContainer.java | 3 ++- .../frc/robot/commands/DriveCommands.java | 10 +++------- .../subsystems/drive/DriveSubsystem.java | 19 +++++-------------- .../frc/robot/subsystems/drive/Module.java | 4 ++++ .../frc/robot/subsystems/drive/ModuleIO.java | 3 +++ .../robot/subsystems/drive/ModuleIOSim.java | 2 ++ .../robot/subsystems/drive/ModuleIOSpark.java | 16 +++++++++++++--- 7 files changed, 32 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a3c8809..9e466a2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -127,7 +127,8 @@ public RobotContainer() { () -> -driverController.getLeftX(), () -> -driverController.getRightX(), driverController.b(), - () -> new Pose2d(gameHelpers.getVirtualTargetTranslation(), Rotation2d.kZero)); + gameHelpers::getVirtualTargetTranslation, + Rotation2d.k180deg); drive.setDefaultCommand(driveFieldOrientedAngularVelocity); // Signal readiness to the driver so they know when to shoot diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 2ab2eb9..a1268fc 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -10,9 +10,7 @@ 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.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -54,9 +52,7 @@ private static Translation2d getLinearVelocityFromJoysticks(double x, double y) linearMagnitude = linearMagnitude * linearMagnitude; // Return new linear velocity - return new Pose2d(Translation2d.kZero, linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, Rotation2d.kZero)) - .getTranslation(); + return new Translation2d(linearMagnitude, linearDirection); } /** @@ -198,12 +194,12 @@ public static Command feedforwardCharacterization(DriveSubsystem drive) { .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); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 4e654a1..50c9dbb 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -23,6 +23,7 @@ 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; @@ -37,7 +38,6 @@ 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.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; import frc.robot.constants.Constants; @@ -350,33 +350,24 @@ public void addVisionMeasurement( /** * Drives field-relative with joystick rotation until the trigger is held, then - * locks heading toward the supplied target pose translation. + * locks heading toward the supplied target translation. */ public Command driveCommand( DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier omega, BooleanSupplier aimAtTarget, - Supplier targetPoseSupplier) { + Supplier targetTranslationSupplier, + Rotation2d angleOffset) { Command manualDrive = DriveCommands.joystickDrive(this, translationX, translationY, omega); Command targetLockDrive = DriveCommands.joystickDriveAtAngle( this, translationX, translationY, - () -> targetPoseSupplier.get().getTranslation().minus(getPose().getTranslation()).getAngle()); + () -> targetTranslationSupplier.get().minus(getPose().getTranslation()).getAngle().plus(angleOffset)); return Commands.either(targetLockDrive, manualDrive, aimAtTarget).withName("DriveCommand"); } - /** Convenience overload for WPILib triggers. */ - public Command driveCommand( - DoubleSupplier translationX, - DoubleSupplier translationY, - DoubleSupplier omega, - Trigger aimAtTarget, - Supplier targetPoseSupplier) { - return driveCommand(translationX, translationY, omega, aimAtTarget::getAsBoolean, targetPoseSupplier); - } - /** Locks the modules in an X pattern while the command is scheduled. */ public Command lockWheels() { return run(this::stopWithX).withName("DriveLockWheels"); diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 621d9c8..d27fb40 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -23,6 +23,7 @@ public class Module { private final Alert driveDisconnectedAlert; private final Alert turnDisconnectedAlert; + private final Alert turnAbsoluteDisconnectedAlert; private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; public Module(ModuleIO io, int index) { @@ -34,6 +35,8 @@ public Module(ModuleIO io, int 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() { @@ -53,6 +56,7 @@ public void periodic() { // Update alerts driveDisconnectedAlert.set(!inputs.driveConnected); turnDisconnectedAlert.set(!inputs.turnConnected); + turnAbsoluteDisconnectedAlert.set(!inputs.turnAbsoluteConnected); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index fcf3791..109b451 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -25,6 +25,9 @@ public static class ModuleIOInputs { 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[] {}; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index d9892ea..6c40beb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -75,6 +75,8 @@ public void updateInputs(ModuleIOInputs inputs) { 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) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index a60264e..330c61f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -47,6 +47,7 @@ public class ModuleIOSpark implements ModuleIO { private final RelativeEncoder turnEncoder; private final CANcoder turnAbsoluteEncoder; private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnAbsolutePositionOdometry; // Closed loop controllers private final SparkClosedLoopController driveController; @@ -60,6 +61,7 @@ public class ModuleIOSpark implements ModuleIO { // 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) { @@ -102,6 +104,7 @@ public ModuleIOSpark(int module) { driveEncoder = driveSpark.getEncoder(); turnEncoder = turnSpark.getEncoder(); turnAbsolutePosition = turnAbsoluteEncoder.getAbsolutePosition(); + turnAbsolutePositionOdometry = turnAbsolutePosition.clone(); driveController = driveSpark.getClosedLoopController(); turnController = turnSpark.getClosedLoopController(); @@ -168,12 +171,17 @@ public ModuleIOSpark(int module) { 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(this::getAbsoluteAngleRadians); + turnPositionQueue = SparkOdometryThread.getInstance() + .registerSignal(() -> Rotation2d.fromRotations(turnAbsolutePositionOdometry.refresh().getValueAsDouble()) + .minus(zeroRotation) + .getRadians()); } @Override @@ -195,15 +203,17 @@ public void updateInputs(ModuleIOInputs inputs) { sparkStickyFault = false; StatusCode turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); - inputs.turnPosition = getAbsoluteAngle(); + 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 && turnEncoderStatus.equals(StatusCode.OK)); + 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(); From c7469bf41d16eb34e6fe594efa590605f958a7d1 Mon Sep 17 00:00:00 2001 From: Scaratech Date: Tue, 28 Apr 2026 09:59:37 -0400 Subject: [PATCH 5/5] fix: things --- .../frc/robot/commands/DriveCommands.java | 74 +++++++++++++++++++ .../subsystems/drive/DriveSubsystem.java | 10 +-- 2 files changed, 79 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index a1268fc..9f3b0a4 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -26,6 +26,7 @@ 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; @@ -150,6 +151,79 @@ public static Command joystickDriveAtAngle( .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. * diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 50c9dbb..f0ce49f 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -36,7 +36,6 @@ 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.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; @@ -359,13 +358,14 @@ public Command driveCommand( BooleanSupplier aimAtTarget, Supplier targetTranslationSupplier, Rotation2d angleOffset) { - Command manualDrive = DriveCommands.joystickDrive(this, translationX, translationY, omega); - Command targetLockDrive = DriveCommands.joystickDriveAtAngle( + return DriveCommands.joystickDriveMaybeAtAngle( this, translationX, translationY, - () -> targetTranslationSupplier.get().minus(getPose().getTranslation()).getAngle().plus(angleOffset)); - return Commands.either(targetLockDrive, manualDrive, aimAtTarget).withName("DriveCommand"); + omega, + aimAtTarget, + () -> targetTranslationSupplier.get().minus(getPose().getTranslation()).getAngle().plus(angleOffset)) + .withName("DriveCommand"); } /** Locks the modules in an X pattern while the command is scheduled. */