From 9e01ed897905af7976152ea352af31d44640e732 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 17 Mar 2026 19:34:39 -0700 Subject: [PATCH 01/24] pre vision shooting --- .../org/team5924/frc2026/RobotContainer.java | 68 ++++--- .../frc2026/commands/drive/DriveCommands.java | 184 ++++++++++++++++++ .../commands/shooter/AutoScoreCommands.java | 33 ++-- .../frc2026/util/LaunchCalculator.java | 20 +- .../frc2026/util/LauncherConstants.java | 2 + 5 files changed, 254 insertions(+), 53 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index ff59d52..dafbb1b 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -45,12 +45,15 @@ import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivot; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivotIO; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivotIOSim; +import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivotIOTalonFX; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIO; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIOSim; +import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIOTalonFX; import org.team5924.frc2026.subsystems.rollers.hopper.Hopper; import org.team5924.frc2026.subsystems.rollers.hopper.HopperIO; import org.team5924.frc2026.subsystems.rollers.hopper.HopperIOSim; +import org.team5924.frc2026.subsystems.rollers.hopper.HopperIOTalonFX; import org.team5924.frc2026.subsystems.rollers.indexer.Indexer; import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIO; import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIOSim; @@ -58,9 +61,11 @@ import org.team5924.frc2026.subsystems.rollers.intake.Intake; import org.team5924.frc2026.subsystems.rollers.intake.IntakeIO; import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOSim; +import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOTalonFX; import org.team5924.frc2026.subsystems.turret.Turret; import org.team5924.frc2026.subsystems.turret.TurretIO; import org.team5924.frc2026.subsystems.turret.TurretIOSim; +import org.team5924.frc2026.subsystems.turret.TurretIOTalonFX; public class RobotContainer { // Subsystems @@ -93,29 +98,6 @@ public RobotContainer() { // Real robot, instantiate hardware IO implementations // -------------------------- real -------------------------- - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - (pose) -> {}); - - // intake = new Intake(new IntakeIOTalonFX()); - // intakePivot = new IntakePivot(new IntakePivotIOTalonFX()); - // hopper = new Hopper(new HopperIOTalonFX()); - indexer = new Indexer(new IndexerIOTalonFX()); - - // shooterHoodLeft = new ShooterHood(new ShooterHoodIOTalonFX(true), true); - // flywheelLeft = new Flywheel(new FlywheelIOTalonFX(true), true); - // turretLeft = new Turret(new TurretIOTalonFX(true), true); - - // shooterHoodRight = new ShooterHood(new ShooterHoodIOTalonFX(false), false); - flywheelRight = new Flywheel(new FlywheelIOTalonFX(false), false); - // turretRight = new Turret(new TurretIOTalonFX(false), false); - - // ---------------------------- IO ---------------------------- // drive = // new Drive( // new GyroIOPigeon2(), @@ -125,18 +107,41 @@ public RobotContainer() { // new ModuleIOTalonFX(TunerConstants.BackRight), // (pose) -> {}); - intake = new Intake(new IntakeIO() {}); - intakePivot = new IntakePivot(new IntakePivotIO() {}); - hopper = new Hopper(new HopperIO() {}); + intake = new Intake(new IntakeIOTalonFX()); + intakePivot = new IntakePivot(new IntakePivotIOTalonFX()); + hopper = new Hopper(new HopperIOTalonFX()); + indexer = new Indexer(new IndexerIOTalonFX()); + + shooterHoodLeft = new ShooterHood(new ShooterHoodIOTalonFX(true), true); + flywheelLeft = new Flywheel(new FlywheelIOTalonFX(true), true); + turretLeft = new Turret(new TurretIOTalonFX(true), true); + + shooterHoodRight = new ShooterHood(new ShooterHoodIOTalonFX(false), false); + flywheelRight = new Flywheel(new FlywheelIOTalonFX(false), false); + turretRight = new Turret(new TurretIOTalonFX(false), false); + + // ---------------------------- IO ---------------------------- + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + (pose) -> {}); + + // intake = new Intake(new IntakeIO() {}); + // intakePivot = new IntakePivot(new IntakePivotIO() {}); + // hopper = new Hopper(new HopperIO() {}); // indexer = new Indexer(new IndexerIO() {}); - shooterHoodLeft = new ShooterHood(new ShooterHoodIO() {}, true); - flywheelLeft = new Flywheel(new FlywheelIO() {}, true); - turretLeft = new Turret(new TurretIO() {}, true); + // shooterHoodLeft = new ShooterHood(new ShooterHoodIO() {}, true); + // flywheelLeft = new Flywheel(new FlywheelIO() {}, true); + // turretLeft = new Turret(new TurretIO() {}, true); - shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); + // shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); // flywheelRight = new Flywheel(new FlywheelIO() {}, false); - turretRight = new Turret(new TurretIO() {}, false); + // turretRight = new Turret(new TurretIO() {}, false); break; case SIM: @@ -308,7 +313,6 @@ private void configureButtonBindings() { drive.setPose( new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); // zero gyro driveController.start().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true)); - // )); // ### hopper on by default hopper.setDefaultCommand( diff --git a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java index 41cb771..6107d6c 100644 --- a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java @@ -31,15 +31,27 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; + +import static edu.wpi.first.units.Units.MetersPerSecond; + import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; +import org.team5924.frc2026.Constants; import org.team5924.frc2026.FieldConstants; +import org.team5924.frc2026.RobotState; +import org.team5924.frc2026.generated.TunerConstants; import org.team5924.frc2026.subsystems.drive.Drive; import org.team5924.frc2026.util.AllianceFlipUtil; +import org.team5924.frc2026.util.GeomUtil; +import org.team5924.frc2026.util.LaunchCalculator; +import org.team5924.frc2026.util.LauncherConstants; +import org.team5924.frc2026.util.LoggedTunableNumber; public class DriveCommands { private static final double DEADBAND = 0.1; @@ -52,6 +64,24 @@ public class DriveCommands { 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 static final LoggedTunableNumber driveLaunchKp = + new LoggedTunableNumber("DriveCommands/Launching/kP", 8.0); + private static final LoggedTunableNumber driveLaunchKd = + new LoggedTunableNumber("DriveCommands/Launching/kD", 0.5); + private static final LoggedTunableNumber driveYawLaunchToleranceDeg = + new LoggedTunableNumber("DriveCommands/Launching/YawToleranceDeg", 5.0); + private static final LoggedTunableNumber drivePitchLaunchToleranceDeg = + new LoggedTunableNumber("DriveCommands/Launching/PitchToleranceDeg", 5.0); + private static final LoggedTunableNumber driveRollLaunchToleranceDeg = + new LoggedTunableNumber("DriveCommands/Launching/RollToleranceDeg", 5.0); + private static final LoggedTunableNumber driveLaunchMaxPolarVelocityRadPerSec = + new LoggedTunableNumber("DriveCommands/Launching/MaxPolarVelocityRadPerSec", 0.6); + private static final LoggedTunableNumber driveLauncherCORMinErrorDeg = + new LoggedTunableNumber("DriveCommands/Launching/DriveLauncherCORMinErrorDeg", 15.0); + private static final LoggedTunableNumber driveLauncherCORMaxErrorDeg = + new LoggedTunableNumber("DriveCommands/Launching/DriveLauncherCORMaxErrorDeg", 30.0); + + private DriveCommands() {} private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { @@ -173,6 +203,160 @@ public static Command driveFacingHub( .minus(drive.getPose().getTranslation()) .getAngle()); } + + public static Command joystickDriveWhileLaunching( + Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { + // Create command + return Commands.run( + () -> { + // Run PID controller + final var parameters = LaunchCalculator.getInstance().getParameters(); + double omegaOutput = + parameters.driveVelocity() + + (parameters + .driveAngle() + .minus(RobotState.getInstance().getRotation()) + .getRadians() + * driveLaunchKp.get()) + + ((parameters.driveVelocity() + - RobotState.getInstance().getRobotVelocity().omegaRadiansPerSecond) + * driveLaunchKd.get()); + + // Calculate speeds + Translation2d fieldRelativeLinearVelocity = + getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()) + .times(TunerConstants.kSpeedAt12Volts.in(MetersPerSecond)); + if (AllianceFlipUtil.shouldFlip()) { + fieldRelativeLinearVelocity = fieldRelativeLinearVelocity.times(-1.0); + } + + // Only limit if launching, not passing + if (!LaunchCalculator.getInstance().getParameters().passing()) { + // Calculate max linear velocity magnitude based on the max polar velocity + double maxLinearVelocityMagnitude = Double.POSITIVE_INFINITY; + double robotAngle = + Math.abs( + AllianceFlipUtil.apply(FieldConstants.Hub.topCenterPoint.toTranslation2d()) + .minus(RobotState.getInstance().getEstimatedPose().getTranslation()) + .getAngle() + .minus(fieldRelativeLinearVelocity.getAngle()) + .getRadians()); + double robotHubDistance = + LaunchCalculator.getInstance().getParameters().distanceNoLookahead(); + double hubAngle = + driveLaunchMaxPolarVelocityRadPerSec.get() + * LaunchCalculator.getInstance().getNaiveTOF(robotHubDistance); + double lookaheadAngle = Math.PI - robotAngle - hubAngle; + + // Calculate limit if triangle is valid (otherwise no limit) + if (lookaheadAngle > 0) { + double robotLookaheadDistance = + robotHubDistance * Math.sin(hubAngle) / Math.sin(lookaheadAngle); + maxLinearVelocityMagnitude = + robotLookaheadDistance + / LaunchCalculator.getInstance().getNaiveTOF(robotHubDistance); + } + + // Apply limit to velocity + if (fieldRelativeLinearVelocity.getNorm() > maxLinearVelocityMagnitude) { + fieldRelativeLinearVelocity = + fieldRelativeLinearVelocity.times( + maxLinearVelocityMagnitude / fieldRelativeLinearVelocity.getNorm()); + } + } + + // Apply chassis speeds + double corScalar = + MathUtil.clamp( + (Math.abs( + parameters + .driveAngle() + .minus(RobotState.getInstance().getRotation()) + .getDegrees()) + - driveLauncherCORMinErrorDeg.get()) + / (driveLauncherCORMaxErrorDeg.get() - driveLauncherCORMinErrorDeg.get()), + 0.0, + 1.0); + Translation2d launcherToRobot = + LauncherConstants.robotToLauncherCenter.getTranslation().toTranslation2d().unaryMinus(); + ChassisSpeeds fieldRelativeSpeedsWithOffset = + GeomUtil.transformVelocity( + new ChassisSpeeds( + fieldRelativeLinearVelocity.getX(), + fieldRelativeLinearVelocity.getY(), + omegaOutput), + launcherToRobot.times(1.0 - corScalar), + RobotState.getInstance().getRotation()); + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + fieldRelativeSpeedsWithOffset, RobotState.getInstance().getRotation())); + + // Override robot setpoint speeds published by drive. We run our calculations using the + // speeds that will ultimately be applied once we are using the full robot-to-launcher + // transform. This prevents the setpoint from changing due to the shifting COR of the + // robot. + ChassisSpeeds fieldRelativeSpeedsWithFullOffset = + GeomUtil.transformVelocity( + new ChassisSpeeds( + fieldRelativeLinearVelocity.getX(), + fieldRelativeLinearVelocity.getY(), + omegaOutput), + launcherToRobot, + RobotState.getInstance().getRotation()); + RobotState.getInstance() + .setRobotSetpointVelocity( + ChassisSpeeds.discretize( + ChassisSpeeds.fromFieldRelativeSpeeds( + fieldRelativeSpeedsWithFullOffset, + RobotState.getInstance().getRotation()), + Constants.LOOP_PERIODIC_SECONDS)); + + // Log data + Logger.recordOutput( + "DriveCommands/Launching/SetpointPose", + new Pose2d( + RobotState.getInstance().getEstimatedPose().getTranslation(), + parameters.driveAngle())); + Logger.recordOutput("DriveCommands/Launching/AtGoalTolerance", atLaunchGoal()); + Logger.recordOutput( + "DriveCommands/Launching/ErrorPosition", + parameters.driveAngle().minus(RobotState.getInstance().getRotation())); + Logger.recordOutput( + "DriveCommands/Launching/ErrorVelocityRadPerSec", + parameters.driveVelocity() + - RobotState.getInstance().getRobotVelocity().omegaRadiansPerSecond); + Logger.recordOutput( + "DriveCommands/Launching/MeasuredPosition", RobotState.getInstance().getRotation()); + Logger.recordOutput( + "DriveCommands/Launching/MeasuredVelocityRadPerSec", + RobotState.getInstance().getRobotVelocity().omegaRadiansPerSecond); + Logger.recordOutput("DriveCommands/Launching/SetpointPosition", parameters.driveAngle()); + Logger.recordOutput( + "DriveCommands/Launching/SetpointVelocityRadPerSec", parameters.driveVelocity()); + }, + drive); + } + + public static boolean atLaunchGoal() { + return true; + // var rotation3d = + // RobotState.getInstance().getEstimatedRotation3dAtTimestamp(Timer.getTimestamp()); + // boolean inPitchAndRollTolerance = + // rotation3d.isEmpty() + // || (Math.abs(rotation3d.get().getX()) + // <= Units.degreesToRadians(driveRollLaunchToleranceDeg.get()) + // && Math.abs(rotation3d.get().getY()) + // <= Units.degreesToRadians(drivePitchLaunchToleranceDeg.get())); + + // return DriverStation.isEnabled() + // && Math.abs( + // RobotState.getInstance() + // .getRotation() + // .minus(LaunchCalculator.getInstance().getParameters().driveAngle()) + // .getRadians()) + // <= Units.degreesToRadians(driveYawLaunchToleranceDeg.get()) + // && inPitchAndRollTolerance; + } /** * Measures the velocity feedforward constants for the drive motors. diff --git a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java index 78ae3c5..8b50f1d 100644 --- a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java @@ -46,21 +46,28 @@ private AutoScoreCommands() {} public static Command runTrackTargetCommand( Turret turret, ShooterHood shooterHood, Flywheel flywheel, boolean isLeft) { return Commands.run( - () -> { - LaunchingParameters launchParams = LaunchCalculator.getInstance().getParameters(isLeft); + () -> { + LaunchingParameters launchParams = + LaunchCalculator.getInstance().getParameters(isLeft); - turret.setAutoInput(launchParams.turretRadians()); - shooterHood.setAutoInput(launchParams.hoodAngle()); - flywheel.setAutoInput(launchParams.flywheelSpeed()); + turret.setAutoInput(launchParams.turretRadians()); + shooterHood.setAutoInput(launchParams.hoodAngle()); + flywheel.setAutoInput(launchParams.flywheelSpeed()); - turret.setGoalState(TurretState.AUTO); - shooterHood.setGoalState(ShooterHoodState.AUTO); - flywheel.setGoalState(FlywheelState.AUTO); + turret.setGoalState(TurretState.AUTO); + shooterHood.setGoalState(ShooterHoodState.AUTO); + flywheel.setGoalState(FlywheelState.AUTO); - LaunchCalculator.getInstance().clearLaunchingParameters(); - }, - turret, - shooterHood, - flywheel); + LaunchCalculator.getInstance().clearLaunchingParameters(); + }, + turret, + shooterHood, + flywheel) + .finallyDo( + () -> { + turret.setGoalState(TurretState.OFF); + shooterHood.setGoalState(ShooterHoodState.OFF); + flywheel.setGoalState(FlywheelState.OFF); + }); } } diff --git a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java index 91c2452..570a687 100644 --- a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java +++ b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java @@ -272,9 +272,15 @@ public static double getMinTimeOfFlight() { public static double getMaxTimeOfFlight() { return timeOfFlightMap.get(maxDistance); } + public LaunchingParameters getParameters() { + return getParameters(robotToLauncherCenter); + } public LaunchingParameters getParameters(boolean isLeft) { - Transform3d robotToLauncher = isLeft ? robotToLauncherLeft : robotToLauncherRight; + return getParameters(isLeft ? robotToLauncherLeft : robotToLauncherRight); + } + + public LaunchingParameters getParameters(Transform3d robotToLauncher) { boolean passing = AllianceFlipUtil.applyX(RobotState.getInstance().getEstimatedPose().getX()) > FieldConstants.LinesVertical.hubCenter; @@ -331,12 +337,12 @@ public LaunchingParameters getParameters(boolean isLeft) { lookaheadLauncherToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); } - double turretRads = getTurretAngle(lookaheadPose, target, isLeft).getRadians(); + double turretRads = getTurretAngle(lookaheadPose, target).getRadians(); // Account for launcher being off center Pose2d lookaheadRobotPose = lookaheadPose.transformBy(robotToLauncher.toTransform2d().inverse()); - Rotation2d driveAngle = getDriveAngleWithLauncherOffset(lookaheadRobotPose, target, isLeft); + Rotation2d driveAngle = getDriveAngleWithLauncherOffset(lookaheadRobotPose, target, robotToLauncher); // Calculate remaining parameters double hoodAngle = @@ -392,8 +398,7 @@ public LaunchingParameters getParameters(boolean isLeft) { return latestParameters; } - private static Rotation2d getTurretAngle( - Pose2d turretPose, Translation2d target, boolean isLeft) { + private static Rotation2d getTurretAngle(Pose2d turretPose, Translation2d target) { Rotation2d turretToHub = target.minus(turretPose.getTranslation()).getAngle(); Rotation2d turretRads = turretPose.getRotation().minus(turretToHub); @@ -401,8 +406,7 @@ private static Rotation2d getTurretAngle( } private static Rotation2d getDriveAngleWithLauncherOffset( - Pose2d robotPose, Translation2d target, boolean isLeft) { - Transform3d robotToLauncher = isLeft ? robotToLauncherLeft : robotToLauncherRight; + Pose2d robotPose, Translation2d target, Transform3d robotToLauncher) {; Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle(); Rotation2d hubAngle = @@ -471,7 +475,7 @@ public static Pose2d getStationaryAimedPose( return new Pose2d( robotTranslation, - getDriveAngleWithLauncherOffset(robotTranslation.toPose2d(), target, isLeft)); + getDriveAngleWithLauncherOffset(robotTranslation.toPose2d(), target, isLeft ? robotToLauncherLeft : robotToLauncherRight)); } /** Adjusts the hood angle offset up or down the specified amount. */ diff --git a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java index 4879bf4..fa8f948 100644 --- a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java +++ b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java @@ -29,5 +29,7 @@ public class LauncherConstants { new Transform3d( Constants.TurretRight.ROBOT_TO_TURRET, new Rotation3d(0.0, 0.0, Math.PI)); // TODO: update + public static Transform3d robotToLauncherCenter = robotToLauncherLeft.plus(robotToLauncherRight).div(2); + private LauncherConstants() {} } From 8639b3678005c2ef05fcf24e8e2ce8c35dae1973 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 17 Mar 2026 19:50:22 -0700 Subject: [PATCH 02/24] pre intake pivot merge --- .../org/team5924/frc2026/RobotContainer.java | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index dafbb1b..5997a61 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -46,6 +46,7 @@ import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivotIO; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivotIOSim; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivotIOTalonFX; +import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivot.IntakePivotState; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIO; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIOSim; @@ -55,6 +56,7 @@ import org.team5924.frc2026.subsystems.rollers.hopper.HopperIOSim; import org.team5924.frc2026.subsystems.rollers.hopper.HopperIOTalonFX; import org.team5924.frc2026.subsystems.rollers.indexer.Indexer; +import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState; import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIO; import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIOSim; import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIOTalonFX; @@ -78,11 +80,9 @@ public class RobotContainer { private final ShooterHood shooterHoodRight; private final Flywheel flywheelRight; - private final Turret turretRight; private final ShooterHood shooterHoodLeft; private final Flywheel flywheelLeft; - private final Turret turretLeft; // Controller private final CommandXboxController driveController = new CommandXboxController(0); @@ -114,11 +114,9 @@ public RobotContainer() { shooterHoodLeft = new ShooterHood(new ShooterHoodIOTalonFX(true), true); flywheelLeft = new Flywheel(new FlywheelIOTalonFX(true), true); - turretLeft = new Turret(new TurretIOTalonFX(true), true); shooterHoodRight = new ShooterHood(new ShooterHoodIOTalonFX(false), false); flywheelRight = new Flywheel(new FlywheelIOTalonFX(false), false); - turretRight = new Turret(new TurretIOTalonFX(false), false); // ---------------------------- IO ---------------------------- drive = @@ -165,11 +163,9 @@ public RobotContainer() { shooterHoodLeft = new ShooterHood(new ShooterHoodIOSim(true), true); flywheelLeft = new Flywheel(new FlywheelIOSim(), true); - turretLeft = new Turret(new TurretIOSim(true), true); shooterHoodRight = new ShooterHood(new ShooterHoodIOSim(false), false); flywheelRight = new Flywheel(new FlywheelIOSim(), false); - turretRight = new Turret(new TurretIOSim(false), false); break; @@ -191,11 +187,9 @@ public RobotContainer() { shooterHoodLeft = new ShooterHood(new ShooterHoodIO() {}, true); flywheelLeft = new Flywheel(new FlywheelIO() {}, true); - turretLeft = new Turret(new TurretIO() {}, true); shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); flywheelRight = new Flywheel(new FlywheelIO() {}, false); - turretRight = new Turret(new TurretIO() {}, false); break; } @@ -382,8 +376,8 @@ private void configureButtonBindings() { .onTrue( Commands.runOnce( () -> { - // intakePivot.setGoalState(IntakePivotState.SHOOTING); - indexer.setGoalState(Indexer.IndexerState.INDEXING); + intakePivot.setGoalState(IntakePivotState.DOWN); + indexer.setGoalState(IndexerState.INDEXING); }, // intakePivot, indexer)); @@ -392,8 +386,8 @@ private void configureButtonBindings() { .onFalse( Commands.runOnce( () -> { - // intakePivot.setGoalState(IntakePivotState.STOW); - indexer.setGoalState(Indexer.IndexerState.OFF); + intakePivot.setGoalState(IntakePivotState.OFF); + indexer.setGoalState(IndexerState.OFF); }, // intakePivot, indexer)); From a5ddcb76912381a2820adec158cb2c28dae1841d Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 17 Mar 2026 20:27:08 -0700 Subject: [PATCH 03/24] pre systems check --- .../org/team5924/frc2026/RobotContainer.java | 337 ++++++------------ .../frc2026/commands/drive/DriveCommands.java | 16 +- .../frc2026/subsystems/flywheel/Flywheel.java | 2 +- .../frc2026/util/LaunchCalculator.java | 12 +- .../frc2026/util/LauncherConstants.java | 3 +- 5 files changed, 122 insertions(+), 248 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index bf88338..537a2bd 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -34,8 +34,10 @@ import org.team5924.frc2026.generated.TunerConstants; import org.team5924.frc2026.subsystems.drive.Drive; import org.team5924.frc2026.subsystems.drive.GyroIO; +import org.team5924.frc2026.subsystems.drive.GyroIOPigeon2; import org.team5924.frc2026.subsystems.drive.GyroIOSim; import org.team5924.frc2026.subsystems.drive.ModuleIO; +import org.team5924.frc2026.subsystems.drive.ModuleIOTalonFX; import org.team5924.frc2026.subsystems.drive.ModuleIOTalonFXSim; import org.team5924.frc2026.subsystems.flywheel.Flywheel; import org.team5924.frc2026.subsystems.flywheel.Flywheel.FlywheelState; @@ -65,10 +67,6 @@ import org.team5924.frc2026.subsystems.rollers.intake.IntakeIO; import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOSim; import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOTalonFX; -import org.team5924.frc2026.subsystems.turret.Turret; -import org.team5924.frc2026.subsystems.turret.TurretIO; -import org.team5924.frc2026.subsystems.turret.TurretIOSim; -import org.team5924.frc2026.subsystems.turret.TurretIOTalonFX; public class RobotContainer { // Subsystems @@ -99,14 +97,14 @@ public RobotContainer() { // Real robot, instantiate hardware IO implementations // -------------------------- real -------------------------- - // drive = - // new Drive( - // new GyroIOPigeon2(), - // new ModuleIOTalonFX(TunerConstants.FrontLeft), - // new ModuleIOTalonFX(TunerConstants.FrontRight), - // new ModuleIOTalonFX(TunerConstants.BackLeft), - // new ModuleIOTalonFX(TunerConstants.BackRight), - // (pose) -> {}); + drive = + new Drive( + new GyroIOPigeon2(), + new ModuleIOTalonFX(TunerConstants.FrontLeft), + new ModuleIOTalonFX(TunerConstants.FrontRight), + new ModuleIOTalonFX(TunerConstants.BackLeft), + new ModuleIOTalonFX(TunerConstants.BackRight), + (pose) -> {}); intake = new Intake(new IntakeIOTalonFX()); intakePivot = new IntakePivot(new IntakePivotIOTalonFX()); @@ -120,14 +118,14 @@ public RobotContainer() { flywheelRight = new Flywheel(new FlywheelIOTalonFX(false), false); // ---------------------------- IO ---------------------------- - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - (pose) -> {}); + // drive = + // new Drive( + // new GyroIO() {}, + // new ModuleIO() {}, + // new ModuleIO() {}, + // new ModuleIO() {}, + // new ModuleIO() {}, + // (pose) -> {}); // intake = new Intake(new IntakeIO() {}); // intakePivot = new IntakePivot(new IntakePivotIO() {}); @@ -271,29 +269,18 @@ private void configureButtonBindings() { () -> -driveController.getLeftX() * Constants.SLOW_MODE_MULTI, () -> -driveController.getRightX() * Constants.SLOW_MODE_MULTI)); - // [driver] 0-DEGREE MODE - driveController - .a() - .onTrue( - DriveCommands.joystickDriveAtAngle( - drive, - () -> -driveController.getLeftY(), - () -> -driveController.getLeftX(), - () -> Rotation2d.kZero)); - - // [driver] Switch to X pattern when X button is pressed - driveController.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + // // [driver] 0-DEGREE MODE + // driveController + // .a() + // .onTrue( + // DriveCommands.joystickDriveAtAngle( + // drive, + // () -> -driveController.getLeftY(), + // () -> -driveController.getLeftX(), + // () -> Rotation2d.kZero)); - // [driver] Reset gyro to 0° when B button is pressed - driveController - .b() - .onTrue( - Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), - drive) - .ignoringDisable(true)); + // // [driver] Switch to X pattern when X button is pressed + // driveController.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); final Runnable resetGyro = Constants.currentMode == Constants.Mode.SIM @@ -307,6 +294,13 @@ private void configureButtonBindings() { new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); // zero gyro driveController.start().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true)); + // [driver] Reset gyro to 0° when B button is pressed + driveController + .b() + .onTrue( + Commands.runOnce(resetGyro, drive) + .ignoringDisable(true)); + // ### hopper on by default hopper.setDefaultCommand( Commands.run(() -> hopper.setGoalState(Hopper.HopperState.ON), hopper)); @@ -323,236 +317,109 @@ private void configureButtonBindings() { // // ### intake pivot down/stow - intakePivot.setDefaultCommand( - Commands.run( - () -> { - intakePivot.setGoalState(IntakePivotState.MANUAL); - intakePivot.setInput(operatorController.getLeftX()); - }, - intakePivot)); - - operatorController - .leftBumper() - .onTrue( - Commands.runOnce( - () -> { - intakePivot.setGoalState(IntakePivotState.DOWN); - // intake.setGoalState(IntakeState.INTAKE); - }, - intakePivot /*, - intake*/)); - operatorController - .rightBumper() - .onTrue( - Commands.runOnce( - () -> { - intakePivot.setGoalState(IntakePivotState.STOW); - // intake.setGoalState(IntakeState.OFF); - }, - intakePivot /*, - intake*/)); - - // // // ### intake pivot spit - driveController - .leftTrigger() - .onTrue( - Commands.runOnce( - () -> { - intakePivot.setGoalState(IntakePivotState.DOWN); - intake.setGoalState(IntakeState.SPITOUT); - }, - intakePivot, - intake)); - driveController - .leftTrigger() - .onFalse( - Commands.runOnce( - () -> { - intakePivot.setGoalState(IntakePivotState.STOW); - intake.setGoalState(IntakeState.OFF); - }, - intakePivot, - intake)); - - // // ### indexing - operatorController - .a() - .onTrue( - Commands.runOnce( - () -> { - intakePivot.setGoalState(IntakePivotState.DOWN); - indexer.setGoalState(IndexerState.INDEXING); - }, - // intakePivot, - indexer)); - operatorController - .a() - .onFalse( - Commands.runOnce( - () -> { - intakePivot.setGoalState(IntakePivotState.OFF); - indexer.setGoalState(IndexerState.OFF); - }, - // intakePivot, - indexer)); - - // // ### shooting voltages - // operatorController - // .a() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B4); - // flywheelLeft.setGoalState(FlywheelState.B4); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .b() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B6); - // flywheelLeft.setGoalState(FlywheelState.B6); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .x() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B8); - // flywheelLeft.setGoalState(FlywheelState.B8); - // }, - // flywheelLeft, - // flywheelRight)); + // intakePivot.setDefaultCommand( + // Commands.run( + // () -> { + // intakePivot.setGoalState(IntakePivotState.MANUAL); + // intakePivot.setInput(operatorController.getLeftX()); + // }, + // intakePivot)); // operatorController - // .y() + // .leftBumper() // .onTrue( // Commands.runOnce( // () -> { - // flywheelRight.setGoalState(FlywheelState.B12); - // flywheelLeft.setGoalState(FlywheelState.B12); + // intakePivot.setGoalState(IntakePivotState.DOWN); + // // intake.setGoalState(IntakeState.INTAKE); // }, - // flywheelLeft, - // flywheelRight)); - + // intakePivot /*, + // intake*/)); // operatorController - // .rightTrigger() + // .rightBumper() // .onTrue( // Commands.runOnce( // () -> { - // flywheelRight.setGoalState(FlywheelState.OFF); - // flywheelLeft.setGoalState(FlywheelState.OFF); + // intakePivot.setGoalState(IntakePivotState.STOW); + // // intake.setGoalState(IntakeState.OFF); // }, - // flywheelLeft, - // flywheelRight)); - - // driveController - // .povUp() - // .onTrue( - // DriveCommands.driveFacingHub( - // drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); - - // // AJGAOEIBAWIBAEROBIAEROBIJAROIBHAEBOIAERBIOHAERBHIOAERb - // // operatorController - // // .leftTrigger() - // // .onTrue( - // // AutoScoreCommands.runTrackTargetCommand( - // // turretLeft, shooterHoodLeft, flywheelLeft, true)); - // // operatorController - // // .leftTrigger() - // // .onTrue( - // // AutoScoreCommands.runTrackTargetCommand( - // // turretRight, shooterHoodRight, flywheelRight, false)); - - // // --------------------------------------------------------- - - // // turretLeft.setDefaultCommand( - // // Commands.run( - // // () -> { - // // turretLeft.setGoalState(TurretState.MANUAL); - // // turretLeft.setInput(driveController.getRightX()); - // // }, - // // turretLeft)); + // intakePivot /*, + // intake*/)); + // // // // ### intake pivot spit // driveController // .leftTrigger() // .onTrue( // Commands.runOnce( // () -> { - // turretLeft.setGoalState(TurretState.ZERO); + // intakePivot.setGoalState(IntakePivotState.DOWN); + // intake.setGoalState(IntakeState.SPITOUT); // }, - // turretLeft)); - + // intakePivot, + // intake)); // driveController - // .rightTrigger() - // .onTrue( + // .leftTrigger() + // .onFalse( // Commands.runOnce( // () -> { - // turretLeft.setGoalState(TurretState.NINETY); + // intakePivot.setGoalState(IntakePivotState.STOW); + // intake.setGoalState(IntakeState.OFF); // }, - // turretLeft)); - - // // --------------------------------------------------------- - - // shooterHoodLeft.setDefaultCommand( - // Commands.run( - // () -> { - // shooterHoodLeft.setGoalState(ShooterHoodState.MANUAL); - // shooterHoodLeft.setInput(operatorController.getRightY()); - // }, - // shooterHoodLeft, - // shooterHoodRight)); - - operatorController - .leftTrigger() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.setGoalState(FlywheelState.OFF); - }, - flywheelRight)); + // intakePivot, + // intake)); - operatorController - .rightTrigger() + // // ### indexing + driveController + .rightBumper() .onTrue( Commands.runOnce( () -> { - flywheelRight.setGoalState(FlywheelState.FAST_LAUNCH); + intakePivot.setGoalState(IntakePivotState.DOWN); + intake.setGoalState(IntakeState.INTAKE); }, - flywheelRight)); - - operatorController - .y() - .onTrue( + intakePivot, + indexer)); + driveController + .rightBumper() + .onFalse( Commands.runOnce( () -> { - flywheelRight.setGoalState(FlywheelState.SLOW_LAUNCH); + intakePivot.setGoalState(IntakePivotState.STOW); + intake.setGoalState(IntakeState.OFF); }, - flywheelRight)); + intakePivot, + indexer)); - operatorController - .povDown() + driveController + .leftBumper() .onTrue( Commands.runOnce( - () -> { - flywheelRight.updateSetpointState(-5.0); - }, - flywheelRight)); + () -> { + flywheelLeft.setGoalState(FlywheelState.FAST_LAUNCH); + flywheelRight.setGoalState(FlywheelState.FAST_LAUNCH); + }, + flywheelLeft, + flywheelRight) + .andThen(Commands.waitSeconds(1.5)) + .andThen( + Commands.runOnce( + () -> { + indexer.setGoalState(IndexerState.INDEXING); + }, + indexer))); - operatorController - .povUp() - .onTrue( + driveController + .leftBumper() + .onFalse( Commands.runOnce( () -> { - flywheelRight.updateSetpointState(5.0); + flywheelLeft.setGoalState(FlywheelState.OFF); + flywheelRight.setGoalState(FlywheelState.OFF); + indexer.setGoalState(IndexerState.OFF); }, - flywheelRight)); + flywheelLeft, + flywheelRight, + indexer)); // // driveController // // .rightTrigger() diff --git a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java index 6107d6c..d64a93b 100644 --- a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java @@ -16,6 +16,8 @@ package org.team5924.frc2026.commands.drive; +import static edu.wpi.first.units.Units.MetersPerSecond; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.SlewRateLimiter; @@ -31,16 +33,12 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; - -import static edu.wpi.first.units.Units.MetersPerSecond; - import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; - import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; import org.team5924.frc2026.FieldConstants; @@ -81,7 +79,6 @@ public class DriveCommands { private static final LoggedTunableNumber driveLauncherCORMaxErrorDeg = new LoggedTunableNumber("DriveCommands/Launching/DriveLauncherCORMaxErrorDeg", 30.0); - private DriveCommands() {} private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { @@ -203,9 +200,9 @@ public static Command driveFacingHub( .minus(drive.getPose().getTranslation()) .getAngle()); } - + public static Command joystickDriveWhileLaunching( - Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { + Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { // Create command return Commands.run( () -> { @@ -278,7 +275,10 @@ public static Command joystickDriveWhileLaunching( 0.0, 1.0); Translation2d launcherToRobot = - LauncherConstants.robotToLauncherCenter.getTranslation().toTranslation2d().unaryMinus(); + LauncherConstants.robotToLauncherCenter + .getTranslation() + .toTranslation2d() + .unaryMinus(); ChassisSpeeds fieldRelativeSpeedsWithOffset = GeomUtil.transformVelocity( new ChassisSpeeds( diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java index e595765..55b1ac2 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java @@ -42,7 +42,7 @@ public class Flywheel extends SubsystemBase { public enum FlywheelState { OFF(() -> 0.0), MOVING(() -> 0.0), - FAST_LAUNCH(new LoggedTunableNumber("Flywheel/FastLaunch", 150)), + FAST_LAUNCH(new LoggedTunableNumber("Flywheel/FastLaunch", 100)), SLOW_LAUNCH(new LoggedTunableNumber("Flywheel/SlowLaunch", 50)), // current at which the example subsystem motor moves when controlled by the operator diff --git a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java index 570a687..42ad4dc 100644 --- a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java +++ b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java @@ -272,6 +272,7 @@ public static double getMinTimeOfFlight() { public static double getMaxTimeOfFlight() { return timeOfFlightMap.get(maxDistance); } + public LaunchingParameters getParameters() { return getParameters(robotToLauncherCenter); } @@ -342,7 +343,8 @@ public LaunchingParameters getParameters(Transform3d robotToLauncher) { // Account for launcher being off center Pose2d lookaheadRobotPose = lookaheadPose.transformBy(robotToLauncher.toTransform2d().inverse()); - Rotation2d driveAngle = getDriveAngleWithLauncherOffset(lookaheadRobotPose, target, robotToLauncher); + Rotation2d driveAngle = + getDriveAngleWithLauncherOffset(lookaheadRobotPose, target, robotToLauncher); // Calculate remaining parameters double hoodAngle = @@ -406,7 +408,8 @@ private static Rotation2d getTurretAngle(Pose2d turretPose, Translation2d target } private static Rotation2d getDriveAngleWithLauncherOffset( - Pose2d robotPose, Translation2d target, Transform3d robotToLauncher) {; + Pose2d robotPose, Translation2d target, Transform3d robotToLauncher) { + ; Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle(); Rotation2d hubAngle = @@ -475,7 +478,10 @@ public static Pose2d getStationaryAimedPose( return new Pose2d( robotTranslation, - getDriveAngleWithLauncherOffset(robotTranslation.toPose2d(), target, isLeft ? robotToLauncherLeft : robotToLauncherRight)); + getDriveAngleWithLauncherOffset( + robotTranslation.toPose2d(), + target, + isLeft ? robotToLauncherLeft : robotToLauncherRight)); } /** Adjusts the hood angle offset up or down the specified amount. */ diff --git a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java index fa8f948..ee61b32 100644 --- a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java +++ b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java @@ -29,7 +29,8 @@ public class LauncherConstants { new Transform3d( Constants.TurretRight.ROBOT_TO_TURRET, new Rotation3d(0.0, 0.0, Math.PI)); // TODO: update - public static Transform3d robotToLauncherCenter = robotToLauncherLeft.plus(robotToLauncherRight).div(2); + public static Transform3d robotToLauncherCenter = + robotToLauncherLeft.plus(robotToLauncherRight).div(2); private LauncherConstants() {} } From 4e2e0ccacd93db84fc7e28d4d3133e719a1bf265 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 17 Mar 2026 20:54:49 -0700 Subject: [PATCH 04/24] initial test, code wip: drive 5 sec crash, loop overrun --- .../java/org/team5924/frc2026/Constants.java | 4 +-- .../frc2026/commands/drive/DriveCommands.java | 29 +++++-------------- .../commands/shooter/AutoScoreCommands.java | 5 ---- .../frc2026/subsystems/flywheel/Flywheel.java | 7 ++++- .../pivots/shooterHood/ShooterHood.java | 10 ++++++- .../subsystems/rollers/indexer/Indexer.java | 2 +- 6 files changed, 25 insertions(+), 32 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index c73c683..3ebb84a 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -168,8 +168,8 @@ public final class IntakePivot { new SoftwareLimitSwitchConfigs() .withReverseSoftLimitThreshold(MIN_POSITION_ROTATIONS) .withForwardSoftLimitThreshold(MAX_POSITION_ROTATIONS) - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitEnable(true); + .withForwardSoftLimitEnable(false) + .withReverseSoftLimitEnable(false); public static final FeedbackConfigs FEEDBACK_CONFIGS = new FeedbackConfigs() diff --git a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java index d64a93b..e36005f 100644 --- a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java @@ -68,10 +68,6 @@ public class DriveCommands { new LoggedTunableNumber("DriveCommands/Launching/kD", 0.5); private static final LoggedTunableNumber driveYawLaunchToleranceDeg = new LoggedTunableNumber("DriveCommands/Launching/YawToleranceDeg", 5.0); - private static final LoggedTunableNumber drivePitchLaunchToleranceDeg = - new LoggedTunableNumber("DriveCommands/Launching/PitchToleranceDeg", 5.0); - private static final LoggedTunableNumber driveRollLaunchToleranceDeg = - new LoggedTunableNumber("DriveCommands/Launching/RollToleranceDeg", 5.0); private static final LoggedTunableNumber driveLaunchMaxPolarVelocityRadPerSec = new LoggedTunableNumber("DriveCommands/Launching/MaxPolarVelocityRadPerSec", 0.6); private static final LoggedTunableNumber driveLauncherCORMinErrorDeg = @@ -338,24 +334,13 @@ public static Command joystickDriveWhileLaunching( } public static boolean atLaunchGoal() { - return true; - // var rotation3d = - // RobotState.getInstance().getEstimatedRotation3dAtTimestamp(Timer.getTimestamp()); - // boolean inPitchAndRollTolerance = - // rotation3d.isEmpty() - // || (Math.abs(rotation3d.get().getX()) - // <= Units.degreesToRadians(driveRollLaunchToleranceDeg.get()) - // && Math.abs(rotation3d.get().getY()) - // <= Units.degreesToRadians(drivePitchLaunchToleranceDeg.get())); - - // return DriverStation.isEnabled() - // && Math.abs( - // RobotState.getInstance() - // .getRotation() - // .minus(LaunchCalculator.getInstance().getParameters().driveAngle()) - // .getRadians()) - // <= Units.degreesToRadians(driveYawLaunchToleranceDeg.get()) - // && inPitchAndRollTolerance; + return DriverStation.isEnabled() + && Math.abs( + RobotState.getInstance() + .getRotation() + .minus(LaunchCalculator.getInstance().getParameters().driveAngle()) + .getRadians()) + <= Units.degreesToRadians(driveYawLaunchToleranceDeg.get()); } /** diff --git a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java index 8b50f1d..547327b 100644 --- a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java @@ -23,7 +23,6 @@ import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; import org.team5924.frc2026.subsystems.turret.Turret; -import org.team5924.frc2026.subsystems.turret.Turret.TurretState; import org.team5924.frc2026.util.LaunchCalculator; import org.team5924.frc2026.util.LaunchCalculator.LaunchingParameters; @@ -50,22 +49,18 @@ public static Command runTrackTargetCommand( LaunchingParameters launchParams = LaunchCalculator.getInstance().getParameters(isLeft); - turret.setAutoInput(launchParams.turretRadians()); shooterHood.setAutoInput(launchParams.hoodAngle()); flywheel.setAutoInput(launchParams.flywheelSpeed()); - turret.setGoalState(TurretState.AUTO); shooterHood.setGoalState(ShooterHoodState.AUTO); flywheel.setGoalState(FlywheelState.AUTO); LaunchCalculator.getInstance().clearLaunchingParameters(); }, - turret, shooterHood, flywheel) .finallyDo( () -> { - turret.setGoalState(TurretState.OFF); shooterHood.setGoalState(ShooterHoodState.OFF); flywheel.setGoalState(FlywheelState.OFF); }); diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java index 55b1ac2..b481db3 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java @@ -16,6 +16,7 @@ package org.team5924.frc2026.subsystems.flywheel; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -68,7 +69,11 @@ public enum FlywheelState { @Getter private FlywheelState goalState = FlywheelState.OFF; private boolean isAtSetpoint = false; - @Setter private double autoInput = 0.0; + private double autoInput = 0.0; + + public void setAutoInput(double inputRads) { + autoInput = MathUtil.clamp(inputRads, 0.0, 100.0); + } public Flywheel(FlywheelIO io, boolean isLeft) { side = isLeft ? "Left" : "Right"; diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java index 88011da..eed672d 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java @@ -16,6 +16,7 @@ package org.team5924.frc2026.subsystems.pivots.shooterHood; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -25,6 +26,7 @@ import lombok.Setter; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; +import org.team5924.frc2026.Constants.GeneralShooterHood; import org.team5924.frc2026.FieldState; import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.EqualsUtil; @@ -73,7 +75,13 @@ public enum ShooterHoodState { private double timeSinceLastStateChange = 0.0; @Setter private double input; - @Setter private double autoInput = 0.0; + private double autoInput = 0.0; + + public void setAutoInput(double inputRads) { + autoInput = + MathUtil.clamp( + inputRads, GeneralShooterHood.MIN_POSITION_RADS, GeneralShooterHood.MAX_POSITION_RADS); + } public ShooterHood(ShooterHoodIO io, boolean isLeft) { side = isLeft ? "Left" : "Right"; diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java index 6554f00..602f490 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java @@ -30,7 +30,7 @@ public class Indexer extends GenericRoller { @Getter public enum IndexerState implements VoltageState { OFF(() -> 0.0), - INDEXING(new LoggedTunableNumber("Indexer/IndexingVoltage", 4.0)); + INDEXING(new LoggedTunableNumber("Indexer/IndexingVoltage", 8.0)); private final DoubleSupplier voltageSupplier; } From f1b6ee542f4697ab89c34c73fa5464393bdbb802 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 21 Mar 2026 16:39:48 -0700 Subject: [PATCH 05/24] pre launch calc tuning --- .../java/org/team5924/frc2026/Constants.java | 10 +-- src/main/java/org/team5924/frc2026/Robot.java | 16 ++-- .../org/team5924/frc2026/RobotContainer.java | 72 ++++++++++++---- .../java/org/team5924/frc2026/RobotState.java | 6 +- .../frc2026/commands/AutoBuilder.java | 6 +- .../frc2026/commands/drive/DriveCommands.java | 4 +- .../frc2026/commands/drive/DriveToPose.java | 2 +- .../commands/shooter/AutoScoreCommands.java | 12 ++- .../frc2026/subsystems/flywheel/Flywheel.java | 6 +- .../intakePivot/IntakePivotIOTalonFX.java | 8 +- .../pivots/shooterHood/ShooterHood.java | 7 +- .../subsystems/rollers/hopper/Hopper.java | 2 +- .../subsystems/vision/VisionConstants.java | 10 ++- .../frc2026/subsystems/vision/VisionIO.java | 6 ++ .../vision/VisionIOPhotonVision.java | 86 ++++++++++++++++++- .../frc2026/util/LaunchCalculator.java | 50 ++++++----- 16 files changed, 223 insertions(+), 80 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 3ebb84a..4761dd4 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -155,8 +155,8 @@ public final class IntakePivot { new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - .withSupplyCurrentLimit(120) - .withStatorCurrentLimit(120) + .withSupplyCurrentLimit(60) + .withStatorCurrentLimit(60) .withSupplyCurrentLimitEnable(true) .withStatorCurrentLimitEnable(true)) .withMotorOutput( @@ -220,7 +220,7 @@ public final class GeneralShooterHood { public static final double CANCODER_TO_MECHANISM = CANCODER_TO_SPUR * SPUR_TO_MECHANISM; public static final double MOTOR_TO_MECHANISM = MOTOR_TO_CANCODER * CANCODER_TO_SPUR * SPUR_TO_MECHANISM; - public static final double EPSILON_RADS = Units.degreesToRadians(2.0); + public static final double EPSILON_RADS = Units.degreesToRadians(0.5); public static final double STATE_TIMEOUT = 5.0; public static final boolean ENABLE_TIMEOUT = false; @@ -421,14 +421,14 @@ public final class ShooterHoodRight { } public final class FlywheelLeaderRight { - public static final int CAN_ID = 32; + public static final int CAN_ID = 33; public static final int BEAM_BREAK_PORT = 0; // TODO: update later public static final TalonFXConfiguration CONFIG = GeneralFlywheel.GENERAL_CONFIG; } public final class FlywheelFollowerRight { - public static final int CAN_ID = 33; + public static final int CAN_ID = 32; public static final TalonFXConfiguration CONFIG = GeneralFlywheel.GENERAL_CONFIG diff --git a/src/main/java/org/team5924/frc2026/Robot.java b/src/main/java/org/team5924/frc2026/Robot.java index d5875b0..8cbd8a3 100644 --- a/src/main/java/org/team5924/frc2026/Robot.java +++ b/src/main/java/org/team5924/frc2026/Robot.java @@ -37,6 +37,7 @@ import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.team5924.frc2026.generated.TunerConstants; import org.team5924.frc2026.util.Elastic; +import org.team5924.frc2026.util.LaunchCalculator; public class Robot extends LoggedRobot { private static final double LOW_BATTERY_VOLTAGE = 11.0; @@ -125,6 +126,12 @@ public Robot() { // Reset alert timers disabledTimer.restart(); + // Low battery alert + if (DriverStation.isEnabled()) { + disabledTimer.reset(); + lowBatteryAlert.set(false); + } + // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(); @@ -147,11 +154,6 @@ public void robotPeriodic() { // Return to non-RT thread priority (do not modify the first argument) // Threads.setCurrentThreadPriority(false, 10); - // Low battery alert - if (DriverStation.isEnabled()) { - disabledTimer.reset(); - lowBatteryAlert.set(false); - } double batteryVoltage = RobotController.getBatteryVoltage(); if (batteryVoltage > 0.0 && batteryVoltage <= LOW_BATTERY_VOLTAGE @@ -208,7 +210,9 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + LaunchCalculator.getInstance().clearLaunchingParameters(); + } /** This function is called once when test mode is enabled. */ @Override diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index b3ba457..32f29b5 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -31,6 +31,7 @@ import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.team5924.frc2026.commands.drive.DriveCommands; +import org.team5924.frc2026.commands.shooter.AutoScoreCommands; import org.team5924.frc2026.generated.TunerConstants; import org.team5924.frc2026.subsystems.drive.Drive; import org.team5924.frc2026.subsystems.drive.GyroIO; @@ -69,13 +70,14 @@ import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOTalonFX; import org.team5924.frc2026.subsystems.vision.Vision; import org.team5924.frc2026.subsystems.vision.VisionConstants; +import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVision; import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVisionSim; public class RobotContainer { // Subsystems private final Drive drive; private SwerveDriveSimulation driveSimulation = null; - private Vision vision; + private final Vision vision; private final Intake intake; private final IntakePivot intakePivot; @@ -119,8 +121,24 @@ public RobotContainer() { shooterHoodLeft = new ShooterHood(new ShooterHoodIOTalonFX(true), true); flywheelLeft = new Flywheel(new FlywheelIOTalonFX(true), true); - shooterHoodRight = new ShooterHood(new ShooterHoodIOTalonFX(false), false); - flywheelRight = new Flywheel(new FlywheelIOTalonFX(false), false); + // shooterHoodRight = new ShooterHood(new ShooterHoodIOTalonFX(false), false); + // flywheelRight = new Flywheel(new FlywheelIOTalonFX(false), false); + + // -------------------------- sim drive -------------------------- + // driveSimulation = + // new SwerveDriveSimulation(Drive.mapleSimConfig, new Pose2d(3, 3, new Rotation2d())); + // SimulatedArena.getInstance().addDriveTrainSimulation(driveSimulation); + // drive = + // new Drive( + // new GyroIOSim(driveSimulation.getGyroSimulation()), + // new ModuleIOTalonFXSim(TunerConstants.FrontLeft, + // driveSimulation.getModules()[0]), + // new ModuleIOTalonFXSim(TunerConstants.FrontRight, + // driveSimulation.getModules()[1]), + // new ModuleIOTalonFXSim(TunerConstants.BackLeft, driveSimulation.getModules()[2]), + // new ModuleIOTalonFXSim(TunerConstants.BackRight, + // driveSimulation.getModules()[3]), + // driveSimulation::setSimulationWorldPose); // ---------------------------- IO ---------------------------- // drive = @@ -140,8 +158,16 @@ public RobotContainer() { // shooterHoodLeft = new ShooterHood(new ShooterHoodIO() {}, true); // flywheelLeft = new Flywheel(new FlywheelIO() {}, true); - // shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); - // flywheelRight = new Flywheel(new FlywheelIO() {}, false); + shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); + flywheelRight = new Flywheel(new FlywheelIO() {}, false); + + vision = + new Vision( + drive::addVisionMeasurement, + new VisionIOPhotonVision( + VisionConstants.FRONT_LEFT_NAME, VisionConstants.FRONT_LEFT_TRANSFORM), + new VisionIOPhotonVision( + VisionConstants.FRONT_RIGHT_NAME, VisionConstants.FRONT_RIGHT_TRANSFORM)); break; case SIM: @@ -308,8 +334,8 @@ private void configureButtonBindings() { ? () -> drive.setPose( driveSimulation - .getSimulatedDriveTrainPose()) // reset odometry to actual robot pose during - // simulation + .getSimulatedDriveTrainPose()) // reset odometry to actual robot pose + // during simulation : () -> drive.setPose( new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); // zero gyro @@ -344,18 +370,29 @@ private void configureButtonBindings() { intakePivot, indexer)); + // driveController + // .leftBumper() + // .onTrue( + // Commands.runOnce( + // () -> { + // flywheelLeft.setGoalState(FlywheelState.FAST_LAUNCH); + // flywheelRight.setGoalState(FlywheelState.FAST_LAUNCH); + // }, + // flywheelLeft, + // flywheelRight) + // .andThen(Commands.waitSeconds(0.5)) + // .andThen( + // Commands.runOnce( + // () -> { + // indexer.setGoalState(IndexerState.INDEXING); + // }, + // indexer))); + driveController .leftBumper() .onTrue( - Commands.runOnce( - () -> { - flywheelLeft.setGoalState(FlywheelState.FAST_LAUNCH); - flywheelRight.setGoalState(FlywheelState.FAST_LAUNCH); - }, - flywheelLeft, - flywheelRight) - .andThen(Commands.waitSeconds(1.5)) - .andThen( + AutoScoreCommands.runTrackTargetCommand(shooterHoodLeft, flywheelLeft, true) + .alongWith( Commands.runOnce( () -> { indexer.setGoalState(IndexerState.INDEXING); @@ -374,6 +411,8 @@ private void configureButtonBindings() { flywheelLeft, flywheelRight, indexer)); + + // // driveController // // .rightTrigger() @@ -456,7 +495,6 @@ private void configureButtonBindings() { // // turretRight.setDefaultCommand(getAutonomousCommand()); // todo // // flywheelLeft.setDefaultCommand(getAutonomousCommand()); // todo // // flywheelRight.setDefaultCommand(getAutonomousCommand()); // todo - } /** diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index 0ceb4d9..c9a8f6f 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -21,7 +21,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import lombok.Getter; import lombok.Setter; -import org.littletonrobotics.junction.AutoLogOutput; import org.team5924.frc2026.subsystems.flywheel.Flywheel.FlywheelState; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivot.IntakePivotState; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; @@ -44,7 +43,7 @@ public static RobotState getInstance() { /** Get the rotation of the estimated pose. */ public Rotation2d getRotation() { - return estimatedPose.getRotation(); + return odometryPose.getRotation(); } public ChassisSpeeds getFieldVelocity() { @@ -61,14 +60,11 @@ public ChassisSpeeds getFieldSetpointVelocity() { // @AutoLogOutput(key = "RobotState/OdometryPose") @Getter @Setter private Pose2d odometryPose = new Pose2d(); - @Getter @Setter @AutoLogOutput private Pose2d estimatedPose = Pose2d.kZero; - public void resetPose(Pose2d pose) { // Gyro offset is the rotation that maps the old gyro rotation (estimated - offset) to the new // frame of rotation gyroOffset = pose.getRotation().minus(odometryPose.getRotation().minus(gyroOffset)); odometryPose = pose; - estimatedPose = pose; } @Getter @Setter private Rotation2d yawPosition = new Rotation2d(); diff --git a/src/main/java/org/team5924/frc2026/commands/AutoBuilder.java b/src/main/java/org/team5924/frc2026/commands/AutoBuilder.java index 89481dd..275bd6e 100644 --- a/src/main/java/org/team5924/frc2026/commands/AutoBuilder.java +++ b/src/main/java/org/team5924/frc2026/commands/AutoBuilder.java @@ -42,13 +42,13 @@ public Command basicDriveAuto() { .resetPose( AllianceFlipUtil.apply( new Pose2d( - RobotState.getInstance().getEstimatedPose().getTranslation(), + RobotState.getInstance().getOdometryPose().getTranslation(), Rotation2d.kPi)))) .andThen( new DriveToPose( drive, - () -> RobotState.getInstance().getEstimatedPose(), - () -> RobotState.getInstance().getEstimatedPose(), + () -> RobotState.getInstance().getOdometryPose(), + () -> RobotState.getInstance().getOdometryPose(), () -> new Translation2d((AllianceFlipUtil.shouldFlip() ? -1.0 : 1.0) * -1.0, 0.0)) .withTimeout(0.6)); diff --git a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java index e36005f..fbe1c0d 100644 --- a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java @@ -230,7 +230,7 @@ public static Command joystickDriveWhileLaunching( double robotAngle = Math.abs( AllianceFlipUtil.apply(FieldConstants.Hub.topCenterPoint.toTranslation2d()) - .minus(RobotState.getInstance().getEstimatedPose().getTranslation()) + .minus(RobotState.getInstance().getOdometryPose().getTranslation()) .getAngle() .minus(fieldRelativeLinearVelocity.getAngle()) .getRadians()); @@ -311,7 +311,7 @@ public static Command joystickDriveWhileLaunching( Logger.recordOutput( "DriveCommands/Launching/SetpointPose", new Pose2d( - RobotState.getInstance().getEstimatedPose().getTranslation(), + RobotState.getInstance().getOdometryPose().getTranslation(), parameters.driveAngle())); Logger.recordOutput("DriveCommands/Launching/AtGoalTolerance", atLaunchGoal()); Logger.recordOutput( diff --git a/src/main/java/org/team5924/frc2026/commands/drive/DriveToPose.java b/src/main/java/org/team5924/frc2026/commands/drive/DriveToPose.java index 0e956d8..09fe611 100644 --- a/src/main/java/org/team5924/frc2026/commands/drive/DriveToPose.java +++ b/src/main/java/org/team5924/frc2026/commands/drive/DriveToPose.java @@ -31,7 +31,7 @@ public class DriveToPose extends Command { private final Drive drive; private final Supplier target; - private Supplier robot = RobotState.getInstance()::getEstimatedPose; + private Supplier robot = RobotState.getInstance()::getOdometryPose; private Supplier linearFF = () -> Translation2d.kZero; private LoggedTunableNumber omegaFF = new LoggedTunableNumber("DriveToPose/omegaFF", 0.0); diff --git a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java index 547327b..392e0ba 100644 --- a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java @@ -22,9 +22,7 @@ import org.team5924.frc2026.subsystems.flywheel.Flywheel.FlywheelState; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; -import org.team5924.frc2026.subsystems.turret.Turret; import org.team5924.frc2026.util.LaunchCalculator; -import org.team5924.frc2026.util.LaunchCalculator.LaunchingParameters; public class AutoScoreCommands { // TODO Make and auto score program @@ -43,14 +41,14 @@ public class AutoScoreCommands { private AutoScoreCommands() {} public static Command runTrackTargetCommand( - Turret turret, ShooterHood shooterHood, Flywheel flywheel, boolean isLeft) { + ShooterHood shooterHood, Flywheel flywheel, boolean isLeft) { return Commands.run( () -> { - LaunchingParameters launchParams = - LaunchCalculator.getInstance().getParameters(isLeft); + // LaunchingParameters launchParams = + // LaunchCalculator.getInstance().getParameters(isLeft); - shooterHood.setAutoInput(launchParams.hoodAngle()); - flywheel.setAutoInput(launchParams.flywheelSpeed()); + // shooterHood.setAutoInput(launchParams.hoodAngle()); + // flywheel.setAutoInput(launchParams.flywheelSpeed()); shooterHood.setGoalState(ShooterHoodState.AUTO); flywheel.setGoalState(FlywheelState.AUTO); diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java index b481db3..ccf3ebb 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java @@ -28,6 +28,7 @@ import org.team5924.frc2026.Constants; import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.EqualsUtil; +import org.team5924.frc2026.util.LaunchCalculator; import org.team5924.frc2026.util.LoggedTunableNumber; public class Flywheel extends SubsystemBase { @@ -164,7 +165,10 @@ private void handleCurrentState() { case MANUAL -> handleManualState(); case OFF -> stop(); case B4, B6, B8, B12 -> runVolts(getTargetVelocityRotationsPerSec()); - case AUTO -> setVelocity(autoInput); + case AUTO -> { + autoInput = LaunchCalculator.getInstance().getParameters(isLeft).flywheelSpeed(); + setVelocity(autoInput); + } default -> setVelocity(getTargetVelocityRotationsPerSec()); } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java index 66dda21..4da9666 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java @@ -59,16 +59,16 @@ public class IntakePivotIOTalonFX implements IntakePivotIO { /* Gains */ private final LoggedTunableNumber kP = new LoggedTunableNumber("IntakePivot/kP", 40.0); private final LoggedTunableNumber kI = new LoggedTunableNumber("IntakePivot/kI", 0.0); - private final LoggedTunableNumber kD = new LoggedTunableNumber("IntakePivot/kD", 0.5); - private final LoggedTunableNumber kS = new LoggedTunableNumber("IntakePivot/kS", .2); + private final LoggedTunableNumber kD = new LoggedTunableNumber("IntakePivot/kD", 0.0); + private final LoggedTunableNumber kS = new LoggedTunableNumber("IntakePivot/kS", 0.2); private final LoggedTunableNumber kV = new LoggedTunableNumber("IntakePivot/kV", 0.0); private final LoggedTunableNumber kG = new LoggedTunableNumber("IntakePivot/kG", 2.8); private final LoggedTunableNumber kA = new LoggedTunableNumber("IntakePivot/kA", 0.0); private final LoggedTunableNumber motionCruiseVelocity = - new LoggedTunableNumber("IntakePivot/MotionCruiseVelocity", 10.0); + new LoggedTunableNumber("IntakePivot/MotionCruiseVelocity", 100.0); private final LoggedTunableNumber motionAcceleration = - new LoggedTunableNumber("IntakePivot/MotionAcceleration", 100.0); + new LoggedTunableNumber("IntakePivot/MotionAcceleration", 1000.0); private final LoggedTunableNumber motionJerk = new LoggedTunableNumber("IntakePivot/MotionJerk", 0.0); diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java index eed672d..814b0b0 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java @@ -30,6 +30,7 @@ import org.team5924.frc2026.FieldState; import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.EqualsUtil; +import org.team5924.frc2026.util.LaunchCalculator; import org.team5924.frc2026.util.LoggedTunableNumber; public class ShooterHood extends SubsystemBase { @@ -142,7 +143,7 @@ public void setGoalState(ShooterHoodState goalState) { DriverStation.reportError( side + " Shooter Hood: MOVING is an invalid goal state; it is a transition state!!", null); - case AUTO -> setRespectiveShooterHoodState(ShooterHoodState.MOVING); + case AUTO -> setRespectiveShooterHoodState(ShooterHoodState.AUTO); case OFF -> setRespectiveShooterHoodState(ShooterHoodState.OFF); default -> setRespectiveShooterHoodState(ShooterHoodState.MOVING); } @@ -178,6 +179,10 @@ private void handleCurrentState() { } case MANUAL -> handleManualState(); case OFF -> stop(); + case AUTO -> { + autoInput = LaunchCalculator.getInstance().getParameters(isLeft).hoodAngle(); + if (!isAtSetpoint) setPosition(autoInput); + } default -> { if (!isAtSetpoint) setPosition(getTargetRads()); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java index 9227bfa..040be11 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java @@ -30,7 +30,7 @@ public class Hopper extends GenericRoller { @RequiredArgsConstructor @Getter public enum HopperState implements VoltageState { - ON(new LoggedTunableNumber("Hopper/OnVoltage", 4.0)), + ON(new LoggedTunableNumber("Hopper/OnVoltage", 6.0)), SPIT(new LoggedTunableNumber("Hopper/SpitVoltage", -4.0)), OFF(() -> 0.0); diff --git a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java index 9bace1f..570c738 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java @@ -38,14 +38,20 @@ public class VisionConstants { Units.inchesToMeters(10.402), Units.inchesToMeters(12.042), Units.inchesToMeters(8.401)), - new Rotation3d(0.0, Units.degreesToRadians(55.0), Units.degreesToRadians(-15.0))); + new Rotation3d( + Units.degreesToRadians(8.0), + Units.degreesToRadians(-55.0), + Units.degreesToRadians(-15.0))); public static final Transform3d FRONT_RIGHT_TRANSFORM = new Transform3d( new Translation3d( Units.inchesToMeters(10.402), Units.inchesToMeters(-12.042), Units.inchesToMeters(8.711)), - new Rotation3d(0.0, Units.degreesToRadians(55.0), Units.degreesToRadians(15.0))); + new Rotation3d( + Units.degreesToRadians(-8.0), + Units.degreesToRadians(-55.0), + Units.degreesToRadians(15.0))); // public static final ArrayList IGNORE_IDS = // new ArrayList(List.of()); diff --git a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIO.java b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIO.java index 82089a3..93cf0e2 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIO.java @@ -17,12 +17,15 @@ package org.team5924.frc2026.subsystems.vision; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation3d; import org.littletonrobotics.junction.AutoLog; public interface VisionIO { @AutoLog public static class VisionIOInputs { public boolean connected = false; + public boolean a = true; + public TranslationRotation cameraToTarget; public PoseObservation[] poseObservations = new PoseObservation[0]; public int[] tagIds = new int[0]; } @@ -36,6 +39,9 @@ public static record PoseObservation( double averageTagDistance, PoseObservationType type) {} + public static record TranslationRotation( + Translation3d translation, double roll, double pitch, double yaw) {} + public static enum PoseObservationType { MEGATAG_1, MEGATAG_2, diff --git a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java index 2b2e6f2..3adcab6 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java @@ -17,18 +17,31 @@ package org.team5924.frc2026.subsystems.vision; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import java.util.HashSet; import java.util.LinkedList; import java.util.List; import java.util.Set; +import lombok.Setter; import org.photonvision.PhotonCamera; import org.team5924.frc2026.Constants; +import org.team5924.frc2026.util.LoggedTunableNumber; /** IO implementation for real PhotonVision hardware. */ public class VisionIOPhotonVision implements VisionIO { protected final PhotonCamera camera; - protected final Transform3d robotToCamera; + @Setter protected Transform3d robotToCamera; + + private final LoggedTunableNumber rollDegrees; + private final LoggedTunableNumber pitchDegrees; + private final LoggedTunableNumber yawDegrees; + + private final LoggedTunableNumber xInches; + private final LoggedTunableNumber yInches; + private final LoggedTunableNumber zInches; /** * Creates a new VisionIOPhotonVision. @@ -38,11 +51,71 @@ public class VisionIOPhotonVision implements VisionIO { */ public VisionIOPhotonVision(String name, Transform3d robotToCamera) { camera = new PhotonCamera(name); - this.robotToCamera = robotToCamera; + // this.robotToCamera = robotToCamera; + + rollDegrees = + new LoggedTunableNumber( + "Vision/Camera" + camera.getName() + "/rotation roll degrees", + Units.radiansToDegrees(robotToCamera.getRotation().getX())); + pitchDegrees = + new LoggedTunableNumber( + "Vision/Camera" + camera.getName() + "/rotation pitch degrees", + Units.radiansToDegrees(robotToCamera.getRotation().getY())); + yawDegrees = + new LoggedTunableNumber( + "Vision/Camera" + camera.getName() + "/rotation yaw degrees", + Units.radiansToDegrees(robotToCamera.getRotation().getZ())); + + xInches = + new LoggedTunableNumber( + "Vision/Camera" + camera.getName() + "/position x inches", + Units.metersToInches(robotToCamera.getX())); + yInches = + new LoggedTunableNumber( + "Vision/Camera" + camera.getName() + "/position y inches", + Units.metersToInches(robotToCamera.getY())); + zInches = + new LoggedTunableNumber( + "Vision/Camera" + camera.getName() + "/position z inches", + Units.metersToInches(robotToCamera.getZ())); + + this.robotToCamera = + new Transform3d( + new Translation3d( + Units.inchesToMeters(xInches.getAsDouble()), + Units.inchesToMeters(yInches.getAsDouble()), + Units.inchesToMeters(zInches.getAsDouble())), + new Rotation3d( + Units.degreesToRadians(rollDegrees.getAsDouble()), + Units.degreesToRadians(pitchDegrees.getAsDouble()), + Units.degreesToRadians(yawDegrees.getAsDouble()))); + } + + private void updateLoggedTunableNumbers() { + LoggedTunableNumber.ifChanged( + hashCode(), + () -> + robotToCamera = + new Transform3d( + new Translation3d( + Units.inchesToMeters(xInches.getAsDouble()), + Units.inchesToMeters(yInches.getAsDouble()), + Units.inchesToMeters(zInches.getAsDouble())), + new Rotation3d( + Units.degreesToRadians(rollDegrees.getAsDouble()), + Units.degreesToRadians(pitchDegrees.getAsDouble()), + Units.degreesToRadians(yawDegrees.getAsDouble()))), + xInches, + yInches, + zInches, + rollDegrees, + pitchDegrees, + yawDegrees); } @Override public void updateInputs(VisionIOInputs inputs) { + updateLoggedTunableNumbers(); inputs.connected = camera.isConnected(); // Read new camera observations @@ -86,6 +159,15 @@ public void updateInputs(VisionIOInputs inputs) { Transform3d fieldToTarget = new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); Transform3d cameraToTarget = target.bestCameraToTarget; + + Rotation3d cameraToTargetRotation = cameraToTarget.getRotation(); + inputs.cameraToTarget = + new TranslationRotation( + cameraToTarget.getTranslation(), + Units.radiansToDegrees(cameraToTargetRotation.getX()), + Units.radiansToDegrees(cameraToTargetRotation.getY()), + Units.radiansToDegrees(cameraToTargetRotation.getZ())); + Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); diff --git a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java index 42ad4dc..e9c03ca 100644 --- a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java +++ b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java @@ -159,18 +159,18 @@ public static record LaunchPreset( phaseDelay = 0.03; // if (true) { - hoodAngleMap.put(0.96, Rotation2d.fromDegrees(10.0)); - hoodAngleMap.put(1.16, Rotation2d.fromDegrees(12.0)); - hoodAngleMap.put(1.58, Rotation2d.fromDegrees(14.0)); - hoodAngleMap.put(2.07, Rotation2d.fromDegrees(18.5)); - hoodAngleMap.put(2.37, Rotation2d.fromDegrees(22.0)); - hoodAngleMap.put(2.47, Rotation2d.fromDegrees(23.0)); - hoodAngleMap.put(2.70, Rotation2d.fromDegrees(24.0)); - hoodAngleMap.put(2.94, Rotation2d.fromDegrees(25.0)); - hoodAngleMap.put(3.48, Rotation2d.fromDegrees(27.0)); - hoodAngleMap.put(3.92, Rotation2d.fromDegrees(32.0)); - hoodAngleMap.put(4.35, Rotation2d.fromDegrees(34.0)); - hoodAngleMap.put(4.84, Rotation2d.fromDegrees(38.0)); + hoodAngleMap.put(0.96, Rotation2d.fromDegrees(10.0 / 2.0)); + hoodAngleMap.put(1.16, Rotation2d.fromDegrees(12.0 / 2.0)); + hoodAngleMap.put(1.58, Rotation2d.fromDegrees(14.0 / 2.0)); + hoodAngleMap.put(2.07, Rotation2d.fromDegrees(18.5 / 2.0)); + hoodAngleMap.put(2.37, Rotation2d.fromDegrees(22.0 / 2.0)); + hoodAngleMap.put(2.47, Rotation2d.fromDegrees(23.0 / 2.0)); + hoodAngleMap.put(2.70, Rotation2d.fromDegrees(24.0 / 2.0)); + hoodAngleMap.put(2.94, Rotation2d.fromDegrees(25.0 / 2.0)); + hoodAngleMap.put(3.48, Rotation2d.fromDegrees(27.0 / 2.0)); + hoodAngleMap.put(3.92, Rotation2d.fromDegrees(32.0 / 2.0)); + hoodAngleMap.put(4.35, Rotation2d.fromDegrees(34.0 / 2.0)); + hoodAngleMap.put(4.84, Rotation2d.fromDegrees(38.0 / 2.0)); flywheelSpeedMap.put(0.96, 150.0); flywheelSpeedMap.put(1.16, 155.0); @@ -191,16 +191,18 @@ public static record LaunchPreset( timeOfFlightMap.put(1.88, 1.09); timeOfFlightMap.put(1.38, 0.90); - passingHoodAngleMap.put(5.46, Rotation2d.fromDegrees(38.0)); - passingHoodAngleMap.put(6.62, Rotation2d.fromDegrees(38.0)); - passingHoodAngleMap.put(7.80, Rotation2d.fromDegrees(38.0)); + { + passingHoodAngleMap.put(5.46, Rotation2d.fromDegrees(38.0)); + passingHoodAngleMap.put(6.62, Rotation2d.fromDegrees(38.0)); + passingHoodAngleMap.put(7.80, Rotation2d.fromDegrees(38.0)); - passingFlywheelSpeedMap.put(5.46, 160.0); - passingFlywheelSpeedMap.put(6.62, 180.0); - passingFlywheelSpeedMap.put(7.80, 200.0); + passingFlywheelSpeedMap.put(5.46, 160.0); + passingFlywheelSpeedMap.put(6.62, 180.0); + passingFlywheelSpeedMap.put(7.80, 200.0); - passingTimeOfFlightMap.put(passingMinDistance, 0.0); - passingTimeOfFlightMap.put(passingMaxDistance, 0.0); + passingTimeOfFlightMap.put(passingMinDistance, 0.0); + passingTimeOfFlightMap.put(passingMaxDistance, 0.0); + } // } else { // // Full field maps @@ -283,14 +285,14 @@ public LaunchingParameters getParameters(boolean isLeft) { public LaunchingParameters getParameters(Transform3d robotToLauncher) { boolean passing = - AllianceFlipUtil.applyX(RobotState.getInstance().getEstimatedPose().getX()) + AllianceFlipUtil.applyX(RobotState.getInstance().getOdometryPose().getX()) > FieldConstants.LinesVertical.hubCenter; if (latestParameters != null) { return latestParameters; } // Calculate estimated pose while accounting for phase delay - Pose2d estimatedPose = RobotState.getInstance().getEstimatedPose(); + Pose2d estimatedPose = RobotState.getInstance().getOdometryPose(); ChassisSpeeds robotRelativeVelocity = RobotState.getInstance().getRobotVelocity(); estimatedPose = estimatedPose.exp( @@ -397,6 +399,8 @@ public LaunchingParameters getParameters(Transform3d robotToLauncher) { Logger.recordOutput( "LaunchCalculator/LauncherToTargetDistance", lookaheadLauncherToTargetDistance); + Logger.recordOutput("LaunchCalculator/LaunchParams", latestParameters); + return latestParameters; } @@ -434,7 +438,7 @@ public void clearLaunchingParameters() { } public Translation2d getPassingTarget() { - double flippedY = AllianceFlipUtil.apply(RobotState.getInstance().getEstimatedPose()).getY(); + double flippedY = AllianceFlipUtil.apply(RobotState.getInstance().getOdometryPose()).getY(); boolean mirror = flippedY > FieldConstants.LinesHorizontal.center; // Check if we need to interpolate From da57b3af804bf96dd90cbe1968287f4997db7273 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 21 Mar 2026 19:02:09 -0700 Subject: [PATCH 06/24] add theoretical launch calc values, indexer wait on flywheel, works!! --- .../java/org/team5924/frc2026/Constants.java | 2 +- .../org/team5924/frc2026/RobotContainer.java | 34 ++- .../java/org/team5924/frc2026/RobotState.java | 1 + .../frc2026/subsystems/flywheel/Flywheel.java | 16 +- .../pivots/shooterHood/ShooterHood.java | 2 + .../subsystems/rollers/indexer/Indexer.java | 7 + .../frc2026/util/LaunchCalculator.java | 214 ++++++++++-------- 7 files changed, 173 insertions(+), 103 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 4761dd4..97ae98e 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -60,7 +60,7 @@ public static enum Mode { } public static final double TRACK_WIDTH_Y_METERS = Units.inchesToMeters(19.5); - public static final boolean TUNING_MODE = false; // TODO: tuning mode off + public static final boolean TUNING_MODE = true; // TODO: tuning mode off public static final boolean ALLOW_ASSERTS = false; public static final double SLOW_MODE_MULTI = 0.33; diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 32f29b5..8f1ec2e 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -388,6 +388,33 @@ private void configureButtonBindings() { // }, // indexer))); + // driveController + // .leftTrigger() + // .onTrue( + // Commands.runOnce( + // () -> { + // LaunchCalculator.getInstance().getParameters(true); + // flywheelLeft.setGoalState(FlywheelState.MANUAL); + // shooterHoodLeft.setGoalState(ShooterHoodState.MANUAL_ANGLE); + // indexer.setGoalState(IndexerState.INDEXING); + // }, + // shooterHoodLeft, + // flywheelLeft, + // indexer)); + + // driveController + // .leftTrigger() + // .onFalse( + // Commands.runOnce( + // () -> { + // flywheelLeft.setGoalState(FlywheelState.OFF); + // shooterHoodLeft.setGoalState(ShooterHoodState.OFF); + // indexer.setGoalState(IndexerState.OFF); + // }, + // shooterHoodLeft, + // flywheelLeft, + // indexer)); + driveController .leftBumper() .onTrue( @@ -398,6 +425,11 @@ private void configureButtonBindings() { indexer.setGoalState(IndexerState.INDEXING); }, indexer))); + // driveController + // .rightTrigger() + // .onTrue( + // DriveCommands.joystickDriveWhileLaunching( + // drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); driveController .leftBumper() @@ -411,8 +443,6 @@ private void configureButtonBindings() { flywheelLeft, flywheelRight, indexer)); - - // // driveController // // .rightTrigger() diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index c9a8f6f..b5337a2 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -89,6 +89,7 @@ public void resetPose(Pose2d pose) { /*### Flywheel ### */ @Getter @Setter private FlywheelState leftFlywheelState = FlywheelState.OFF; @Getter @Setter private FlywheelState rightFlywheelState = FlywheelState.OFF; + @Getter @Setter private boolean leftFlywheelAtSetpoint = false; /*### Shooter Hood ### */ @Getter @Setter private ShooterHoodState leftShooterHoodState = ShooterHoodState.OFF; diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java index ccf3ebb..6bcf31b 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java @@ -48,7 +48,7 @@ public enum FlywheelState { SLOW_LAUNCH(new LoggedTunableNumber("Flywheel/SlowLaunch", 50)), // current at which the example subsystem motor moves when controlled by the operator - MANUAL(new LoggedTunableNumber("Flywheel/OperatorCurrent", 200)), + MANUAL(new LoggedTunableNumber("Flywheel/OperatorCurrent", 135)), AUTO(() -> 0.0), SETPOINT(() -> 0.0), @@ -124,9 +124,6 @@ public void stop() { public void setGoalState(FlywheelState goalState) { if (this.goalState.equals(goalState)) return; - if (goalState.equals(FlywheelState.MANUAL) && Math.abs(input) <= Constants.JOYSTICK_DEADZONE) - return; - this.goalState = goalState; switch (goalState) { case MANUAL -> setRespectiveFlywheelState(FlywheelState.MANUAL); @@ -155,6 +152,7 @@ private double getTargetVelocityRotationsPerSec() { private void handleCurrentState() { isAtSetpoint = isAtSetpoint(); + if (isLeft) RobotState.getInstance().setLeftFlywheelAtSetpoint(isAtSetpoint); switch (getRespectiveFlywheelState()) { case MOVING -> { @@ -176,12 +174,12 @@ private void handleCurrentState() { private void handleManualState() { if (!goalState.equals(FlywheelState.MANUAL)) return; - if (Math.abs(input) <= Constants.JOYSTICK_DEADZONE) { - stop(); - return; - } + // if (Math.abs(input) <= Constants.JOYSTICK_DEADZONE) { + // stop(); + // return; + // } - setVelocity(FlywheelState.MANUAL.getVelocityRotationsPerSec().getAsDouble() * input); + setVelocity(FlywheelState.MANUAL.getVelocityRotationsPerSec().getAsDouble()); } public void updateSetpointState(double change) { diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java index 814b0b0..5f0fd28 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java @@ -53,6 +53,8 @@ public enum ShooterHoodState { NEUTRAL_SHUFFLING(() -> 0.0), OPPONENT_SHUFFLING(() -> 0.0), + MANUAL_ANGLE(new LoggedTunableNumber("ShooterHood/ManualAngle", Math.toRadians(0.0))), + MAX(new LoggedTunableNumber("ShooterHood/Max", Math.toRadians(30))), CENTER(new LoggedTunableNumber("ShooterHood/Center", Math.toRadians(15))), AUTO(() -> 0.0), diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java index 602f490..5b6aac9 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java @@ -46,6 +46,13 @@ public void setGoalState(IndexerState goalState) { RobotState.getInstance().setIndexerState(goalState); } + @Override + protected void handleCurrentState() { + if (RobotState.getInstance().isLeftFlywheelAtSetpoint()) + io.runVolts(getGoalState().getVoltageSupplier().getAsDouble()); + else io.stop(); + } + @Override public void periodic() { super.periodic(); diff --git a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java index e9c03ca..59869bb 100644 --- a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java +++ b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java @@ -102,11 +102,11 @@ public record LaunchingParameters( public static final double trenchPresetDistance = 3.03; public static final double outpostPresetDistance = 4.84; public static final double passingPresetDistance = 7.0; - public static final LaunchPreset passingPreset; - public static final LaunchPreset hubPreset; - public static final LaunchPreset towerPreset; - public static final LaunchPreset trenchPreset; - public static final LaunchPreset outpostPreset; + // public static final LaunchPreset passingPreset; + // public static final LaunchPreset hubPreset; + // public static final LaunchPreset towerPreset; + // public static final LaunchPreset trenchPreset; + // public static final LaunchPreset outpostPreset; public static final LaunchPreset hoodMinPreset = new LaunchPreset( new LoggedTunableNumber( @@ -159,50 +159,82 @@ public static record LaunchPreset( phaseDelay = 0.03; // if (true) { - hoodAngleMap.put(0.96, Rotation2d.fromDegrees(10.0 / 2.0)); - hoodAngleMap.put(1.16, Rotation2d.fromDegrees(12.0 / 2.0)); - hoodAngleMap.put(1.58, Rotation2d.fromDegrees(14.0 / 2.0)); - hoodAngleMap.put(2.07, Rotation2d.fromDegrees(18.5 / 2.0)); - hoodAngleMap.put(2.37, Rotation2d.fromDegrees(22.0 / 2.0)); - hoodAngleMap.put(2.47, Rotation2d.fromDegrees(23.0 / 2.0)); - hoodAngleMap.put(2.70, Rotation2d.fromDegrees(24.0 / 2.0)); - hoodAngleMap.put(2.94, Rotation2d.fromDegrees(25.0 / 2.0)); - hoodAngleMap.put(3.48, Rotation2d.fromDegrees(27.0 / 2.0)); - hoodAngleMap.put(3.92, Rotation2d.fromDegrees(32.0 / 2.0)); - hoodAngleMap.put(4.35, Rotation2d.fromDegrees(34.0 / 2.0)); - hoodAngleMap.put(4.84, Rotation2d.fromDegrees(38.0 / 2.0)); - - flywheelSpeedMap.put(0.96, 150.0); - flywheelSpeedMap.put(1.16, 155.0); - flywheelSpeedMap.put(1.58, 160.0); - flywheelSpeedMap.put(2.07, 165.0); - flywheelSpeedMap.put(2.37, 170.0); - flywheelSpeedMap.put(2.47, 170.0); - flywheelSpeedMap.put(2.70, 170.0); - flywheelSpeedMap.put(2.94, 175.0); - flywheelSpeedMap.put(3.48, 175.0); - flywheelSpeedMap.put(3.92, 180.0); - flywheelSpeedMap.put(4.35, 185.0); - flywheelSpeedMap.put(4.84, 190.0); - - timeOfFlightMap.put(5.68, 1.16); - timeOfFlightMap.put(4.55, 1.12); - timeOfFlightMap.put(3.15, 1.11); - timeOfFlightMap.put(1.88, 1.09); - timeOfFlightMap.put(1.38, 0.90); - - { - passingHoodAngleMap.put(5.46, Rotation2d.fromDegrees(38.0)); - passingHoodAngleMap.put(6.62, Rotation2d.fromDegrees(38.0)); - passingHoodAngleMap.put(7.80, Rotation2d.fromDegrees(38.0)); - - passingFlywheelSpeedMap.put(5.46, 160.0); - passingFlywheelSpeedMap.put(6.62, 180.0); - passingFlywheelSpeedMap.put(7.80, 200.0); - - passingTimeOfFlightMap.put(passingMinDistance, 0.0); - passingTimeOfFlightMap.put(passingMaxDistance, 0.0); - } + hoodAngleMap.put(2.388, Rotation2d.fromRadians(0.0)); + hoodAngleMap.put(2.450, Rotation2d.fromRadians(0.031)); + hoodAngleMap.put(2.570, Rotation2d.fromRadians(0.040)); + hoodAngleMap.put(2.730, Rotation2d.fromRadians(0.052)); + hoodAngleMap.put(3.050, Rotation2d.fromRadians(0.052)); + hoodAngleMap.put(3.550, Rotation2d.fromRadians(0.110)); + hoodAngleMap.put(4.000, Rotation2d.fromRadians(0.139)); + hoodAngleMap.put(4.470, Rotation2d.fromRadians(0.181)); + + flywheelSpeedMap.put(2.388, 75.0); + flywheelSpeedMap.put(2.450, 75.0); + flywheelSpeedMap.put(2.570, 77.5); + flywheelSpeedMap.put(2.730, 77.5); + flywheelSpeedMap.put(3.050, 80.0); + flywheelSpeedMap.put(3.550, 85.0); + flywheelSpeedMap.put(4.000, 87.5); + flywheelSpeedMap.put(4.470, 92.5); + + timeOfFlightMap.put(2.388, 0.814); + timeOfFlightMap.put(3.050, 0.879); + timeOfFlightMap.put(4.000, 0.935); + timeOfFlightMap.put(4.470, 0.975); + + // ------------------------ + + // minDistance = 0.9; + // maxDistance = 4.9; + // passingMinDistance = 0.0; + // passingMaxDistance = 12.0; + // phaseDelay = 0.03; + + // // if (true) { + // hoodAngleMap.put(0.96, Rotation2d.fromDegrees(10.0 / 2.0)); + // hoodAngleMap.put(1.16, Rotation2d.fromDegrees(12.0 / 2.0)); + // hoodAngleMap.put(1.58, Rotation2d.fromDegrees(14.0 / 2.0)); + // hoodAngleMap.put(2.07, Rotation2d.fromDegrees(18.5 / 2.0)); + // hoodAngleMap.put(2.37, Rotation2d.fromDegrees(22.0 / 2.0)); + // hoodAngleMap.put(2.47, Rotation2d.fromDegrees(23.0 / 2.0)); + // hoodAngleMap.put(2.70, Rotation2d.fromDegrees(24.0 / 2.0)); + // hoodAngleMap.put(2.94, Rotation2d.fromDegrees(25.0 / 2.0)); + // hoodAngleMap.put(3.48, Rotation2d.fromDegrees(27.0 / 2.0)); + // hoodAngleMap.put(3.92, Rotation2d.fromDegrees(32.0 / 2.0)); + // hoodAngleMap.put(4.35, Rotation2d.fromDegrees(34.0 / 2.0)); + // hoodAngleMap.put(4.84, Rotation2d.fromDegrees(38.0 / 2.0)); + + // flywheelSpeedMap.put(0.96, 150.0); + // flywheelSpeedMap.put(1.16, 155.0); + // flywheelSpeedMap.put(1.58, 160.0); + // flywheelSpeedMap.put(2.07, 165.0); + // flywheelSpeedMap.put(2.37, 170.0); + // flywheelSpeedMap.put(2.47, 170.0); + // flywheelSpeedMap.put(2.70, 170.0); + // flywheelSpeedMap.put(2.94, 175.0); + // flywheelSpeedMap.put(3.48, 175.0); + // flywheelSpeedMap.put(3.92, 180.0); + // flywheelSpeedMap.put(4.35, 185.0); + // flywheelSpeedMap.put(4.84, 190.0); + + // timeOfFlightMap.put(5.68, 1.16); + // timeOfFlightMap.put(4.55, 1.12); + // timeOfFlightMap.put(3.15, 1.11); + // timeOfFlightMap.put(1.88, 1.09); + // timeOfFlightMap.put(1.38, 0.90); + + // { + // passingHoodAngleMap.put(5.46, Rotation2d.fromDegrees(38.0)); + // passingHoodAngleMap.put(6.62, Rotation2d.fromDegrees(38.0)); + // passingHoodAngleMap.put(7.80, Rotation2d.fromDegrees(38.0)); + + // passingFlywheelSpeedMap.put(5.46, 160.0); + // passingFlywheelSpeedMap.put(6.62, 180.0); + // passingFlywheelSpeedMap.put(7.80, 200.0); + + // passingTimeOfFlightMap.put(passingMinDistance, 0.0); + // passingTimeOfFlightMap.put(passingMaxDistance, 0.0); + // } // } else { // // Full field maps @@ -225,46 +257,46 @@ public static record LaunchPreset( // passingTimeOfFlightMap.put(passingMaxDistance, 0.0); // } - passingPreset = - new LaunchPreset( - new LoggedTunableNumber( - "LaunchCalculator/Presets/Passing/HoodAngle", - passingHoodAngleMap.get(passingPresetDistance).getDegrees()), - new LoggedTunableNumber( - "LaunchCalculator/Presets/Passing/FlywheelSpeed", - passingFlywheelSpeedMap.get(passingPresetDistance))); - hubPreset = - new LaunchPreset( - new LoggedTunableNumber( - "LaunchCalculator/Presets/Hub/HoodAngle", - hoodAngleMap.get(hubPresetDistance).getDegrees()), - new LoggedTunableNumber( - "LaunchCalculator/Presets/Hub/FlywheelSpeed", - flywheelSpeedMap.get(hubPresetDistance))); - towerPreset = - new LaunchPreset( - new LoggedTunableNumber( - "LaunchCalculator/Presets/Tower/HoodAngle", - hoodAngleMap.get(towerPresetDistance).getDegrees()), - new LoggedTunableNumber( - "LaunchCalculator/Presets/Tower/FlywheelSpeed", - flywheelSpeedMap.get(towerPresetDistance))); - trenchPreset = - new LaunchPreset( - new LoggedTunableNumber( - "LaunchCalculator/Presets/Trench/HoodAngle", - hoodAngleMap.get(trenchPresetDistance).getDegrees()), - new LoggedTunableNumber( - "LaunchCalculator/Presets/Trench/FlywheelSpeed", - flywheelSpeedMap.get(trenchPresetDistance))); - outpostPreset = - new LaunchPreset( - new LoggedTunableNumber( - "LaunchCalculator/Presets/Outpost/HoodAngle", - hoodAngleMap.get(outpostPresetDistance).getDegrees()), - new LoggedTunableNumber( - "LaunchCalculator/Presets/Outpost/FlywheelSpeed", - flywheelSpeedMap.get(outpostPresetDistance))); + // passingPreset = + // new LaunchPreset( + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Passing/HoodAngle", + // passingHoodAngleMap.get(passingPresetDistance).getDegrees()), + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Passing/FlywheelSpeed", + // passingFlywheelSpeedMap.get(passingPresetDistance))); + // hubPreset = + // new LaunchPreset( + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Hub/HoodAngle", + // hoodAngleMap.get(hubPresetDistance).getDegrees()), + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Hub/FlywheelSpeed", + // flywheelSpeedMap.get(hubPresetDistance))); + // towerPreset = + // new LaunchPreset( + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Tower/HoodAngle", + // hoodAngleMap.get(towerPresetDistance).getDegrees()), + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Tower/FlywheelSpeed", + // flywheelSpeedMap.get(towerPresetDistance))); + // trenchPreset = + // new LaunchPreset( + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Trench/HoodAngle", + // hoodAngleMap.get(trenchPresetDistance).getDegrees()), + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Trench/FlywheelSpeed", + // flywheelSpeedMap.get(trenchPresetDistance))); + // outpostPreset = + // new LaunchPreset( + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Outpost/HoodAngle", + // hoodAngleMap.get(outpostPresetDistance).getDegrees()), + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Outpost/FlywheelSpeed", + // flywheelSpeedMap.get(outpostPresetDistance))); } public static double getMinTimeOfFlight() { @@ -276,7 +308,7 @@ public static double getMaxTimeOfFlight() { } public LaunchingParameters getParameters() { - return getParameters(robotToLauncherCenter); + return getParameters(true); } public LaunchingParameters getParameters(boolean isLeft) { @@ -310,7 +342,7 @@ public LaunchingParameters getParameters(Transform3d robotToLauncher) { double launcherToTargetDistance = target.getDistance(launcherPosition.getTranslation()); // Calculate field relative launcher velocity - var robotVelocity = RobotState.getInstance().getFieldSetpointVelocity(); + var robotVelocity = new ChassisSpeeds(); // RobotState.getInstance().getFieldSetpointVelocity(); var robotAngle = RobotState.getInstance().getRotation(); ChassisSpeeds launcherVelocity = DriverStation.isAutonomous() From 4a5e04f890467785c3b0114a2ca5701019ccc796 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 21 Mar 2026 20:36:31 -0700 Subject: [PATCH 07/24] tuning mode off --- src/main/java/org/team5924/frc2026/Constants.java | 2 +- src/main/java/org/team5924/frc2026/RobotContainer.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 97ae98e..4761dd4 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -60,7 +60,7 @@ public static enum Mode { } public static final double TRACK_WIDTH_Y_METERS = Units.inchesToMeters(19.5); - public static final boolean TUNING_MODE = true; // TODO: tuning mode off + public static final boolean TUNING_MODE = false; // TODO: tuning mode off public static final boolean ALLOW_ASSERTS = false; public static final double SLOW_MODE_MULTI = 0.33; diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 8f1ec2e..5f504f3 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -427,7 +427,7 @@ private void configureButtonBindings() { indexer))); // driveController // .rightTrigger() - // .onTrue( + // .whileTrue( // DriveCommands.joystickDriveWhileLaunching( // drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); From 7ed3a5e4b0a2ab3d6733d44f94d8b567d9da345e Mon Sep 17 00:00:00 2001 From: James Guleno Date: Mon, 23 Mar 2026 20:47:26 -0700 Subject: [PATCH 08/24] deprecate left/right side of subsystems in implementation and constants, refine constants, relocate subsystem current/goal states into respective subsystem files, add booleans in RobotContainer.java to switch between real/io implementations --- .../java/org/team5924/frc2026/Constants.java | 220 +------- .../org/team5924/frc2026/RobotContainer.java | 471 ++++-------------- .../java/org/team5924/frc2026/RobotState.java | 31 -- .../commands/shooter/AutoScoreCommands.java | 10 +- .../shooter/ManualShooterCommands.java | 11 - .../frc2026/subsystems/flywheel/Flywheel.java | 57 +-- .../subsystems/flywheel/FlywheelIOSim.java | 4 +- .../flywheel/FlywheelIOTalonFX.java | 193 +++---- .../pivots/intakePivot/IntakePivot.java | 15 +- .../pivots/shooterHood/ShooterHood.java | 68 +-- .../pivots/shooterHood/ShooterHoodIOSim.java | 10 +- .../shooterHood/ShooterHoodIOTalonFX.java | 193 +++---- .../subsystems/rollers/hopper/Hopper.java | 4 +- .../subsystems/rollers/indexer/Indexer.java | 4 +- .../subsystems/rollers/intake/Intake.java | 4 +- .../frc2026/subsystems/turret/Turret.java | 213 -------- .../frc2026/subsystems/turret/TurretIO.java | 79 --- .../subsystems/turret/TurretIOSim.java | 79 --- .../subsystems/turret/TurretIOTalonFX.java | 386 -------------- .../subsystems/vision/VisionConstants.java | 12 +- .../frc2026/util/LaunchCalculator.java | 32 +- .../frc2026/util/LauncherConstants.java | 10 +- 22 files changed, 326 insertions(+), 1780 deletions(-) delete mode 100644 src/main/java/org/team5924/frc2026/subsystems/turret/Turret.java delete mode 100644 src/main/java/org/team5924/frc2026/subsystems/turret/TurretIO.java delete mode 100644 src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOSim.java delete mode 100644 src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOTalonFX.java diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index c73c683..a62cd52 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -34,8 +34,6 @@ import edu.wpi.first.math.util.Units; import com.ctre.phoenix6.signals.SensorDirectionValue; -import edu.wpi.first.math.geometry.Translation3d; - import edu.wpi.first.wpilibj.RobotBase; /** @@ -207,9 +205,14 @@ public final class Indexer { /* General Subsystems */ - public final class GeneralShooterHood { + public final class ShooterHood { public static final String BUS = "rio"; public static final double SIM_MOI = 0.001; + public static final int CAN_ID = 34; + + /* CANCoder */ + public static final int CANCODER_ID = 36; + public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; // spur = hood driving gear, mechanism = shooter hood gear public static final double MOTOR_TO_CANCODER = (40.0 / 12.0) * (24.0 / 17.0); @@ -252,25 +255,34 @@ public final class GeneralShooterHood { .withForwardSoftLimitEnable(false) .withReverseSoftLimitEnable(false); - public static final FeedbackConfigs GENERAL_FEEDBACK_CONFIGS = + public static final FeedbackConfigs FEEDBACK_CONFIGS = new FeedbackConfigs() .withSensorToMechanismRatio(CANCODER_TO_MECHANISM) .withRotorToSensorRatio(MOTOR_TO_CANCODER) - .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder); + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) + .withFeedbackRemoteSensorID(CANCODER_ID) + .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - public static final MagnetSensorConfigs GENERAL_CANCODER_CONFIGS = + public static final MagnetSensorConfigs CANCODER_CONFIGS = new MagnetSensorConfigs() .withAbsoluteSensorDiscontinuityPoint(1.0) - .withSensorDirection(SensorDirectionValue.Clockwise_Positive); + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); } - public final class GeneralFlywheel { + public final class Flywheel { + public static final int CAN_ID = 30; + public static final int BEAM_BREAK_ID = 0; // TODO: update later + + public static final int FOLLOWER_CAN_ID = 31; + public static final double FOLLOWER_SIM_MOI = 0.001; + public static final double EPSILON_VELOCITY = 2.5; public static final double MOTOR_TO_MECHANISM = 15.0 / 26.0; public static final String BUS = "rio"; public static final double SIM_MOI = 0.001; - public static final TalonFXConfiguration GENERAL_CONFIG = + public static final TalonFXConfiguration CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() @@ -280,6 +292,12 @@ public final class GeneralFlywheel { new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast)); + + public static final TalonFXConfiguration FOLLOWER_CONFIG = + CONFIG + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive)); public static final FeedbackConfigs FEEDBACK_CONFIGS = new FeedbackConfigs() @@ -287,188 +305,4 @@ public final class GeneralFlywheel { .withSensorToMechanismRatio(MOTOR_TO_MECHANISM) .withRotorToSensorRatio(1.0); } - - public final class GeneralTurret { - public static final String BUS = "rio"; - - public static final double SIM_MOI = 0.001; - - public static final double MOTOR_TO_CANCODER = (20.0 / 12.0); - public static final double CANCODER_TO_MECHANISM = (135.0 / 20.0); - public static final double MOTOR_TO_MECHANISM = MOTOR_TO_CANCODER * CANCODER_TO_MECHANISM; - - public static final double EPSILON_RADS = Units.degreesToRadians(10.0); - public static final double STATE_TIMEOUT = 1.0; - - /* Configs */ - public static final TalonFXConfiguration GENERAL_CONFIG = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withSupplyCurrentLimit(60) - .withStatorCurrentLimit(10) - .withStatorCurrentLimitEnable(true)) - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake)); - - public static final SoftwareLimitSwitchConfigs GENERAL_SOFTWARE_LIMIT_CONFIGS = - new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(false) - .withReverseSoftLimitEnable(false); - - public static final FeedbackConfigs GENERAL_FEEDBACK_CONFIGS = - new FeedbackConfigs() - .withSensorToMechanismRatio(CANCODER_TO_MECHANISM) - .withRotorToSensorRatio(MOTOR_TO_CANCODER) - .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder); - - public static final MagnetSensorConfigs GENERAL_CANCODER_CONFIGS = - new MagnetSensorConfigs() - .withAbsoluteSensorDiscontinuityPoint(1) - .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive); - } - - /* Left */ - public final class ShooterHoodLeft { - public static final int CAN_ID = 34; - - /* CANCoder */ - public static final int CANCODER_ID = 36; - public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - - public static final FeedbackConfigs FEEDBACK_CONFIGS = - GeneralShooterHood.GENERAL_FEEDBACK_CONFIGS - .withFeedbackRemoteSensorID(CANCODER_ID) - .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - - public static final MagnetSensorConfigs CANCODER_CONFIGS = - GeneralShooterHood.GENERAL_CANCODER_CONFIGS - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); - } - - public final class FlywheelLeaderLeft { - public static final int CAN_ID = 30; - public static final int BEAM_BREAK_PORT = 0; // TODO: update later - - public static final TalonFXConfiguration CONFIG = GeneralFlywheel.GENERAL_CONFIG; - } - - public final class FlywheelFollowerLeft { - public static final int CAN_ID = 31; - public static final double SIM_MOI = 0.001; - public static final int BEAM_BREAK_ID = 0; - - public static final TalonFXConfiguration CONFIG = - GeneralFlywheel.GENERAL_CONFIG - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive)); - } - - public final class TurretLeft { - public static final int CAN_ID = 20; - - // +x -> forward; +y -> left - public static final Translation3d ROBOT_TO_TURRET = - new Translation3d( - Units.inchesToMeters(4.5), - Units.inchesToMeters(7.505), - Units.inchesToMeters(15.340)); - - /* CANCoder */ - public static final int CANCODER_ID = 22; - public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - - public static final double MIN_POSITION_ROTATIONS = 0 - Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - public static final double MAX_POSITION_ROTATIONS = 150.0 / 360.0 + Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - - public static final double MIN_POSITION_RADS = Units.rotationsToRadians(MIN_POSITION_ROTATIONS); - public static final double MAX_POSITION_RADS = Units.rotationsToRadians(MAX_POSITION_ROTATIONS); - - public static final SoftwareLimitSwitchConfigs SOFTWARE_LIMIT_CONFIGS = - GeneralTurret.GENERAL_SOFTWARE_LIMIT_CONFIGS - .withReverseSoftLimitThreshold(MIN_POSITION_ROTATIONS) - .withForwardSoftLimitThreshold(MAX_POSITION_ROTATIONS); - - public static final FeedbackConfigs FEEDBACK_CONFIGS = - GeneralTurret.GENERAL_FEEDBACK_CONFIGS - .withFeedbackRemoteSensorID(CANCODER_ID) - .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - - public static final MagnetSensorConfigs CANCODER_CONFIGS = - GeneralTurret.GENERAL_CANCODER_CONFIGS - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); - } - - /* Right */ - public final class ShooterHoodRight { - public static final int CAN_ID = 35; - - /* CANCoder */ - public static final int CANCODER_ID = 37; - public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - - public static final FeedbackConfigs FEEDBACK_CONFIGS = - GeneralShooterHood.GENERAL_FEEDBACK_CONFIGS - .withFeedbackRemoteSensorID(CANCODER_ID) - .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - - public static final MagnetSensorConfigs CANCODER_CONFIGS = - GeneralShooterHood.GENERAL_CANCODER_CONFIGS - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); - } - - public final class FlywheelLeaderRight { - public static final int CAN_ID = 32; - public static final int BEAM_BREAK_PORT = 0; // TODO: update later - - public static final TalonFXConfiguration CONFIG = GeneralFlywheel.GENERAL_CONFIG; - } - - public final class FlywheelFollowerRight { - public static final int CAN_ID = 33; - - public static final TalonFXConfiguration CONFIG = - GeneralFlywheel.GENERAL_CONFIG - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive)); - } - - public final class TurretRight { - public static final int CAN_ID = 21; - - // +x -> forward; +y -> left - public static final Translation3d ROBOT_TO_TURRET = - new Translation3d( - Units.inchesToMeters(4.5), - Units.inchesToMeters(-7.505), - Units.inchesToMeters(15.340)); - - /* CANCoder */ - public static final int CANCODER_ID = 23; - public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - - public static final double MIN_POSITION_ROTATIONS = -150.0 / 360.0 - Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - public static final double MAX_POSITION_ROTATIONS = 0.0 + Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - - public static final double MIN_POSITION_RADS = Units.rotationsToRadians(MIN_POSITION_ROTATIONS); - public static final double MAX_POSITION_RADS = Units.rotationsToRadians(MAX_POSITION_ROTATIONS); - - public static final SoftwareLimitSwitchConfigs SOFTWARE_LIMIT_CONFIGS = - GeneralTurret.GENERAL_SOFTWARE_LIMIT_CONFIGS - .withReverseSoftLimitThreshold(MIN_POSITION_ROTATIONS) - .withForwardSoftLimitThreshold(MAX_POSITION_ROTATIONS); - - public static final FeedbackConfigs FEEDBACK_CONFIGS = - GeneralTurret.GENERAL_FEEDBACK_CONFIGS - .withFeedbackRemoteSensorID(CANCODER_ID) - .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - - public static final MagnetSensorConfigs CANCODER_CONFIGS = - GeneralTurret.GENERAL_CANCODER_CONFIGS - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); - } } diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 1007604..34c972f 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -34,29 +34,15 @@ import org.team5924.frc2026.generated.TunerConstants; import org.team5924.frc2026.subsystems.drive.Drive; import org.team5924.frc2026.subsystems.drive.GyroIO; +import org.team5924.frc2026.subsystems.drive.GyroIOPigeon2; import org.team5924.frc2026.subsystems.drive.GyroIOSim; import org.team5924.frc2026.subsystems.drive.ModuleIO; +import org.team5924.frc2026.subsystems.drive.ModuleIOTalonFX; import org.team5924.frc2026.subsystems.drive.ModuleIOTalonFXSim; -import org.team5924.frc2026.subsystems.vision.Vision; -import org.team5924.frc2026.subsystems.vision.VisionConstants; -import org.team5924.frc2026.subsystems.vision.VisionIO; -import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVision; -import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVisionSim; -import org.team5924.frc2026.subsystems.rollers.hopper.Hopper; -import org.team5924.frc2026.subsystems.rollers.hopper.Hopper.HopperState; -import org.team5924.frc2026.subsystems.rollers.hopper.HopperIO; -import org.team5924.frc2026.subsystems.rollers.indexer.Indexer; -import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState; -import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIO; -import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIOTalonFX; -import org.team5924.frc2026.subsystems.rollers.intake.Intake; -import org.team5924.frc2026.subsystems.rollers.intake.Intake.IntakeState; -import org.team5924.frc2026.subsystems.rollers.intake.IntakeIO; -import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOSim; import org.team5924.frc2026.subsystems.flywheel.Flywheel; -import org.team5924.frc2026.subsystems.flywheel.Flywheel.FlywheelState; import org.team5924.frc2026.subsystems.flywheel.FlywheelIO; import org.team5924.frc2026.subsystems.flywheel.FlywheelIOSim; +import org.team5924.frc2026.subsystems.flywheel.FlywheelIOTalonFX; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivot; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivot.IntakePivotState; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivotIO; @@ -65,16 +51,30 @@ import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIO; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIOSim; +import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIOTalonFX; +import org.team5924.frc2026.subsystems.rollers.hopper.Hopper; +import org.team5924.frc2026.subsystems.rollers.hopper.HopperIO; import org.team5924.frc2026.subsystems.rollers.hopper.HopperIOSim; +import org.team5924.frc2026.subsystems.rollers.hopper.HopperIOTalonFX; +import org.team5924.frc2026.subsystems.rollers.indexer.Indexer; +import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIO; import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIOSim; -import org.team5924.frc2026.subsystems.turret.Turret; -import org.team5924.frc2026.subsystems.turret.TurretIO; -import org.team5924.frc2026.subsystems.turret.TurretIOSim; +import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIOTalonFX; +import org.team5924.frc2026.subsystems.rollers.intake.Intake; +import org.team5924.frc2026.subsystems.rollers.intake.Intake.IntakeState; +import org.team5924.frc2026.subsystems.rollers.intake.IntakeIO; +import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOSim; +import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOTalonFX; +import org.team5924.frc2026.subsystems.vision.Vision; +import org.team5924.frc2026.subsystems.vision.VisionConstants; +import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVision; +import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVisionSim; public class RobotContainer { // Subsystems private final Drive drive; private SwerveDriveSimulation driveSimulation = null; + private Vision vision; private final Intake intake; @@ -82,13 +82,18 @@ public class RobotContainer { private final Hopper hopper; private final Indexer indexer; - private final ShooterHood shooterHoodRight; - private final Flywheel flywheelRight; - private final Turret turretRight; + private final ShooterHood shooterHood; + private final Flywheel flywheel; + + // Real/IO implementation + private final boolean realDrive = true; + private final boolean realIntake = true; + private final boolean realIntakePivot = true; + private final boolean realHopper = true; + private final boolean realIndexer = true; - private final ShooterHood shooterHoodLeft; - private final Flywheel flywheelLeft; - private final Turret turretLeft; + private final boolean realShooterHood = true; + private final boolean realFlywheel = true; // Controller private final CommandXboxController driveController = new CommandXboxController(0); @@ -105,13 +110,21 @@ public RobotContainer() { // -------------------------- real -------------------------- drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - (pose) -> {}); + realDrive + ? new Drive( + new GyroIOPigeon2(), + new ModuleIOTalonFX(TunerConstants.FrontLeft), + new ModuleIOTalonFX(TunerConstants.FrontRight), + new ModuleIOTalonFX(TunerConstants.BackLeft), + new ModuleIOTalonFX(TunerConstants.BackRight), + (pose) -> {}) + : new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + (pose) -> {}); vision = new Vision( @@ -121,41 +134,23 @@ public RobotContainer() { new VisionIOPhotonVision( VisionConstants.FRONT_RIGHT_NAME, VisionConstants.FRONT_RIGHT_TRANSFORM)); - // intake = new Intake(new IntakeIOTalonFX()); - intakePivot = new IntakePivot(new IntakePivotIOTalonFX()); - // hopper = new Hopper(new HopperIOTalonFX()); - // indexer = new Indexer(new IndexerIOTalonFX()); - - // shooterHoodLeft = new ShooterHood(new ShooterHoodIOTalonFX(true), true); - // flywheelLeft = new Flywheel(new FlywheelIOTalonFX(true), true); - // turretLeft = new Turret(new TurretIOTalonFX(true), true); - - // shooterHoodRight = new ShooterHood(new ShooterHoodIOTalonFX(false), false); - // flywheelRight = new Flywheel(new FlywheelIOTalonFX(false), false); - // turretRight = new Turret(new TurretIOTalonFX(false), false); - - // ---------------------------- IO ---------------------------- - // drive = - // new Drive( - // new GyroIOPigeon2(), - // new ModuleIOTalonFX(TunerConstants.FrontLeft), - // new ModuleIOTalonFX(TunerConstants.FrontRight), - // new ModuleIOTalonFX(TunerConstants.BackLeft), - // new ModuleIOTalonFX(TunerConstants.BackRight), - // (pose) -> {}); - - intake = new Intake(new IntakeIO() {}); - // intakePivot = new IntakePivot(new IntakePivotIO() {}); - hopper = new Hopper(new HopperIO() {}); - indexer = new Indexer(new IndexerIO() {}); - - shooterHoodLeft = new ShooterHood(new ShooterHoodIO() {}, true); - flywheelLeft = new Flywheel(new FlywheelIO() {}, true); - turretLeft = new Turret(new TurretIO() {}, true); - - shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); - flywheelRight = new Flywheel(new FlywheelIO() {}, false); - turretRight = new Turret(new TurretIO() {}, false); + intake = realIntake ? new Intake(new IntakeIOTalonFX()) : new Intake(new IntakeIO() {}); + intakePivot = + realIntakePivot + ? new IntakePivot(new IntakePivotIOTalonFX()) + : new IntakePivot(new IntakePivotIO() {}); + hopper = realHopper ? new Hopper(new HopperIOTalonFX()) : new Hopper(new HopperIO() {}); + indexer = + realIndexer ? new Indexer(new IndexerIOTalonFX()) : new Indexer(new IndexerIO() {}); + + shooterHood = + realShooterHood + ? new ShooterHood(new ShooterHoodIOTalonFX()) + : new ShooterHood(new ShooterHoodIO() {}); + flywheel = + realFlywheel + ? new Flywheel(new FlywheelIOTalonFX()) + : new Flywheel(new FlywheelIO() {}); break; case SIM: @@ -191,13 +186,8 @@ public RobotContainer() { hopper = new Hopper(new HopperIOSim()); indexer = new Indexer(new IndexerIOSim()); - shooterHoodLeft = new ShooterHood(new ShooterHoodIOSim(true), true); - flywheelLeft = new Flywheel(new FlywheelIOSim(), true); - turretLeft = new Turret(new TurretIOSim(true), true); - - shooterHoodRight = new ShooterHood(new ShooterHoodIOSim(false), false); - flywheelRight = new Flywheel(new FlywheelIOSim(), false); - turretRight = new Turret(new TurretIOSim(false), false); + shooterHood = new ShooterHood(new ShooterHoodIOSim()); + flywheel = new Flywheel(new FlywheelIOSim()); break; @@ -219,13 +209,8 @@ public RobotContainer() { hopper = new Hopper(new HopperIO() {}); // TODO: Add replay IO implementation indexer = new Indexer(new IndexerIO() {}); - shooterHoodLeft = new ShooterHood(new ShooterHoodIO() {}, true); - flywheelLeft = new Flywheel(new FlywheelIO() {}, true); - turretLeft = new Turret(new TurretIO() {}, true); - - shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); - flywheelRight = new Flywheel(new FlywheelIO() {}, false); - turretRight = new Turret(new TurretIO() {}, false); + shooterHood = new ShooterHood(new ShooterHoodIO() {}); + flywheel = new Flywheel(new FlywheelIO() {}); break; } @@ -349,61 +334,21 @@ private void configureButtonBindings() { hopper.setDefaultCommand( Commands.run(() -> hopper.setGoalState(Hopper.HopperState.ON), hopper)); - // flywheelLeft.setDefaultCommand( - // Commands.runOnce( - // () -> flywheelLeft.setGoalState(FlywheelState.OFF), - // flywheelLeft)); - - // flywheelRight.setDefaultCommand( - // Commands.runOnce( - // () -> flywheelRight.setGoalState(FlywheelState.OFF), - // flywheelRight)); - - // // ### intake pivot down/stow - - intakePivot.setDefaultCommand( - Commands.run( - () -> { - intakePivot.setGoalState(IntakePivotState.MANUAL); - intakePivot.setInput(operatorController.getLeftX()); - }, - intakePivot)); - - operatorController - .leftBumper() - .onTrue( - Commands.runOnce( - () -> { - intakePivot.setGoalState(IntakePivotState.DOWN); - // intake.setGoalState(IntakeState.INTAKE); - }, - intakePivot /*, - intake*/)); - operatorController - .rightBumper() - .onTrue( - Commands.runOnce( - () -> { - intakePivot.setGoalState(IntakePivotState.STOW); - // intake.setGoalState(IntakeState.OFF); - }, - intakePivot /*, - intake*/)); - - // // // ### intake pivot spit + /* ### intake ### */ driveController - .leftTrigger() + .leftBumper() .onTrue( Commands.runOnce( () -> { intakePivot.setGoalState(IntakePivotState.DOWN); - intake.setGoalState(IntakeState.SPITOUT); + intake.setGoalState(IntakeState.INTAKE); }, intakePivot, intake)); + driveController - .leftTrigger() - .onFalse( + .rightBumper() + .onTrue( Commands.runOnce( () -> { intakePivot.setGoalState(IntakePivotState.STOW); @@ -412,268 +357,38 @@ private void configureButtonBindings() { intakePivot, intake)); - // // ### indexing - operatorController - .a() - .onTrue( - Commands.runOnce( - () -> { - // intakePivot.setGoalState(IntakePivotState.SHOOTING); - indexer.setGoalState(Indexer.IndexerState.INDEXING); - }, - // intakePivot, - indexer)); - operatorController - .a() - .onFalse( - Commands.runOnce( - () -> { - // intakePivot.setGoalState(IntakePivotState.STOW); - indexer.setGoalState(Indexer.IndexerState.OFF); - }, - // intakePivot, - indexer)); - - // // ### shooting voltages - // operatorController - // .a() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B4); - // flywheelLeft.setGoalState(FlywheelState.B4); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .b() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B6); - // flywheelLeft.setGoalState(FlywheelState.B6); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .x() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B8); - // flywheelLeft.setGoalState(FlywheelState.B8); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .y() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B12); - // flywheelLeft.setGoalState(FlywheelState.B12); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .rightTrigger() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.OFF); - // flywheelLeft.setGoalState(FlywheelState.OFF); - // }, - // flywheelLeft, - // flywheelRight)); - - // driveController - // .povUp() - // .onTrue( - // DriveCommands.driveFacingHub( - // drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); - - // // AJGAOEIBAWIBAEROBIAEROBIJAROIBHAEBOIAERBIOHAERBHIOAERb - // // operatorController - // // .leftTrigger() - // // .onTrue( - // // AutoScoreCommands.runTrackTargetCommand( - // // turretLeft, shooterHoodLeft, flywheelLeft, true)); - // // operatorController - // // .leftTrigger() - // // .onTrue( - // // AutoScoreCommands.runTrackTargetCommand( - // // turretRight, shooterHoodRight, flywheelRight, false)); - - // // --------------------------------------------------------- - - // // turretLeft.setDefaultCommand( - // // Commands.run( - // // () -> { - // // turretLeft.setGoalState(TurretState.MANUAL); - // // turretLeft.setInput(driveController.getRightX()); - // // }, - // // turretLeft)); + // manual intake + intakePivot.setDefaultCommand( + Commands.run( + () -> { + intakePivot.setGoalState(IntakePivotState.MANUAL); + intakePivot.setInput(operatorController.getLeftX()); + }, + intakePivot)); + // // ### intake pivot spit // driveController // .leftTrigger() // .onTrue( // Commands.runOnce( // () -> { - // turretLeft.setGoalState(TurretState.ZERO); + // intakePivot.setGoalState(IntakePivotState.DOWN); + // intake.setGoalState(IntakeState.SPITOUT); // }, - // turretLeft)); - + // intakePivot, + // intake)); // driveController - // .rightTrigger() - // .onTrue( + // .leftTrigger() + // .onFalse( // Commands.runOnce( // () -> { - // turretLeft.setGoalState(TurretState.NINETY); + // intakePivot.setGoalState(IntakePivotState.STOW); + // intake.setGoalState(IntakeState.OFF); // }, - // turretLeft)); - - // // --------------------------------------------------------- - - // shooterHoodLeft.setDefaultCommand( - // Commands.run( - // () -> { - // shooterHoodLeft.setGoalState(ShooterHoodState.MANUAL); - // shooterHoodLeft.setInput(operatorController.getRightY()); - // }, - // shooterHoodLeft, - // shooterHoodRight)); - - operatorController - .leftTrigger() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.setGoalState(FlywheelState.OFF); - }, - flywheelRight)); - - operatorController - .rightTrigger() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.setGoalState(FlywheelState.FAST_LAUNCH); - }, - flywheelRight)); - - operatorController - .y() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.setGoalState(FlywheelState.SLOW_LAUNCH); - }, - flywheelRight)); - - operatorController - .povDown() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.updateSetpointState(-5.0); - }, - flywheelRight)); - - operatorController - .povUp() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.updateSetpointState(5.0); - }, - flywheelRight)); - - // // driveController - // // .rightTrigger() - // // .onTrue( - // // Commands.runOnce( - // // () -> { - // // // shooterHoodLeft.setGoalState(ShooterHoodState.OFF); - // // // shooterHoodRight.setGoalState(ShooterHoodState.OFF); - // // // turretLeft.setGoalState(Turret.TurretState.OFF); - // // // turretRight.setGoalState(Turret.TurretState.OFF); - // // flywheelLeft.setGoalState(FlywheelState.BUMPER_SHOOTING); - // // flywheelRight.setGoalState(FlywheelState.BUMPER_SHOOTING); - // // }, - // // shooterHoodLeft, - // // shooterHoodRight, - // // turretLeft, - // // turretRight, - // // flywheelLeft, - // // flywheelRight)); - - // // operatorController - // // .leftBumper() - // // .onTrue( - // // Commands.runOnce( - // // () -> { - // // hopper.setGoalState(HopperState.ON); - // // indexer.setGoalState(IndexerState.INDEXING); - // // flywheelLeft.setGoalState(FlywheelState.BUMPER_SHOOTING); - // // flywheelRight.setGoalState(FlywheelState.BUMPER_SHOOTING); - // // })); - - // // operatorController - // // .leftBumper() - // // .onFalse( - // // Commands.runOnce( - // // () -> { - // // hopper.setGoalState(HopperState.OFF); - // // indexer.setGoalState(IndexerState.OFF); - // // flywheelLeft.setGoalState(FlywheelState.OFF); - // // flywheelRight.setGoalState(FlywheelState.OFF); - // // })); - - // // operatorController - // // .leftTrigger() - // // .onTrue(Commands.runOnce(() -> intake.setGoalState(IntakeState.INTAKE))); - // // operatorController - // // .leftTrigger() - // // .onFalse(Commands.runOnce(() -> intake.setGoalState(IntakeState.OFF))); - - // // operatorController - // // .rightBumper() - // // .onTrue( - // // Commands.runOnce( - // // () -> { - // // flywheelRight.setGoalState(FlywheelState.MANUAL); - // // })); - - // // operatorController - // // .rightBumper() - // // .onFalse( - // // Commands.runOnce( - // // () -> { - // // flywheelRight.setGoalState(FlywheelState.OFF); - // // })); - - // // shooterHoodRight.setDefaultCommand( - // // ShooterCommands.manualShooterHood(shooterHoodRight, () -> - // // operatorController.getRightY())); - // // shooterHoodLeft.setDefaultCommand( - // // ShooterCommands.manualShooterHood(shooterHoodLeft, () -> - // // operatorController.getRightY())); - // // turretRight.setDefaultCommand( - // // ShooterCommands.manualTurret(turretRight, () -> operatorController.getLeftX())); - // // turretLeft.setDefaultCommand( - // // ShooterCommands.manualTurret(turretLeft, () -> operatorController.getRightX())); - - // // shooterHoodLeft.setDefaultCommand(getAutonomousCommand()); // todo - // // shooterHoodRight.setDefaultCommand(getAutonomousCommand()); // todo - // // turretLeft.setDefaultCommand(getAutonomousCommand()); // todo - // // turretRight.setDefaultCommand(getAutonomousCommand()); // todo - // // flywheelLeft.setDefaultCommand(getAutonomousCommand()); // todo - // // flywheelRight.setDefaultCommand(getAutonomousCommand()); // todo + // intakePivot, + // intake)); + // TODO: shooting, auto shooting } /** diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index 0ceb4d9..b1accf9 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -22,13 +22,6 @@ import lombok.Getter; import lombok.Setter; import org.littletonrobotics.junction.AutoLogOutput; -import org.team5924.frc2026.subsystems.flywheel.Flywheel.FlywheelState; -import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivot.IntakePivotState; -import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; -import org.team5924.frc2026.subsystems.rollers.hopper.Hopper.HopperState; -import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState; -import org.team5924.frc2026.subsystems.rollers.intake.Intake.IntakeState; -import org.team5924.frc2026.subsystems.turret.Turret.TurretState; @Getter public class RobotState { @@ -73,28 +66,4 @@ public void resetPose(Pose2d pose) { @Getter @Setter private Rotation2d yawPosition = new Rotation2d(); @Getter @Setter private double yawVelocityRadPerSec = 0.0; - - /* ### Intake ### */ - @Getter @Setter private IntakeState intakeState = IntakeState.OFF; - - /* ### Intake ### */ - @Getter @Setter private IntakePivotState intakePivotState = IntakePivotState.OFF; - - /* ### Hopper ### */ - @Getter @Setter private HopperState hopperState = HopperState.OFF; - - /* ### Indexer ### */ - @Getter @Setter private IndexerState indexerState = IndexerState.OFF; - - // Turret - @Getter @Setter private TurretState leftTurretState = TurretState.OFF; - @Getter @Setter private TurretState rightTurretState = TurretState.OFF; - - /*### Flywheel ### */ - @Getter @Setter private FlywheelState leftFlywheelState = FlywheelState.OFF; - @Getter @Setter private FlywheelState rightFlywheelState = FlywheelState.OFF; - - /*### Shooter Hood ### */ - @Getter @Setter private ShooterHoodState leftShooterHoodState = ShooterHoodState.OFF; - @Getter @Setter private ShooterHoodState rightShooterHoodState = ShooterHoodState.OFF; } diff --git a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java index 78ae3c5..dc8b8f6 100644 --- a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java @@ -22,8 +22,6 @@ import org.team5924.frc2026.subsystems.flywheel.Flywheel.FlywheelState; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; -import org.team5924.frc2026.subsystems.turret.Turret; -import org.team5924.frc2026.subsystems.turret.Turret.TurretState; import org.team5924.frc2026.util.LaunchCalculator; import org.team5924.frc2026.util.LaunchCalculator.LaunchingParameters; @@ -43,23 +41,19 @@ public class AutoScoreCommands { // } private AutoScoreCommands() {} - public static Command runTrackTargetCommand( - Turret turret, ShooterHood shooterHood, Flywheel flywheel, boolean isLeft) { + public static Command runTrackTargetCommand(ShooterHood shooterHood, Flywheel flywheel) { return Commands.run( () -> { - LaunchingParameters launchParams = LaunchCalculator.getInstance().getParameters(isLeft); + LaunchingParameters launchParams = LaunchCalculator.getInstance().getParameters(); - turret.setAutoInput(launchParams.turretRadians()); shooterHood.setAutoInput(launchParams.hoodAngle()); flywheel.setAutoInput(launchParams.flywheelSpeed()); - turret.setGoalState(TurretState.AUTO); shooterHood.setGoalState(ShooterHoodState.AUTO); flywheel.setGoalState(FlywheelState.AUTO); LaunchCalculator.getInstance().clearLaunchingParameters(); }, - turret, shooterHood, flywheel); } diff --git a/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java b/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java index ee50abf..962f264 100644 --- a/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java @@ -21,8 +21,6 @@ import java.util.function.DoubleSupplier; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; -import org.team5924.frc2026.subsystems.turret.Turret; -import org.team5924.frc2026.subsystems.turret.Turret.TurretState; public class ManualShooterCommands { @@ -36,13 +34,4 @@ public static Command manualShooterHood(ShooterHood hood, DoubleSupplier inputSu }, hood); } - - public static Command manualTurret(Turret turret, DoubleSupplier inputSupplier) { - return Commands.run( - () -> { - turret.setGoalState(TurretState.MANUAL); - turret.setInput(inputSupplier.getAsDouble()); - }, - turret); - } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java index e595765..1599f4e 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java @@ -25,13 +25,11 @@ import lombok.Setter; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.EqualsUtil; import org.team5924.frc2026.util.LoggedTunableNumber; public class Flywheel extends SubsystemBase { private final FlywheelIO io; - private final boolean isLeft; private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); @@ -60,41 +58,36 @@ public enum FlywheelState { private final DoubleSupplier velocityRotationsPerSec; } - private final String side; - private final Alert flywheelMotorDisconnected; protected final Alert overheatAlert; @Getter private FlywheelState goalState = FlywheelState.OFF; + private FlywheelState currentState = FlywheelState.OFF; private boolean isAtSetpoint = false; @Setter private double autoInput = 0.0; - public Flywheel(FlywheelIO io, boolean isLeft) { - side = isLeft ? "Left" : "Right"; - this.isLeft = isLeft; + public Flywheel(FlywheelIO io) { this.io = io; this.goalState = FlywheelState.OFF; this.flywheelMotorDisconnected = - new Alert(side + " Flywheel Motor Disconnected!", Alert.AlertType.kWarning); + new Alert("Flywheel Motor Disconnected!", Alert.AlertType.kWarning); - overheatAlert = new Alert(side + " Flywheel motor overheating!", Alert.AlertType.kWarning); + overheatAlert = new Alert("Flywheel motor overheating!", Alert.AlertType.kWarning); } @Override public void periodic() { io.periodicUpdates(); io.updateInputs(inputs); - Logger.processInputs("Flywheel/" + side, inputs); + Logger.processInputs("Flywheel/", inputs); - Logger.recordOutput("Flywheel/" + side + "/GoalState", goalState.toString()); - Logger.recordOutput( - "Flywheel/" + side + "/CurrentState", getRespectiveFlywheelState().toString()); + Logger.recordOutput("Flywheel/GoalState", goalState.toString()); + Logger.recordOutput("Flywheel/CurrentState", currentState.toString()); Logger.recordOutput( - "Flywheel/" + side + "/TargetVelocityRotationsPerSec", getTargetVelocityRotationsPerSec()); - Logger.recordOutput( - "Flywheel/" + side + "/CurrentVelocityRotationsPerSec", inputs.velocityRotationsPerSec); - Logger.recordOutput("Flywheel/" + side + "/IsAtSetpoint", isAtSetpoint); + "Flywheel/TargetVelocityRotationsPerSec", getTargetVelocityRotationsPerSec()); + Logger.recordOutput("Flywheel/CurrentVelocityRotationsPerSec", inputs.velocityRotationsPerSec); + Logger.recordOutput("Flywheel/IsAtSetpoint", isAtSetpoint); flywheelMotorDisconnected.set(!inputs.motorConnected); @@ -123,14 +116,14 @@ public void setGoalState(FlywheelState goalState) { this.goalState = goalState; switch (goalState) { - case MANUAL -> setRespectiveFlywheelState(FlywheelState.MANUAL); + case MANUAL -> currentState = FlywheelState.MANUAL; case MOVING -> DriverStation.reportError( "Flywheel: MOVING is an invalid goal state; it is a transition state!!", null); - case OFF -> setRespectiveFlywheelState(FlywheelState.OFF); - case B4, B6, B8, B12 -> setRespectiveFlywheelState(goalState); - case AUTO -> setRespectiveFlywheelState(FlywheelState.AUTO); - default -> setRespectiveFlywheelState(FlywheelState.MOVING); + case OFF -> currentState = FlywheelState.OFF; + case B4, B6, B8, B12 -> currentState = goalState; + case AUTO -> currentState = FlywheelState.AUTO; + default -> currentState = FlywheelState.MOVING; } } @@ -138,7 +131,7 @@ public boolean isAtSetpoint() { return EqualsUtil.epsilonEquals( getTargetVelocityRotationsPerSec(), inputs.velocityRotationsPerSec, - Constants.GeneralFlywheel.EPSILON_VELOCITY); + Constants.Flywheel.EPSILON_VELOCITY); } private double getTargetVelocityRotationsPerSec() { @@ -150,11 +143,10 @@ private double getTargetVelocityRotationsPerSec() { private void handleCurrentState() { isAtSetpoint = isAtSetpoint(); - switch (getRespectiveFlywheelState()) { + switch (currentState) { case MOVING -> { setVelocity(getTargetVelocityRotationsPerSec()); - if (isAtSetpoint() && goalState != FlywheelState.AUTO) - setRespectiveFlywheelState(goalState); + if (isAtSetpoint() && goalState != FlywheelState.AUTO) currentState = goalState; } case MANUAL -> handleManualState(); case OFF -> stop(); @@ -177,17 +169,6 @@ private void handleManualState() { public void updateSetpointState(double change) { autoInput = inputs.setpointVelocityRotationsPerSec + change; - setGoalState(FlywheelState.SETPOINT); - } - - private void setRespectiveFlywheelState(FlywheelState state) { - if (isLeft) RobotState.getInstance().setLeftFlywheelState(state); - else RobotState.getInstance().setRightFlywheelState(state); - } - - private FlywheelState getRespectiveFlywheelState() { - return isLeft - ? RobotState.getInstance().getLeftFlywheelState() - : RobotState.getInstance().getRightFlywheelState(); + goalState = FlywheelState.SETPOINT; } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java index eced248..c3fa25d 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java @@ -34,9 +34,7 @@ public FlywheelIOSim() { sim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - gearbox, - Constants.GeneralFlywheel.SIM_MOI, - Constants.GeneralFlywheel.MOTOR_TO_MECHANISM), + gearbox, Constants.Flywheel.SIM_MOI, Constants.Flywheel.MOTOR_TO_MECHANISM), gearbox); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java index 22664cd..4568a83 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java @@ -36,11 +36,7 @@ import edu.wpi.first.units.measure.Voltage; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; -import org.team5924.frc2026.Constants.FlywheelFollowerLeft; -import org.team5924.frc2026.Constants.FlywheelFollowerRight; -import org.team5924.frc2026.Constants.FlywheelLeaderLeft; -import org.team5924.frc2026.Constants.FlywheelLeaderRight; -import org.team5924.frc2026.Constants.GeneralFlywheel; +import org.team5924.frc2026.Constants.Flywheel; import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; @@ -60,35 +56,20 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final MotionMagicConfigs motionMagicConfigs; private double setpointVelocityRotationsPerSec; - /* Gains Left */ - private final LoggedTunableNumber kPLeft = new LoggedTunableNumber("Flywheel/Left/kP", 0.5); - private final LoggedTunableNumber kILeft = new LoggedTunableNumber("Flywheel/Left/kI", 0.0); - private final LoggedTunableNumber kDLeft = new LoggedTunableNumber("Flywheel/Left/kD", 0.0); - private final LoggedTunableNumber kSLeft = new LoggedTunableNumber("Flywheel/Left/kS", 0.25); - private final LoggedTunableNumber kVLeft = new LoggedTunableNumber("Flywheel/Left/kV", 0.0705); - private final LoggedTunableNumber kALeft = new LoggedTunableNumber("Flywheel/Left/kA", 0.0); - - private final LoggedTunableNumber motionCruiseVelocityLeft = - new LoggedTunableNumber("Flywheel/Left/MotionCruiseVelocity", 10.0); - private final LoggedTunableNumber motionAccelerationLeft = - new LoggedTunableNumber("Flywheel/Left/MotionAcceleration", 100.0); - private final LoggedTunableNumber motionJerkLeft = - new LoggedTunableNumber("Flywheel/Left/MotionJerk", 0.0); - - /* Gains Right */ - private final LoggedTunableNumber kPRight = new LoggedTunableNumber("Flywheel/Right/kP", 0.5); - private final LoggedTunableNumber kIRight = new LoggedTunableNumber("Flywheel/Right/kI", 0.0); - private final LoggedTunableNumber kDRight = new LoggedTunableNumber("Flywheel/Right/kD", 0.0); - private final LoggedTunableNumber kSRight = new LoggedTunableNumber("Flywheel/Right/kS", 0.25); - private final LoggedTunableNumber kVRight = new LoggedTunableNumber("Flywheel/Right/kV", 0.0705); - private final LoggedTunableNumber kARight = new LoggedTunableNumber("Flywheel/Right/kA", 0.0); - - private final LoggedTunableNumber motionCruiseVelocityRight = - new LoggedTunableNumber("Flywheel/Right/MotionCruiseVelocity", 10.0); - private final LoggedTunableNumber motionAccelerationRight = - new LoggedTunableNumber("Flywheel/Right/MotionAcceleration", 100.0); - private final LoggedTunableNumber motionJerkRight = - new LoggedTunableNumber("Flywheel/Right/MotionJerk", 0.0); + /* Gains */ + private final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheel/kP", 0.5); + private final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheel/kI", 0.0); + private final LoggedTunableNumber kD = new LoggedTunableNumber("Flywheel/kD", 0.0); + private final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheel/kS", 0.25); + private final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheel/kV", 0.0705); + private final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheel/kA", 0.0); + + private final LoggedTunableNumber motionCruiseVelocity = + new LoggedTunableNumber("Flywheel/MotionCruiseVelocity", 10.0); + private final LoggedTunableNumber motionAcceleration = + new LoggedTunableNumber("Flywheel/MotionAcceleration", 100.0); + private final LoggedTunableNumber motionJerk = + new LoggedTunableNumber("Flywheel/MotionJerk", 0.0); /* Status Signals */ private final StatusSignal position; @@ -105,25 +86,10 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final VoltageOut voltageOut; private final MotionMagicVelocityVoltage motionMagicVelocity; - private final Runnable periodicUpdateSlot0; - private final Runnable periodicUpdateMotionMagic; + public FlywheelIOTalonFX() { + leaderTalon = new TalonFX(Flywheel.CAN_ID, new CANBus(Flywheel.BUS)); - private final boolean isLeft; - private final String side; - - public FlywheelIOTalonFX(boolean isLeft) { - this.isLeft = isLeft; - side = isLeft ? "Left" : "Right"; - - leaderTalon = - new TalonFX( - isLeft ? FlywheelLeaderLeft.CAN_ID : FlywheelLeaderRight.CAN_ID, - new CANBus(GeneralFlywheel.BUS)); - - followerTalon = - new TalonFX( - isLeft ? FlywheelFollowerLeft.CAN_ID : FlywheelFollowerRight.CAN_ID, - new CANBus(GeneralFlywheel.BUS)); + followerTalon = new TalonFX(Flywheel.FOLLOWER_CAN_ID, new CANBus(Flywheel.BUS)); leaderConfig = leaderTalon.getConfigurator(); followerConfig = followerTalon.getConfigurator(); @@ -134,42 +100,20 @@ public FlywheelIOTalonFX(boolean isLeft) { motionMagicConfigs = new MotionMagicConfigs(); updateMotionMagicConfigs(); - periodicUpdateSlot0 = - () -> { - updateSlot0Configs(); - - StatusCode statusCode = leaderConfig.apply(slot0Configs); - if (!statusCode.isOK()) { - Logger.recordOutput("Flywheel/" + side + "/UpdateSlot0Report", statusCode); - } - }; - - periodicUpdateMotionMagic = - () -> { - updateMotionMagicConfigs(); - - StatusCode statusCode = leaderConfig.apply(motionMagicConfigs); - if (!statusCode.isOK()) { - Logger.recordOutput("Flywheel/" + side + "/UpdateStatusCodeReport", statusCode); - } - }; - // Apply Configs StatusCode[] statusArray = new StatusCode[10]; - statusArray[0] = - leaderConfig.apply(isLeft ? FlywheelLeaderLeft.CONFIG : FlywheelLeaderRight.CONFIG); + statusArray[0] = leaderConfig.apply(Flywheel.CONFIG); statusArray[1] = leaderConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); statusArray[2] = leaderConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[3] = leaderConfig.apply(GeneralFlywheel.FEEDBACK_CONFIGS); + statusArray[3] = leaderConfig.apply(Flywheel.FEEDBACK_CONFIGS); statusArray[4] = leaderConfig.apply(slot0Configs); statusArray[5] = leaderConfig.apply(motionMagicConfigs); - statusArray[6] = - followerConfig.apply(isLeft ? FlywheelFollowerLeft.CONFIG : FlywheelFollowerRight.CONFIG); + statusArray[6] = followerConfig.apply(Flywheel.FOLLOWER_CONFIG); statusArray[7] = followerConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); statusArray[8] = followerConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[9] = followerConfig.apply(GeneralFlywheel.FEEDBACK_CONFIGS); + statusArray[9] = followerConfig.apply(Flywheel.FEEDBACK_CONFIGS); boolean isErrorPresent = false; for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; @@ -177,16 +121,11 @@ public FlywheelIOTalonFX(boolean isLeft) { if (isErrorPresent) Elastic.sendNotification( new Notification( - NotificationLevel.WARNING, - side + "Flywheel Configs", - "Error in " + side + " shooter flywheel configs!")); + NotificationLevel.WARNING, "Flywheel Configs", "Error in shooter flywheel configs!")); - Logger.recordOutput("Flywheel/" + side + "/InitConfReport", statusArray); + Logger.recordOutput("Flywheel/InitConfReport", statusArray); - followerTalon.setControl( - isLeft - ? new Follower(FlywheelLeaderLeft.CAN_ID, MotorAlignmentValue.Opposed) - : new Follower(FlywheelLeaderRight.CAN_ID, MotorAlignmentValue.Opposed)); + followerTalon.setControl(new Follower(Flywheel.CAN_ID, MotorAlignmentValue.Opposed)); // Get select status signals and set update frequency position = leaderTalon.getPosition(); @@ -250,55 +189,51 @@ public void periodicUpdates() { } private void updateSlot0Configs() { - if (isLeft) { - slot0Configs.kP = kPLeft.get(); - slot0Configs.kI = kILeft.get(); - slot0Configs.kD = kDLeft.get(); - slot0Configs.kS = kSLeft.get(); - slot0Configs.kV = kVLeft.get(); - slot0Configs.kA = kALeft.get(); - } else { - slot0Configs.kP = kPRight.get(); - slot0Configs.kI = kIRight.get(); - slot0Configs.kD = kDRight.get(); - slot0Configs.kS = kSRight.get(); - slot0Configs.kV = kVRight.get(); - slot0Configs.kA = kARight.get(); - } + slot0Configs.kP = kP.get(); + slot0Configs.kI = kI.get(); + slot0Configs.kD = kD.get(); + slot0Configs.kS = kS.get(); + slot0Configs.kV = kV.get(); + slot0Configs.kA = kA.get(); } private void updateMotionMagicConfigs() { - if (isLeft) { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationLeft.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityLeft.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkLeft.get(); - } else { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationRight.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityRight.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkRight.get(); - } + motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); + motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); + motionMagicConfigs.MotionMagicJerk = motionJerk.get(); } private void updateLoggedTunableNumbers() { - if (isLeft) { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPLeft, kILeft, kDLeft, kSLeft, kVLeft, kALeft); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationLeft, - motionCruiseVelocityLeft, - motionJerkLeft); - } else { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPRight, kIRight, kDRight, kSRight, kVRight, kARight); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationRight, - motionCruiseVelocityRight, - motionJerkRight); - } + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + updateSlot0Configs(); + + StatusCode statusCode = leaderConfig.apply(slot0Configs); + if (!statusCode.isOK()) { + Logger.recordOutput("Flywheel/UpdateSlot0Report", statusCode); + } + }, + kP, + kI, + kD, + kS, + kV, + kA); + + LoggedTunableNumber.ifChanged( + hashCode() + 1, + () -> { + updateMotionMagicConfigs(); + + StatusCode statusCode = leaderConfig.apply(motionMagicConfigs); + if (!statusCode.isOK()) { + Logger.recordOutput("Flywheel/UpdateStatusCodeReport", statusCode); + } + }, + motionAcceleration, + motionCruiseVelocity, + motionJerk); } @Override diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java index 526a87d..aff7f93 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java @@ -26,7 +26,6 @@ import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; import org.team5924.frc2026.FieldState; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.EqualsUtil; import org.team5924.frc2026.util.LoggedTunableNumber; @@ -59,6 +58,7 @@ public enum IntakePivotState { protected final Alert overheatAlert; @Getter private IntakePivotState goalState = IntakePivotState.OFF; + private IntakePivotState currentState = IntakePivotState.OFF; private boolean isAtSetpoint = false; private double lastStateChange = 0.0; private double timeSinceLastStateChange = 0.0; @@ -84,8 +84,7 @@ public void periodic() { handleCurrentState(); Logger.recordOutput("IntakePivot/GoalState", goalState.toString()); - Logger.recordOutput( - "IntakePivot/CurrentState", RobotState.getInstance().getIntakePivotState().toString()); + Logger.recordOutput("IntakePivot/CurrentState", currentState.toString()); Logger.recordOutput("IntakePivot/TargetRads", goalState.rads.getAsDouble()); Logger.recordOutput("IntakePivot/CurrentRads", inputs.positionRads); Logger.recordOutput("IntakePivot/IsAtSetpoint", isAtSetpoint); @@ -111,12 +110,12 @@ public void setGoalState(IntakePivotState goalState) { this.goalState = goalState; switch (goalState) { - case MANUAL -> RobotState.getInstance().setIntakePivotState(IntakePivotState.MANUAL); + case MANUAL -> currentState = IntakePivotState.MANUAL; case MOVING -> DriverStation.reportError( "IntakePivot: MOVING is an invalid goal state; it is a transition state!!", null); - case OFF -> RobotState.getInstance().setIntakePivotState(IntakePivotState.OFF); - default -> RobotState.getInstance().setIntakePivotState(IntakePivotState.MOVING); + case OFF -> currentState = IntakePivotState.OFF; + default -> currentState = IntakePivotState.MOVING; } lastStateChange = FieldState.getInstance().getTime(); @@ -134,10 +133,10 @@ private void handleCurrentState() { timeSinceLastStateChange = FieldState.getInstance().getTime() - lastStateChange; isAtSetpoint = isAtSetpoint(); - switch (RobotState.getInstance().getIntakePivotState()) { + switch (currentState) { case MOVING -> { setPosition(goalState.getRads().getAsDouble()); - if (isAtSetpoint) RobotState.getInstance().setIntakePivotState(goalState); + if (isAtSetpoint) currentState = goalState; } case MANUAL -> handleManualState(); case OFF -> stop(); diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java index 88011da..77b8c66 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java @@ -26,13 +26,11 @@ import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; import org.team5924.frc2026.FieldState; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.EqualsUtil; import org.team5924.frc2026.util.LoggedTunableNumber; public class ShooterHood extends SubsystemBase { private final ShooterHoodIO io; - private final boolean isLeft; private final ShooterHoodIOInputsAutoLogged inputs = new ShooterHoodIOInputsAutoLogged(); @@ -60,14 +58,13 @@ public enum ShooterHoodState { private final DoubleSupplier rads; } - private final String side; - private final Alert motorDisconnectedAlert; protected final Alert overheatAlert; private final Alert notImplementedAlert; private boolean showNotImplementedAlert; - @Getter private ShooterHoodState goalState; + @Getter private ShooterHoodState goalState = ShooterHoodState.OFF; + private ShooterHoodState currentState = ShooterHoodState.OFF; private boolean isAtSetpoint = false; private double lastStateChange = 0.0; private double timeSinceLastStateChange = 0.0; @@ -75,38 +72,33 @@ public enum ShooterHoodState { @Setter private double input; @Setter private double autoInput = 0.0; - public ShooterHood(ShooterHoodIO io, boolean isLeft) { - side = isLeft ? "Left" : "Right"; + public ShooterHood(ShooterHoodIO io) { this.io = io; - this.isLeft = isLeft; goalState = ShooterHoodState.OFF; motorDisconnectedAlert = - new Alert(side + " Shooter Hood Motor Disconnected!", Alert.AlertType.kWarning); - notImplementedAlert = - new Alert(side + " Auto Shooting not yet implemented!", Alert.AlertType.kWarning); - overheatAlert = new Alert(side + " Shooter Hood motor overheating!", Alert.AlertType.kWarning); + new Alert("Shooter Hood Motor Disconnected!", Alert.AlertType.kWarning); + notImplementedAlert = new Alert("Auto Shooting not yet implemented!", Alert.AlertType.kWarning); + overheatAlert = new Alert("Shooter Hood motor overheating!", Alert.AlertType.kWarning); } @Override public void periodic() { io.periodicUpdates(); io.updateInputs(inputs); - Logger.processInputs("ShooterHood/" + side, inputs); + Logger.processInputs("ShooterHood", inputs); motorDisconnectedAlert.set(!inputs.motorConnected); overheatAlert.set(inputs.tempCelsius > Constants.OVERHEAT_THRESHOLD); handleCurrentState(); - Logger.recordOutput("ShooterHood/" + side + "/GoalState", goalState.toString()); - Logger.recordOutput( - "ShooterHood/" + side + "/CurrentState", getRespectiveShooterHoodState().toString()); - Logger.recordOutput("ShooterHood/" + side + "/TargetRads", getTargetRads()); - Logger.recordOutput("ShooterHood/" + side + "/CurrentRads", inputs.positionRads); - Logger.recordOutput("ShooterHood/" + side + "/IsAtSetpoint", isAtSetpoint); - Logger.recordOutput( - "ShooterHood/" + side + "/TimeSinceLastStateChange", timeSinceLastStateChange); + Logger.recordOutput("ShooterHood/GoalState", goalState.toString()); + Logger.recordOutput("ShooterHood/CurrentState", currentState.toString()); + Logger.recordOutput("ShooterHood/TargetRads", getTargetRads()); + Logger.recordOutput("ShooterHood/CurrentRads", inputs.positionRads); + Logger.recordOutput("ShooterHood/IsAtSetpoint", isAtSetpoint); + Logger.recordOutput("ShooterHood/TimeSinceLastStateChange", timeSinceLastStateChange); } public void runVolts(double volts) { @@ -128,15 +120,13 @@ public void setGoalState(ShooterHoodState goalState) { this.goalState = goalState; switch (goalState) { - case MANUAL, AUTO_SHOOTING, NEUTRAL_SHUFFLING, OPPONENT_SHUFFLING -> - setRespectiveShooterHoodState(goalState); + case MANUAL, AUTO_SHOOTING, NEUTRAL_SHUFFLING, OPPONENT_SHUFFLING -> currentState = goalState; case MOVING -> DriverStation.reportError( - side + " Shooter Hood: MOVING is an invalid goal state; it is a transition state!!", - null); - case AUTO -> setRespectiveShooterHoodState(ShooterHoodState.MOVING); - case OFF -> setRespectiveShooterHoodState(ShooterHoodState.OFF); - default -> setRespectiveShooterHoodState(ShooterHoodState.MOVING); + "Shooter Hood: MOVING is an invalid goal state; it is a transition state!!", null); + case AUTO -> currentState = ShooterHoodState.MOVING; + case OFF -> currentState = ShooterHoodState.OFF; + default -> currentState = ShooterHoodState.MOVING; } lastStateChange = FieldState.getInstance().getTime(); @@ -144,10 +134,10 @@ public void setGoalState(ShooterHoodState goalState) { @SuppressWarnings("unused") public boolean isAtSetpoint() { - return (!Constants.GeneralShooterHood.ENABLE_TIMEOUT - || timeSinceLastStateChange > Constants.GeneralShooterHood.STATE_TIMEOUT) + return (!Constants.ShooterHood.ENABLE_TIMEOUT + || timeSinceLastStateChange > Constants.ShooterHood.STATE_TIMEOUT) && EqualsUtil.epsilonEquals( - inputs.positionRads, getTargetRads(), Constants.GeneralShooterHood.EPSILON_RADS); + inputs.positionRads, getTargetRads(), Constants.ShooterHood.EPSILON_RADS); } private double getTargetRads() { @@ -159,11 +149,10 @@ private void handleCurrentState() { isAtSetpoint = isAtSetpoint(); showNotImplementedAlert = false; - switch (getRespectiveShooterHoodState()) { + switch (currentState) { case MOVING -> { setPosition(getTargetRads()); - if (isAtSetpoint() && goalState != ShooterHoodState.AUTO) - setRespectiveShooterHoodState(goalState); + if (isAtSetpoint() && goalState != ShooterHoodState.AUTO) currentState = goalState; } case AUTO_SHOOTING, NEUTRAL_SHUFFLING, OPPONENT_SHUFFLING -> { showNotImplementedAlert = true; // TODO: handle this sometime @@ -188,15 +177,4 @@ private void handleManualState() { runVolts(ShooterHoodState.MANUAL.getRads().getAsDouble() * input); } - - private void setRespectiveShooterHoodState(ShooterHoodState state) { - if (isLeft) RobotState.getInstance().setLeftShooterHoodState(state); - else RobotState.getInstance().setRightShooterHoodState(state); - } - - private ShooterHoodState getRespectiveShooterHoodState() { - return isLeft - ? RobotState.getInstance().getLeftShooterHoodState() - : RobotState.getInstance().getRightShooterHoodState(); - } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOSim.java index b04277e..5b37ea9 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOSim.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOSim.java @@ -29,13 +29,11 @@ public class ShooterHoodIOSim implements ShooterHoodIO { private double appliedVoltage = 0.0; private double setpoint = 0.0; - public ShooterHoodIOSim(boolean isLeft) { + public ShooterHoodIOSim() { sim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - gearbox, - Constants.GeneralShooterHood.SIM_MOI, - Constants.GeneralShooterHood.MOTOR_TO_MECHANISM), + gearbox, Constants.ShooterHood.SIM_MOI, Constants.ShooterHood.MOTOR_TO_MECHANISM), gearbox); } @@ -63,9 +61,7 @@ public void runVolts(double volts) { public void setPosition(double rads) { rads = MathUtil.clamp( - rads, - Constants.GeneralShooterHood.MIN_POSITION_RADS, - Constants.GeneralShooterHood.MAX_POSITION_RADS); + rads, Constants.ShooterHood.MIN_POSITION_RADS, Constants.ShooterHood.MAX_POSITION_RADS); setpoint = rads; sim.setAngle(rads); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java index 19e6a16..6cf5c7a 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java @@ -38,9 +38,7 @@ import edu.wpi.first.wpilibj.DriverStation; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; -import org.team5924.frc2026.Constants.GeneralShooterHood; -import org.team5924.frc2026.Constants.ShooterHoodLeft; -import org.team5924.frc2026.Constants.ShooterHoodRight; +import org.team5924.frc2026.Constants.ShooterHood; import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; @@ -59,36 +57,20 @@ public class ShooterHoodIOTalonFX implements ShooterHoodIO { private final MotionMagicConfigs motionMagicConfigs; private double setpointRads; - /* Gains Left */ - private final LoggedTunableNumber kPLeft = new LoggedTunableNumber("ShooterHood/Left/kP", 250.0); - private final LoggedTunableNumber kILeft = new LoggedTunableNumber("ShooterHood/Left/kI", 0.0); - private final LoggedTunableNumber kDLeft = new LoggedTunableNumber("ShooterHood/Left/kD", 5.0); - private final LoggedTunableNumber kSLeft = new LoggedTunableNumber("ShooterHood/Left/kS", 0.0); - private final LoggedTunableNumber kVLeft = new LoggedTunableNumber("ShooterHood/Left/kV", 0.0); - private final LoggedTunableNumber kALeft = new LoggedTunableNumber("ShooterHood/Left/kA", 0.0); - - private final LoggedTunableNumber motionCruiseVelocityLeft = - new LoggedTunableNumber("ShooterHood/Left/MotionCruiseVelocity", 90.0); - private final LoggedTunableNumber motionAccelerationLeft = - new LoggedTunableNumber("ShooterHood/Left/MotionAcceleration", 900.0); - private final LoggedTunableNumber motionJerkLeft = - new LoggedTunableNumber("ShooterHood/Left/MotionJerk", 0.0); - - /* Gains Right */ - private final LoggedTunableNumber kPRight = - new LoggedTunableNumber("ShooterHood/Right/kP", 250.0); - private final LoggedTunableNumber kIRight = new LoggedTunableNumber("ShooterHood/Right/kI", 0.0); - private final LoggedTunableNumber kDRight = new LoggedTunableNumber("ShooterHood/Right/kD", 5.0); - private final LoggedTunableNumber kSRight = new LoggedTunableNumber("ShooterHood/Right/kS", 0.0); - private final LoggedTunableNumber kVRight = new LoggedTunableNumber("ShooterHood/Right/kV", 0.0); - private final LoggedTunableNumber kARight = new LoggedTunableNumber("ShooterHood/Right/kA", 0.0); - - private final LoggedTunableNumber motionCruiseVelocityRight = - new LoggedTunableNumber("ShooterHood/Right/MotionCruiseVelocity", 90.0); - private final LoggedTunableNumber motionAccelerationRight = - new LoggedTunableNumber("ShooterHood/Right/MotionAcceleration", 900.0); - private final LoggedTunableNumber motionJerkRight = - new LoggedTunableNumber("ShooterHood/Right/MotionJerk", 0.0); + /* Gains */ + private final LoggedTunableNumber kP = new LoggedTunableNumber("ShooterHood/kP", 250.0); + private final LoggedTunableNumber kI = new LoggedTunableNumber("ShooterHood/kI", 0.0); + private final LoggedTunableNumber kD = new LoggedTunableNumber("ShooterHood/kD", 5.0); + private final LoggedTunableNumber kS = new LoggedTunableNumber("ShooterHood/kS", 0.0); + private final LoggedTunableNumber kV = new LoggedTunableNumber("ShooterHood/kV", 0.0); + private final LoggedTunableNumber kA = new LoggedTunableNumber("ShooterHood/kA", 0.0); + + private final LoggedTunableNumber motionCruiseVelocity = + new LoggedTunableNumber("ShooterHood/MotionCruiseVelocity", 90.0); + private final LoggedTunableNumber motionAcceleration = + new LoggedTunableNumber("ShooterHood/MotionAcceleration", 900.0); + private final LoggedTunableNumber motionJerk = + new LoggedTunableNumber("ShooterHood/MotionJerk", 0.0); /* Status Signals */ private final StatusSignal position; @@ -111,24 +93,9 @@ public class ShooterHoodIOTalonFX implements ShooterHoodIO { private final PositionVoltage positionOut; private final MotionMagicTorqueCurrentFOC motionMagicCurrent; - private final Runnable periodicUpdateSlot0; - private final Runnable periodicUpdateMotionMagic; - - private final boolean isLeft; - private final String side; - - public ShooterHoodIOTalonFX(boolean isLeft) { - this.isLeft = isLeft; - side = isLeft ? "Left" : "Right"; - - talon = - new TalonFX( - isLeft ? ShooterHoodLeft.CAN_ID : ShooterHoodRight.CAN_ID, - new CANBus(GeneralShooterHood.BUS)); - cancoder = - new CANcoder( - isLeft ? ShooterHoodLeft.CANCODER_ID : ShooterHoodRight.CANCODER_ID, - new CANBus(GeneralShooterHood.BUS)); + public ShooterHoodIOTalonFX() { + talon = new TalonFX(ShooterHood.CAN_ID, new CANBus(ShooterHood.BUS)); + cancoder = new CANcoder(ShooterHood.CAN_ID, new CANBus(ShooterHood.BUS)); talonConfig = talon.getConfigurator(); @@ -138,42 +105,17 @@ public ShooterHoodIOTalonFX(boolean isLeft) { motionMagicConfigs = new MotionMagicConfigs(); updateMotionMagicConfigs(); - periodicUpdateSlot0 = - () -> { - updateSlot0Configs(); - - StatusCode statusCode = talon.getConfigurator().apply(slot0Configs); - if (!statusCode.isOK()) { - Logger.recordOutput("ShooterHood/" + side + "/UpdateSlot0Report", statusCode); - } - }; - - periodicUpdateMotionMagic = - () -> { - updateMotionMagicConfigs(); - - StatusCode statusCode = talon.getConfigurator().apply(motionMagicConfigs); - if (!statusCode.isOK()) { - Logger.recordOutput("ShooterHood/" + side + "/UpdateStatusCodeReport", statusCode); - } - }; - // Apply Configs StatusCode[] statusArray = new StatusCode[8]; - statusArray[0] = talonConfig.apply(GeneralShooterHood.CONFIG); + statusArray[0] = talonConfig.apply(ShooterHood.CONFIG); statusArray[1] = talonConfig.apply(slot0Configs); statusArray[2] = talonConfig.apply(motionMagicConfigs); statusArray[3] = talonConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); statusArray[4] = talonConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[5] = talonConfig.apply(GeneralShooterHood.SOFTWARE_LIMIT_CONFIGS); - statusArray[6] = - talonConfig.apply( - isLeft ? ShooterHoodLeft.FEEDBACK_CONFIGS : ShooterHoodRight.FEEDBACK_CONFIGS); - statusArray[7] = - cancoder - .getConfigurator() - .apply(isLeft ? ShooterHoodLeft.CANCODER_CONFIGS : ShooterHoodRight.CANCODER_CONFIGS); + statusArray[5] = talonConfig.apply(ShooterHood.SOFTWARE_LIMIT_CONFIGS); + statusArray[6] = talonConfig.apply(ShooterHood.FEEDBACK_CONFIGS); + statusArray[7] = cancoder.getConfigurator().apply(ShooterHood.CANCODER_CONFIGS); boolean isErrorPresent = false; for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; @@ -182,10 +124,10 @@ public ShooterHoodIOTalonFX(boolean isLeft) { Elastic.sendNotification( new Notification( NotificationLevel.WARNING, - side + " Shooter Hood Configs", - "Error in applying " + side + " Shooter Hood configs!")); + "Shooter Hood Configs", + "Error in applying Shooter Hood configs!")); - Logger.recordOutput("ShooterHood/" + side + "/InitConfReport", statusArray); + Logger.recordOutput("ShooterHood/InitConfReport", statusArray); // Get select status signals and set update frequency position = talon.getPosition(); @@ -287,55 +229,51 @@ public void periodicUpdates() { } private void updateSlot0Configs() { - if (isLeft) { - slot0Configs.kP = kPLeft.get(); - slot0Configs.kI = kILeft.get(); - slot0Configs.kD = kDLeft.get(); - slot0Configs.kS = kSLeft.get(); - slot0Configs.kV = kVLeft.get(); - slot0Configs.kA = kALeft.get(); - } else { - slot0Configs.kP = kPRight.get(); - slot0Configs.kI = kIRight.get(); - slot0Configs.kD = kDRight.get(); - slot0Configs.kS = kSRight.get(); - slot0Configs.kV = kVRight.get(); - slot0Configs.kA = kARight.get(); - } + slot0Configs.kP = kP.get(); + slot0Configs.kI = kI.get(); + slot0Configs.kD = kD.get(); + slot0Configs.kS = kS.get(); + slot0Configs.kV = kV.get(); + slot0Configs.kA = kA.get(); } private void updateMotionMagicConfigs() { - if (isLeft) { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationLeft.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityLeft.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkLeft.get(); - } else { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationRight.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityRight.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkRight.get(); - } + motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); + motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); + motionMagicConfigs.MotionMagicJerk = motionJerk.get(); } private void updateLoggedTunableNumbers() { - if (isLeft) { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPLeft, kILeft, kDLeft, kSLeft, kVLeft, kALeft); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationLeft, - motionCruiseVelocityLeft, - motionJerkLeft); - } else { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPRight, kIRight, kDRight, kSRight, kVRight, kARight); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationRight, - motionCruiseVelocityRight, - motionJerkRight); - } + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + updateSlot0Configs(); + + StatusCode statusCode = talon.getConfigurator().apply(slot0Configs); + if (!statusCode.isOK()) { + Logger.recordOutput("ShooterHood/UpdateSlot0Report", statusCode); + } + }, + kP, + kI, + kD, + kS, + kV, + kA); + + LoggedTunableNumber.ifChanged( + hashCode() + 1, + () -> { + updateMotionMagicConfigs(); + + StatusCode statusCode = talon.getConfigurator().apply(motionMagicConfigs); + if (!statusCode.isOK()) { + Logger.recordOutput("ShooterHood/UpdateStatusCodeReport", statusCode); + } + }, + motionAcceleration, + motionCruiseVelocity, + motionJerk); } @Override @@ -366,8 +304,7 @@ public void stop() { } private double clampRads(double rads) { - return MathUtil.clamp( - rads, GeneralShooterHood.MIN_POSITION_RADS, GeneralShooterHood.MAX_POSITION_RADS); + return MathUtil.clamp(rads, ShooterHood.MIN_POSITION_RADS, ShooterHood.MAX_POSITION_RADS); } private double radsToMotorPosition(double rads) { diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java index 9227bfa..955e9e9 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java @@ -19,7 +19,6 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller.VoltageState; import org.team5924.frc2026.util.LoggedTunableNumber; @@ -38,6 +37,7 @@ public enum HopperState implements VoltageState { } private HopperState goalState = HopperState.OFF; + private HopperState currentState = HopperState.OFF; public Hopper(HopperIO io) { super("Hopper", io); @@ -45,7 +45,7 @@ public Hopper(HopperIO io) { public void setGoalState(HopperState goalState) { this.goalState = goalState; - RobotState.getInstance().setHopperState(goalState); + currentState = goalState; } @Override diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java index 6554f00..2d21815 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java @@ -19,7 +19,6 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller.VoltageState; import org.team5924.frc2026.util.LoggedTunableNumber; @@ -36,6 +35,7 @@ public enum IndexerState implements VoltageState { } private IndexerState goalState = IndexerState.OFF; + private IndexerState currentState = IndexerState.OFF; public Indexer(IndexerIO indexerIO) { super("Indexer", indexerIO); @@ -43,7 +43,7 @@ public Indexer(IndexerIO indexerIO) { public void setGoalState(IndexerState goalState) { this.goalState = goalState; - RobotState.getInstance().setIndexerState(goalState); + currentState = goalState; } @Override diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java index 2aba387..2484660 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java @@ -19,7 +19,6 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller.VoltageState; import org.team5924.frc2026.util.LoggedTunableNumber; @@ -38,6 +37,7 @@ public enum IntakeState implements VoltageState { } private IntakeState goalState = IntakeState.OFF; + private IntakeState currentState = IntakeState.OFF; public Intake(IntakeIO io) { super("Intake", io); @@ -45,6 +45,6 @@ public Intake(IntakeIO io) { public void setGoalState(IntakeState goalState) { this.goalState = goalState; - RobotState.getInstance().setIntakeState(goalState); + currentState = goalState; } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/turret/Turret.java b/src/main/java/org/team5924/frc2026/subsystems/turret/Turret.java deleted file mode 100644 index ef32c6a..0000000 --- a/src/main/java/org/team5924/frc2026/subsystems/turret/Turret.java +++ /dev/null @@ -1,213 +0,0 @@ -/* - * Turret.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.turret; - -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.function.DoubleSupplier; -import lombok.Getter; -import lombok.Setter; -import org.littletonrobotics.junction.Logger; -import org.team5924.frc2026.Constants; -import org.team5924.frc2026.FieldState; -import org.team5924.frc2026.RobotState; -import org.team5924.frc2026.util.EqualsUtil; -import org.team5924.frc2026.util.LoggedTunableNumber; - -public class Turret extends SubsystemBase { - - private final TurretIO io; - private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); - - @Setter private double input; - - private final boolean isLeft; - - public enum TurretState { - OFF(() -> 0.0), - MOVING(() -> 0.0), - - // voltage at which the example subsystem motor moves when controlled by the operator - MANUAL(new LoggedTunableNumber("Turret/Volts/OperatorVoltage", 1.0)), - - NINETY(new LoggedTunableNumber("Turret/Volts/Ninety", Math.PI / 2)), - - ZERO(() -> 0.0), - - AUTO(() -> 0.0); - - @Getter private final DoubleSupplier rads; - - TurretState(DoubleSupplier rads) { - this.rads = rads; - } - } - - @Getter private TurretState goalState = TurretState.OFF; - - private final Alert motorDisconnected; - - protected final Alert overheatAlert; - - private double lastStateChange = 0.0; - - private double exceedBoundsDirection; - private boolean shouldContinue; - - private final String side; - - @Setter private double autoInput = 0.0; - - public Turret(TurretIO io, boolean isLeft) { - side = isLeft ? "Left" : "Right"; - this.io = io; - this.goalState = TurretState.OFF; - this.motorDisconnected = - new Alert(side + " Turret Motor Disconnected!", Alert.AlertType.kWarning); - this.isLeft = isLeft; - - overheatAlert = new Alert(side + " Turret motor overheating!", Alert.AlertType.kWarning); - } - - @Override - public void periodic() { - io.periodicUpdates(); - io.updateInputs(inputs); - Logger.processInputs("Turret/" + side, inputs); - - Logger.recordOutput("Turret/" + side + "/GoalState", goalState.toString()); - Logger.recordOutput("Turret/" + side + "/CurrentState", getRespectiveTurretState()); - Logger.recordOutput("Turret/" + side + "/TargetRads", goalState.rads.getAsDouble()); - Logger.recordOutput("Turret/" + side + "/CurrentRads", inputs.positionRads); - Logger.recordOutput("Turret/" + side + "/ExceedBoundsDirection", exceedBoundsDirection); - Logger.recordOutput("Turret/" + side + "/ShouldContinue", shouldContinue); - - handleCurrentState(); - - motorDisconnected.set(!inputs.motorConnected); - - boolean isOverheating = inputs.tempCelsius > Constants.OVERHEAT_THRESHOLD; - overheatAlert.set(isOverheating); - } - - public boolean isAtSetpoint() { - // return RobotState.getTime() - lastStateChange > Constants.GeneralTurret.STATE_TIMEOUT - return EqualsUtil.epsilonEquals( - inputs.setpointRads, inputs.positionRads, Constants.GeneralTurret.EPSILON_RADS); - } - - private void handleCurrentState() { - TurretState currentState = - isLeft - ? RobotState.getInstance().getLeftTurretState() - : RobotState.getInstance().getRightTurretState(); - switch (currentState) { - case MOVING -> { - if (isAtSetpoint() && goalState != TurretState.AUTO) setRespectiveTurretState(goalState); - } - case MANUAL -> handleManualState(); - case OFF -> io.stop(); - case NINETY -> { - io.setPosition(goalState.rads.getAsDouble() * (isLeft ? 1 : -1)); - } - default -> io.setPosition(goalState.rads.getAsDouble()); - } - } - - private void handleManualState() { - if (!goalState.equals(TurretState.MANUAL)) return; - - if (Math.abs(input) <= Constants.JOYSTICK_DEADZONE) { - io.runVolts(0); - return; - } - - tryRunVolts(TurretState.MANUAL.getRads().getAsDouble() * input); - } - - public void tryRunVolts(double volts) { - // if (!(shouldContinue = shouldContinueInDirection(volts, inputs.turretPositionRads))) return; - - io.runVolts(volts); - } - - public boolean shouldContinueInDirection(double volts, double rads) { - double voltDirection = Math.signum(volts); - return (voltDirection != (exceedBoundsDirection = exceedBoundsDirection(rads))); - } - - /** - * @param rads rads - * @return -1 for min bound, 0 for within, 1 for upper bound - */ - public double exceedBoundsDirection(double rads) { - double min = - isLeft ? Constants.TurretLeft.MIN_POSITION_RADS : Constants.TurretRight.MIN_POSITION_RADS; - double max = - isLeft ? Constants.TurretLeft.MAX_POSITION_RADS : Constants.TurretRight.MAX_POSITION_RADS; - if (rads <= min) return -1.0; - if (rads >= max) return 1.0; - return 0.0; - } - - public void setGoalState(TurretState goalState) { - if (this.goalState.equals(goalState)) return; - if (goalState.equals(TurretState.MANUAL) && Math.abs(input) <= Constants.JOYSTICK_DEADZONE) - return; - - if (goalState != TurretState.MOVING) this.goalState = goalState; - switch (goalState) { - case MANUAL: - setRespectiveTurretState(TurretState.MANUAL); - break; - case MOVING: - DriverStation.reportError( - side + " Turret: MOVING is an invalid goal state; it is a transition state!!", null); - break; - case OFF: - setRespectiveTurretState(TurretState.OFF); - io.stop(); - break; - case AUTO: - setRespectiveTurretState(TurretState.MOVING); - io.setPosition(autoInput); - break; - default: - setRespectiveTurretState(TurretState.MOVING); - io.setPosition(goalState.rads.getAsDouble()); - break; - } - - lastStateChange = FieldState.getInstance().getTime(); - } - - private void setRespectiveTurretState(TurretState state) { - if (isLeft) RobotState.getInstance().setLeftTurretState(state); - else RobotState.getInstance().setRightTurretState(state); - } - - private TurretState getRespectiveTurretState() { - return isLeft - ? RobotState.getInstance().getLeftTurretState() - : RobotState.getInstance().getRightTurretState(); - } - - public double getSetpoint() { - return inputs.setpointRads; - } -} diff --git a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIO.java b/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIO.java deleted file mode 100644 index 09f847b..0000000 --- a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIO.java +++ /dev/null @@ -1,79 +0,0 @@ -/* - * TurretIO.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.turret; - -import org.littletonrobotics.junction.AutoLog; - -public interface TurretIO { - @AutoLog - public static class TurretIOInputs { - public boolean motorConnected = true; - public double position = 0.0; - public double positionRads = 0.0; - public double positionCancoder = 0.0; - public double velocityRadsPerSec = 0.0; - public double appliedVoltage = 0.0; - public double supplyCurrentAmps = 0.0; - public double torqueCurrentAmps = 0.0; - public double tempCelsius = 0.0; - - public double motionMagicVelocityTarget = 0.0; - public double motionMagicPositionTarget = 0.0; - - public double setpointRads = 0.0; - public double acceleration = 0.0; - - public boolean cancoderConnected = true; - public double cancoderAbsolutePosition = 0.0; - public double cancoderVelocity = 0.0; - public double cancoderSupplyVoltage = 0.0; - public double cancoderPositionRotations = 0.0; - } - - /** - * Updates the inputs object with the latest data from hardware - * - * @param inputs Inputs to update - */ - public default void updateInputs(TurretIOInputs inputs) {} - - /** Updates that are be called in turret periodic */ - public default void periodicUpdates() {} - - /** - * Sets the turret motor to the specified voltage - * - * @param volts number of volts - */ - public default void runVolts(double volts) {} - - /** - * Sets the turret motor to a specified angle - * - * @param rads target angle - */ - public default void setPosition(double rads) {} - - /** Holds the turret motor at a set position */ - public default void holdPosition(double rads) {} - - /** stops the motor */ - default void stop() {} - - /** Sets the turret position to specified rads from center */ - default void setPositionSetpoint(double radiansFromCenter, double radsPerSecond) {} -} diff --git a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOSim.java deleted file mode 100644 index d8669e2..0000000 --- a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOSim.java +++ /dev/null @@ -1,79 +0,0 @@ -/* - * TurretIOSim.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.turret; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import org.team5924.frc2026.Constants; - -public class TurretIOSim implements TurretIO { - private final DCMotorSim sim; - private final DCMotor gearbox = DCMotor.getKrakenX60Foc(1); - private double appliedVoltage = 0.0; - private double setpoint = 0.0; - private final double minPositionRads; - private final double maxPositionRads; - - public TurretIOSim(boolean isLeft) { - sim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - gearbox, - Constants.GeneralTurret.MOTOR_TO_MECHANISM, - Constants.GeneralTurret.SIM_MOI), - gearbox); - minPositionRads = - isLeft ? Constants.TurretLeft.MIN_POSITION_RADS : Constants.TurretRight.MIN_POSITION_RADS; - maxPositionRads = - isLeft ? Constants.TurretLeft.MAX_POSITION_RADS : Constants.TurretRight.MAX_POSITION_RADS; - } - - @Override - public void updateInputs(TurretIOInputs inputs) { - if (DriverStation.isDisabled()) runVolts(0.0); - - sim.update(Constants.LOOP_PERIODIC_SECONDS); - inputs.motorConnected = true; - inputs.positionRads = sim.getAngularPositionRad(); - inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); - inputs.appliedVoltage = appliedVoltage; - inputs.supplyCurrentAmps = sim.getCurrentDrawAmps(); - inputs.setpointRads = setpoint; - inputs.tempCelsius = 25.0; - } - - @Override - public void runVolts(double volts) { - appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); - sim.setInputVoltage(appliedVoltage); - } - - @Override - public void setPosition(double rads) { - rads = MathUtil.clamp(rads, minPositionRads, maxPositionRads); - setpoint = rads; - sim.setAngle(rads); - } - - @Override - public void stop() { - runVolts(0.0); - } -} diff --git a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOTalonFX.java deleted file mode 100644 index 7595a42..0000000 --- a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOTalonFX.java +++ /dev/null @@ -1,386 +0,0 @@ -/* - * TurretIOTalonFX.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.turret; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfigurator; -import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Temperature; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DriverStation; -import org.littletonrobotics.junction.Logger; -import org.team5924.frc2026.Constants; -import org.team5924.frc2026.Constants.GeneralTurret; -import org.team5924.frc2026.Constants.TurretLeft; -import org.team5924.frc2026.Constants.TurretRight; -import org.team5924.frc2026.util.Elastic; -import org.team5924.frc2026.util.Elastic.Notification; -import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; -import org.team5924.frc2026.util.LoggedTunableNumber; - -public class TurretIOTalonFX implements TurretIO { - /* Hardware */ - private final TalonFX turretTalon; - private final CANcoder turretCANCoder; - - /* Configurators */ - private TalonFXConfigurator turretTalonConfig; - - /* Configs */ - private final Slot0Configs slot0Configs; - private final MotionMagicConfigs motionMagicConfigs; - private double setpointRads; - - /* Gains Left */ - private final LoggedTunableNumber kPLeft = new LoggedTunableNumber("Turret/Left/kP", 25.0); - private final LoggedTunableNumber kILeft = new LoggedTunableNumber("Turret/Left/kI", 0.0); - private final LoggedTunableNumber kDLeft = new LoggedTunableNumber("Turret/Left/kD", 1.00); - private final LoggedTunableNumber kSLeft = new LoggedTunableNumber("Turret/Left/kS", 0.0); - private final LoggedTunableNumber kVLeft = new LoggedTunableNumber("Turret/Left/kV", 0.0); - private final LoggedTunableNumber kALeft = new LoggedTunableNumber("Turret/Left/kA", 0.00); - - private final LoggedTunableNumber motionCruiseVelocityLeft = - new LoggedTunableNumber("Turret/Left/MotionCruiseVelocity", 10.0); - private final LoggedTunableNumber motionAccelerationLeft = - new LoggedTunableNumber("Turret/Left/MotionAcceleration", 20.0); - private final LoggedTunableNumber motionJerkLeft = - new LoggedTunableNumber("Turret/Left/MotionJerk", 0.0); - - /* Gains Right */ - private final LoggedTunableNumber kPRight = new LoggedTunableNumber("Turret/Right/kP", 25.0); - private final LoggedTunableNumber kIRight = new LoggedTunableNumber("Turret/Right/kI", 0.0); - private final LoggedTunableNumber kDRight = new LoggedTunableNumber("Turret/Right/kD", 1.00); - private final LoggedTunableNumber kSRight = new LoggedTunableNumber("Turret/Right/kS", 0.0); - private final LoggedTunableNumber kVRight = new LoggedTunableNumber("Turret/Right/kV", 0.0); - private final LoggedTunableNumber kARight = new LoggedTunableNumber("Turret/Right/kA", 0.00); - - private final LoggedTunableNumber motionCruiseVelocityRight = - new LoggedTunableNumber("Turret/Right/MotionCruiseVelocity", 10.0); - private final LoggedTunableNumber motionAccelerationRight = - new LoggedTunableNumber("Turret/Right/MotionAcceleration", 100.0); - private final LoggedTunableNumber motionJerkRight = - new LoggedTunableNumber("Turret/Right/MotionJerk", 0.0); - - /* Status Signals */ - private final StatusSignal turretPosition; - private final StatusSignal turretVelocity; - private final StatusSignal turretAppliedVoltage; - private final StatusSignal turretSupplyCurrent; - private final StatusSignal turretTorqueCurrent; - private final StatusSignal turretTempCelsius; - - private final StatusSignal cancoderAbsolutePosition; - private final StatusSignal cancoderVelocity; - private final StatusSignal cancoderSupplyVoltage; - private final StatusSignal cancoderPositionRotations; - - private final StatusSignal closedLoopReferenceSlope; - private double prevClosedLoopReferenceSlope = 0.0; - private double prevReferenceSlopeTimestamp = 0.0; - - private final TorqueCurrentFOC currentOut; - private final PositionVoltage positionOut; - private final MotionMagicTorqueCurrentFOC motionMagicCurrent; - - private final double minPositionRads; - private final double maxPositionRads; - - private final Runnable periodicUpdateSlot0; - private final Runnable periodicUpdateMotionMagic; - - private final boolean isLeft; - private final String side; - - public TurretIOTalonFX(boolean isLeft) { - this.isLeft = isLeft; - side = isLeft ? "Left" : "Right"; - minPositionRads = isLeft ? TurretLeft.MIN_POSITION_RADS : TurretRight.MIN_POSITION_RADS; - maxPositionRads = isLeft ? TurretLeft.MAX_POSITION_RADS : TurretRight.MAX_POSITION_RADS; - - turretTalon = - new TalonFX(isLeft ? TurretLeft.CAN_ID : TurretRight.CAN_ID, new CANBus(GeneralTurret.BUS)); - turretCANCoder = - new CANcoder( - isLeft ? TurretLeft.CANCODER_ID : TurretRight.CANCODER_ID, - new CANBus(GeneralTurret.BUS)); - - turretTalonConfig = turretTalon.getConfigurator(); - - slot0Configs = new Slot0Configs(); - updateSlot0Configs(); - - motionMagicConfigs = new MotionMagicConfigs(); - updateMotionMagicConfigs(); - - periodicUpdateSlot0 = - () -> { - updateSlot0Configs(); - - StatusCode statusCode = turretTalonConfig.apply(slot0Configs); - if (!statusCode.isOK()) { - Logger.recordOutput("Turret/" + side + "/UpdateSlot0Report", statusCode); - } - }; - - periodicUpdateMotionMagic = - () -> { - updateMotionMagicConfigs(); - - StatusCode statusCode = turretTalonConfig.apply(motionMagicConfigs); - if (!statusCode.isOK()) { - Logger.recordOutput("Turret/" + side + "/UpdateStatusCodeReport", statusCode); - } - }; - - // Apply Configs - StatusCode[] statusArray = new StatusCode[8]; - - statusArray[0] = turretTalonConfig.apply(GeneralTurret.GENERAL_CONFIG); - statusArray[1] = turretTalonConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); - statusArray[2] = turretTalonConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[3] = - turretTalonConfig.apply( - isLeft ? TurretLeft.SOFTWARE_LIMIT_CONFIGS : TurretRight.SOFTWARE_LIMIT_CONFIGS); - statusArray[4] = - turretTalonConfig.apply( - isLeft ? TurretLeft.FEEDBACK_CONFIGS : TurretRight.FEEDBACK_CONFIGS); - statusArray[5] = turretTalonConfig.apply(motionMagicConfigs); - statusArray[6] = turretTalonConfig.apply(slot0Configs); - statusArray[7] = - turretCANCoder - .getConfigurator() - .apply(isLeft ? TurretLeft.CANCODER_CONFIGS : TurretRight.CANCODER_CONFIGS); - - boolean isErrorPresent = false; - for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; - - if (isErrorPresent) - Elastic.sendNotification( - new Notification( - NotificationLevel.WARNING, - side + " Turret Configs", - "Error in applying " + side + " Turret configs!")); - - Logger.recordOutput("Turret/" + side + "/InitConfReport", statusArray); - - // Get select status signals and set update frequency - turretPosition = turretTalon.getPosition(); - turretVelocity = turretTalon.getVelocity(); - turretAppliedVoltage = turretTalon.getMotorVoltage(); - turretSupplyCurrent = turretTalon.getSupplyCurrent(); - turretTorqueCurrent = turretTalon.getTorqueCurrent(); - turretTempCelsius = turretTalon.getDeviceTemp(); - - cancoderAbsolutePosition = turretCANCoder.getAbsolutePosition(); - cancoderVelocity = turretCANCoder.getVelocity(); - cancoderSupplyVoltage = turretCANCoder.getSupplyVoltage(); - cancoderPositionRotations = turretCANCoder.getPosition(); - - closedLoopReferenceSlope = turretTalon.getClosedLoopReferenceSlope(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, - turretPosition, - turretVelocity, - turretAppliedVoltage, - turretSupplyCurrent, - turretTorqueCurrent, - turretTempCelsius, - cancoderAbsolutePosition, - cancoderVelocity, - cancoderSupplyVoltage, - cancoderPositionRotations, - closedLoopReferenceSlope); - - currentOut = new TorqueCurrentFOC(0.0); - positionOut = new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true).withSlot(0); - motionMagicCurrent = new MotionMagicTorqueCurrentFOC(0.0).withSlot(0); - - BaseStatusSignal.waitForAll(0.5, turretPosition, cancoderAbsolutePosition); - - turretTalon.setPosition(0.0); - turretCANCoder.setPosition(0.0); - } - - @Override - public void updateInputs(TurretIOInputs inputs) { - inputs.motorConnected = - BaseStatusSignal.refreshAll( - turretPosition, - turretVelocity, - turretAppliedVoltage, - turretSupplyCurrent, - turretTorqueCurrent, - turretTempCelsius, - closedLoopReferenceSlope) - .isOK(); - - inputs.cancoderConnected = - BaseStatusSignal.refreshAll( - cancoderAbsolutePosition, - cancoderVelocity, - cancoderSupplyVoltage, - cancoderPositionRotations) - .isOK(); - - inputs.position = - BaseStatusSignal.getLatencyCompensatedValueAsDouble(turretPosition, turretVelocity); - inputs.positionRads = Units.rotationsToRadians(inputs.position); - - inputs.velocityRadsPerSec = Units.rotationsToRadians(turretVelocity.getValueAsDouble()); - inputs.appliedVoltage = turretAppliedVoltage.getValueAsDouble(); - inputs.supplyCurrentAmps = turretSupplyCurrent.getValueAsDouble(); - inputs.torqueCurrentAmps = turretTorqueCurrent.getValueAsDouble(); - inputs.tempCelsius = turretTempCelsius.getValueAsDouble(); - - inputs.motionMagicVelocityTarget = - motorPositionToRads(turretTalon.getClosedLoopReferenceSlope().getValueAsDouble()); - inputs.motionMagicPositionTarget = - motorPositionToRads(turretTalon.getClosedLoopReference().getValueAsDouble()); - - inputs.setpointRads = setpointRads; - - double currentTime = closedLoopReferenceSlope.getTimestamp().getTime(); - double timeDiff = currentTime - prevReferenceSlopeTimestamp; - if (timeDiff > 0.0) { - inputs.acceleration = - (inputs.motionMagicVelocityTarget - prevClosedLoopReferenceSlope) / timeDiff; - } - prevClosedLoopReferenceSlope = inputs.motionMagicVelocityTarget; - prevReferenceSlopeTimestamp = currentTime; - - inputs.cancoderAbsolutePosition = cancoderAbsolutePosition.getValueAsDouble(); - inputs.cancoderVelocity = cancoderVelocity.getValueAsDouble(); - inputs.cancoderSupplyVoltage = cancoderSupplyVoltage.getValueAsDouble(); - inputs.cancoderPositionRotations = cancoderPositionRotations.getValueAsDouble(); - - inputs.positionCancoder = inputs.cancoderPositionRotations; - } - - @Override - public void periodicUpdates() { - updateLoggedTunableNumbers(); - } - - private void updateSlot0Configs() { - if (isLeft) { - slot0Configs.kP = kPLeft.get(); - slot0Configs.kI = kILeft.get(); - slot0Configs.kD = kDLeft.get(); - slot0Configs.kS = kSLeft.get(); - slot0Configs.kV = kVLeft.get(); - slot0Configs.kA = kALeft.get(); - } else { - slot0Configs.kP = kPRight.get(); - slot0Configs.kI = kIRight.get(); - slot0Configs.kD = kDRight.get(); - slot0Configs.kS = kSRight.get(); - slot0Configs.kV = kVRight.get(); - slot0Configs.kA = kARight.get(); - } - } - - private void updateMotionMagicConfigs() { - if (isLeft) { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationLeft.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityLeft.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkLeft.get(); - } else { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationRight.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityRight.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkRight.get(); - } - } - - private void updateLoggedTunableNumbers() { - if (isLeft) { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPLeft, kILeft, kDLeft, kSLeft, kVLeft, kALeft); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationLeft, - motionCruiseVelocityLeft, - motionJerkLeft); - } else { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPRight, kIRight, kDRight, kSRight, kVRight, kARight); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationRight, - motionCruiseVelocityRight, - motionJerkRight); - } - } - - @Override - public void runVolts(double volts) { - turretTalon.setControl(currentOut.withOutput(40 * volts)); - } - - @Override - public void setPosition(double rads) { - if (!DriverStation.isEnabled()) { - stop(); - return; - } - - setpointRads = clampRads(rads); - turretTalon.setControl(motionMagicCurrent.withPosition(radsToMotorPosition(setpointRads))); - } - - @Override - public void holdPosition(double rads) { - turretTalon.setControl(positionOut.withPosition(radsToMotorPosition(rads))); - } - - @Override - public void stop() { - turretTalon.stopMotor(); - } - - private double clampRads(double rads) { - return MathUtil.clamp(rads, minPositionRads, maxPositionRads); - } - - private double radsToMotorPosition(double rads) { - while (rads < minPositionRads) rads += 2 * Math.PI; - while (rads > maxPositionRads) rads -= 2 * Math.PI; - - if (rads < minPositionRads) turretPosition.getValueAsDouble(); - return Units.radiansToRotations(rads); - } - - private double motorPositionToRads(double motorPosition) { - return Units.rotationsToRadians(motorPosition); - } -} diff --git a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java index c3385a3..9bace1f 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java @@ -35,16 +35,16 @@ public class VisionConstants { public static final Transform3d FRONT_LEFT_TRANSFORM = new Transform3d( new Translation3d( - Units.inchesToMeters(10.402), - Units.inchesToMeters(12.042), - Units.inchesToMeters(8.401)), + Units.inchesToMeters(10.402), + Units.inchesToMeters(12.042), + Units.inchesToMeters(8.401)), new Rotation3d(0.0, Units.degreesToRadians(55.0), Units.degreesToRadians(-15.0))); public static final Transform3d FRONT_RIGHT_TRANSFORM = new Transform3d( new Translation3d( - Units.inchesToMeters(10.402), - Units.inchesToMeters(-12.042), - Units.inchesToMeters(8.711)), + Units.inchesToMeters(10.402), + Units.inchesToMeters(-12.042), + Units.inchesToMeters(8.711)), new Rotation3d(0.0, Units.degreesToRadians(55.0), Units.degreesToRadians(15.0))); // public static final ArrayList IGNORE_IDS = diff --git a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java index 91c2452..a1851dd 100644 --- a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java +++ b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java @@ -22,7 +22,6 @@ import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; @@ -65,7 +64,6 @@ public record LaunchingParameters( double hoodAngle, double hoodVelocity, double flywheelSpeed, - double turretRadians, double distance, double distanceNoLookahead, double timeOfFlight, @@ -111,13 +109,13 @@ public record LaunchingParameters( new LaunchPreset( new LoggedTunableNumber( "LaunchCalculator/Presets/HoodMin/HoodAngle", - Units.radiansToDegrees(Constants.GeneralShooterHood.MIN_POSITION_RADS)), + Units.radiansToDegrees(Constants.ShooterHood.MIN_POSITION_RADS)), new LoggedTunableNumber("LaunchCalculator/Presets/HoodMin/FlywheelSpeed", 50)); public static final LaunchPreset hoodMaxPreset = new LaunchPreset( new LoggedTunableNumber( "LaunchCalculator/Presets/HoodMax/HoodAngle", - Units.radiansToDegrees(Constants.GeneralShooterHood.MAX_POSITION_RADS)), + Units.radiansToDegrees(Constants.ShooterHood.MAX_POSITION_RADS)), new LoggedTunableNumber("LaunchCalculator/Presets/HoodMax/FlywheelSpeed", 50)); public static final LoggedTunableNumber passingIdleSpeed = @@ -273,8 +271,7 @@ public static double getMaxTimeOfFlight() { return timeOfFlightMap.get(maxDistance); } - public LaunchingParameters getParameters(boolean isLeft) { - Transform3d robotToLauncher = isLeft ? robotToLauncherLeft : robotToLauncherRight; + public LaunchingParameters getParameters() { boolean passing = AllianceFlipUtil.applyX(RobotState.getInstance().getEstimatedPose().getX()) > FieldConstants.LinesVertical.hubCenter; @@ -331,12 +328,10 @@ public LaunchingParameters getParameters(boolean isLeft) { lookaheadLauncherToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); } - double turretRads = getTurretAngle(lookaheadPose, target, isLeft).getRadians(); - // Account for launcher being off center Pose2d lookaheadRobotPose = lookaheadPose.transformBy(robotToLauncher.toTransform2d().inverse()); - Rotation2d driveAngle = getDriveAngleWithLauncherOffset(lookaheadRobotPose, target, isLeft); + Rotation2d driveAngle = getDriveAngleWithLauncherOffset(lookaheadRobotPose, target); // Calculate remaining parameters double hoodAngle = @@ -377,7 +372,6 @@ public LaunchingParameters getParameters(boolean isLeft) { hoodAngle + Units.degreesToRadians(hoodAngleOffsetDeg), hoodVelocity, flywheelVelocity, - turretRads, lookaheadLauncherToTargetDistance, launcherToTargetDistance, timeOfFlight, @@ -392,18 +386,8 @@ public LaunchingParameters getParameters(boolean isLeft) { return latestParameters; } - private static Rotation2d getTurretAngle( - Pose2d turretPose, Translation2d target, boolean isLeft) { - - Rotation2d turretToHub = target.minus(turretPose.getTranslation()).getAngle(); - Rotation2d turretRads = turretPose.getRotation().minus(turretToHub); - return turretRads; - } - private static Rotation2d getDriveAngleWithLauncherOffset( - Pose2d robotPose, Translation2d target, boolean isLeft) { - Transform3d robotToLauncher = isLeft ? robotToLauncherLeft : robotToLauncherRight; - + Pose2d robotPose, Translation2d target) { Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle(); Rotation2d hubAngle = new Rotation2d( @@ -461,8 +445,7 @@ public Translation2d getPassingTarget() { * @param forceBlue Always use the blue hub target * @return The target pose for the aimed robot. */ - public static Pose2d getStationaryAimedPose( - Translation2d robotTranslation, boolean forceBlue, boolean isLeft) { + public static Pose2d getStationaryAimedPose(Translation2d robotTranslation, boolean forceBlue) { // Calculate target Translation2d target = FieldConstants.Hub.topCenterPoint.toTranslation2d(); if (!forceBlue) { @@ -470,8 +453,7 @@ public static Pose2d getStationaryAimedPose( } return new Pose2d( - robotTranslation, - getDriveAngleWithLauncherOffset(robotTranslation.toPose2d(), target, isLeft)); + robotTranslation, getDriveAngleWithLauncherOffset(robotTranslation.toPose2d(), target)); } /** Adjusts the hood angle offset up or down the specified amount. */ diff --git a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java index 4879bf4..7a1b170 100644 --- a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java +++ b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java @@ -18,16 +18,12 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import org.team5924.frc2026.Constants; +import edu.wpi.first.math.geometry.Translation3d; public class LauncherConstants { - public static Transform3d robotToLauncherLeft = + public static Transform3d robotToLauncher = new Transform3d( - Constants.TurretLeft.ROBOT_TO_TURRET, new Rotation3d(0.0, 0.0, Math.PI)); // TODO: update - - public static Transform3d robotToLauncherRight = - new Transform3d( - Constants.TurretRight.ROBOT_TO_TURRET, new Rotation3d(0.0, 0.0, Math.PI)); // TODO: update + new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, Math.PI)); // TODO: update private LauncherConstants() {} } From 31f7584c790468275998992c58a68d8460967de8 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 24 Mar 2026 18:00:42 -0700 Subject: [PATCH 09/24] update flywheel to have 4 motors --- .../java/org/team5924/frc2026/Constants.java | 19 +++++++++++---- .../frc2026/subsystems/flywheel/Flywheel.java | 24 +++---------------- .../flywheel/FlywheelIOTalonFX.java | 18 +++++++++----- .../shooterHood/ShooterHoodIOTalonFX.java | 2 +- 4 files changed, 31 insertions(+), 32 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index a62cd52..dd0c3d7 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -174,7 +174,7 @@ public final class IntakePivot { .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) .withSensorToMechanismRatio(MOTOR_TO_MECHANISM) .withRotorToSensorRatio(1.0); - } + } public final class Hopper { public static final int CAN_ID = 50; @@ -190,6 +190,7 @@ public final class Hopper { public final class Indexer { public final static int CAN_ID = 51; public static final String BUS = "rio"; + // controls two rollers, so reduction is weird public static final double MOTOR_TO_MECHANISM = 36.0 / 16.0; public static final double SIM_MOI = 0.001; @@ -203,7 +204,6 @@ public final class Indexer { .withInverted(InvertedValue.Clockwise_Positive)); } - /* General Subsystems */ public final class ShooterHood { public static final String BUS = "rio"; @@ -275,6 +275,9 @@ public final class Flywheel { public static final int BEAM_BREAK_ID = 0; // TODO: update later public static final int FOLLOWER_CAN_ID = 31; + public static final int OPPOSER_ONE_CAN_ID = 32; + public static final int OPPOSER_TWO_CAN_ID = 33; + public static final double FOLLOWER_SIM_MOI = 0.001; public static final double EPSILON_VELOCITY = 2.5; @@ -293,8 +296,16 @@ public final class Flywheel { .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast)); - public static final TalonFXConfiguration FOLLOWER_CONFIG = - CONFIG + public static final TalonFXConfiguration OPPOSER_CONFIG = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(60) + .withStatorCurrentLimit(60)) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast)) .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive)); diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java index 1599f4e..a8f62ca 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java @@ -43,11 +43,9 @@ public enum FlywheelState { FAST_LAUNCH(new LoggedTunableNumber("Flywheel/FastLaunch", 150)), SLOW_LAUNCH(new LoggedTunableNumber("Flywheel/SlowLaunch", 50)), - // current at which the example subsystem motor moves when controlled by the operator - MANUAL(new LoggedTunableNumber("Flywheel/OperatorCurrent", 200)), AUTO(() -> 0.0), - SETPOINT(() -> 0.0), + MANUAL_SETPOINT(() -> 0.0), B4(() -> 4.0), B6(() -> 6.0), @@ -111,12 +109,8 @@ public void stop() { public void setGoalState(FlywheelState goalState) { if (this.goalState.equals(goalState)) return; - if (goalState.equals(FlywheelState.MANUAL) && Math.abs(input) <= Constants.JOYSTICK_DEADZONE) - return; - this.goalState = goalState; switch (goalState) { - case MANUAL -> currentState = FlywheelState.MANUAL; case MOVING -> DriverStation.reportError( "Flywheel: MOVING is an invalid goal state; it is a transition state!!", null); @@ -135,7 +129,7 @@ public boolean isAtSetpoint() { } private double getTargetVelocityRotationsPerSec() { - return goalState == FlywheelState.AUTO || goalState == FlywheelState.SETPOINT + return goalState == FlywheelState.AUTO || goalState == FlywheelState.MANUAL_SETPOINT ? autoInput : goalState.velocityRotationsPerSec.getAsDouble(); } @@ -148,7 +142,6 @@ private void handleCurrentState() { setVelocity(getTargetVelocityRotationsPerSec()); if (isAtSetpoint() && goalState != FlywheelState.AUTO) currentState = goalState; } - case MANUAL -> handleManualState(); case OFF -> stop(); case B4, B6, B8, B12 -> runVolts(getTargetVelocityRotationsPerSec()); case AUTO -> setVelocity(autoInput); @@ -156,19 +149,8 @@ private void handleCurrentState() { } } - private void handleManualState() { - if (!goalState.equals(FlywheelState.MANUAL)) return; - - if (Math.abs(input) <= Constants.JOYSTICK_DEADZONE) { - stop(); - return; - } - - setVelocity(FlywheelState.MANUAL.getVelocityRotationsPerSec().getAsDouble() * input); - } - public void updateSetpointState(double change) { autoInput = inputs.setpointVelocityRotationsPerSec + change; - goalState = FlywheelState.SETPOINT; + setGoalState(FlywheelState.MANUAL_SETPOINT); } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java index 4568a83..d2e131c 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java @@ -46,10 +46,14 @@ public class FlywheelIOTalonFX implements FlywheelIO { /* Hardware */ private final TalonFX leaderTalon; private final TalonFX followerTalon; + private final TalonFX opposerOneTalon; + private final TalonFX opposerTwoTalon; /* Configurators */ private TalonFXConfigurator leaderConfig; private TalonFXConfigurator followerConfig; + private TalonFXConfigurator opposerOneConfig; + private TalonFXConfigurator opposerTwoConfig; /* Configs */ private final Slot0Configs slot0Configs; @@ -88,11 +92,14 @@ public class FlywheelIOTalonFX implements FlywheelIO { public FlywheelIOTalonFX() { leaderTalon = new TalonFX(Flywheel.CAN_ID, new CANBus(Flywheel.BUS)); - followerTalon = new TalonFX(Flywheel.FOLLOWER_CAN_ID, new CANBus(Flywheel.BUS)); + opposerOneTalon = new TalonFX(Flywheel.OPPOSER_ONE_CAN_ID, new CANBus(Flywheel.BUS)); + opposerTwoTalon = new TalonFX(Flywheel.OPPOSER_TWO_CAN_ID, new CANBus(Flywheel.BUS)); leaderConfig = leaderTalon.getConfigurator(); followerConfig = followerTalon.getConfigurator(); + opposerOneConfig = opposerOneTalon.getConfigurator(); + opposerTwoConfig = opposerTwoTalon.getConfigurator(); slot0Configs = new Slot0Configs(); updateSlot0Configs(); @@ -101,7 +108,7 @@ public FlywheelIOTalonFX() { updateMotionMagicConfigs(); // Apply Configs - StatusCode[] statusArray = new StatusCode[10]; + StatusCode[] statusArray = new StatusCode[9]; statusArray[0] = leaderConfig.apply(Flywheel.CONFIG); statusArray[1] = leaderConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); @@ -110,10 +117,9 @@ public FlywheelIOTalonFX() { statusArray[4] = leaderConfig.apply(slot0Configs); statusArray[5] = leaderConfig.apply(motionMagicConfigs); - statusArray[6] = followerConfig.apply(Flywheel.FOLLOWER_CONFIG); - statusArray[7] = followerConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); - statusArray[8] = followerConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[9] = followerConfig.apply(Flywheel.FEEDBACK_CONFIGS); + statusArray[6] = followerConfig.apply(Flywheel.CONFIG); + statusArray[7] = opposerOneConfig.apply(Flywheel.OPPOSER_CONFIG); + statusArray[8] = opposerTwoConfig.apply(Flywheel.OPPOSER_CONFIG); boolean isErrorPresent = false; for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java index 6cf5c7a..0d8cf6d 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java @@ -95,7 +95,7 @@ public class ShooterHoodIOTalonFX implements ShooterHoodIO { public ShooterHoodIOTalonFX() { talon = new TalonFX(ShooterHood.CAN_ID, new CANBus(ShooterHood.BUS)); - cancoder = new CANcoder(ShooterHood.CAN_ID, new CANBus(ShooterHood.BUS)); + cancoder = new CANcoder(ShooterHood.CANCODER_ID, new CANBus(ShooterHood.BUS)); talonConfig = talon.getConfigurator(); From 32a6e2e98df05893df4f1f52f3f967796e65611e Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 24 Mar 2026 18:20:33 -0700 Subject: [PATCH 10/24] update launcher position (roughly accurate) --- .../java/org/team5924/frc2026/util/LauncherConstants.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java index 7a1b170..9d7d3da 100644 --- a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java +++ b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java @@ -19,11 +19,17 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; public class LauncherConstants { + // TODO: update to be more accurate public static Transform3d robotToLauncher = new Transform3d( - new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, Math.PI)); // TODO: update + new Translation3d( + Units.inchesToMeters(5.764), + Units.inchesToMeters(0.0), + Units.inchesToMeters(21.203)), + new Rotation3d(0.0, 0.0, Math.PI)); private LauncherConstants() {} } From b42598b9d9d90ccc0163da24bb8854c26c065f8b Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 24 Mar 2026 18:22:22 -0700 Subject: [PATCH 11/24] make TalonFXConfigurators final, reformatting --- .../frc2026/subsystems/flywheel/FlywheelIOTalonFX.java | 8 ++++---- .../pivots/intakePivot/IntakePivotIOTalonFX.java | 2 +- .../pivots/shooterHood/ShooterHoodIOTalonFX.java | 2 +- .../java/org/team5924/frc2026/util/LauncherConstants.java | 6 ++---- 4 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java index d2e131c..e191f3e 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java @@ -50,10 +50,10 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final TalonFX opposerTwoTalon; /* Configurators */ - private TalonFXConfigurator leaderConfig; - private TalonFXConfigurator followerConfig; - private TalonFXConfigurator opposerOneConfig; - private TalonFXConfigurator opposerTwoConfig; + private final TalonFXConfigurator leaderConfig; + private final TalonFXConfigurator followerConfig; + private final TalonFXConfigurator opposerOneConfig; + private final TalonFXConfigurator opposerTwoConfig; /* Configs */ private final Slot0Configs slot0Configs; diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java index 66dda21..dbbbc9c 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java @@ -49,7 +49,7 @@ public class IntakePivotIOTalonFX implements IntakePivotIO { private final TalonFX talon; /* Configurators */ - private TalonFXConfigurator talonConfig; + private final TalonFXConfigurator talonConfig; /* Configs */ private final Slot0Configs slot0Configs; diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java index 0d8cf6d..2487eff 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java @@ -50,7 +50,7 @@ public class ShooterHoodIOTalonFX implements ShooterHoodIO { private final CANcoder cancoder; /* Configurators */ - private TalonFXConfigurator talonConfig; + private final TalonFXConfigurator talonConfig; /* Configs */ private final Slot0Configs slot0Configs; diff --git a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java index 9d7d3da..08e2756 100644 --- a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java +++ b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java @@ -22,13 +22,11 @@ import edu.wpi.first.math.util.Units; public class LauncherConstants { - // TODO: update to be more accurate + // TODO: update to be more accurate public static Transform3d robotToLauncher = new Transform3d( new Translation3d( - Units.inchesToMeters(5.764), - Units.inchesToMeters(0.0), - Units.inchesToMeters(21.203)), + Units.inchesToMeters(5.764), Units.inchesToMeters(0.0), Units.inchesToMeters(21.203)), new Rotation3d(0.0, 0.0, Math.PI)); private LauncherConstants() {} From 043dd670d7db120851217eb43d135eb171ce6d48 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 24 Mar 2026 18:46:08 -0700 Subject: [PATCH 12/24] remove beam break constants --- src/main/java/org/team5924/frc2026/Constants.java | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index dd0c3d7..c951929 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -182,8 +182,6 @@ public final class Hopper { public static final double MOTOR_TO_MECHANISM = (16.0 / 12.0) * (24.0 / 16.0); public static final double SIM_MOI = 0.001; - public static final int BEAM_BREAK_ID = 0; - public static final TalonFXConfiguration CONFIG = GenericRoller.CONFIG; } @@ -195,8 +193,6 @@ public final class Indexer { public static final double MOTOR_TO_MECHANISM = 36.0 / 16.0; public static final double SIM_MOI = 0.001; - public final static int BEAM_BREAK_ID = 0; - public static final TalonFXConfiguration CONFIG = GenericRoller.CONFIG .withMotorOutput( @@ -272,7 +268,6 @@ public final class ShooterHood { public final class Flywheel { public static final int CAN_ID = 30; - public static final int BEAM_BREAK_ID = 0; // TODO: update later public static final int FOLLOWER_CAN_ID = 31; public static final int OPPOSER_ONE_CAN_ID = 32; From beae2f13e41771ae0fbeabca04b8e49cffb7d001 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 24 Mar 2026 19:26:46 -0700 Subject: [PATCH 13/24] add disconnection + temp logging and alerts for flywheel --- .../frc2026/subsystems/flywheel/Flywheel.java | 26 ++++++++++++++----- .../subsystems/flywheel/FlywheelIO.java | 6 ++--- .../subsystems/flywheel/FlywheelIOSim.java | 7 +++-- .../flywheel/FlywheelIOTalonFX.java | 26 ++++++++++++++----- 4 files changed, 46 insertions(+), 19 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java index a8f62ca..9df475e 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java @@ -56,8 +56,8 @@ public enum FlywheelState { private final DoubleSupplier velocityRotationsPerSec; } - private final Alert flywheelMotorDisconnected; - protected final Alert overheatAlert; + private final Alert[] motorDisconnected = new Alert[4]; + protected final Alert[] overheatAlert = new Alert[4]; @Getter private FlywheelState goalState = FlywheelState.OFF; private FlywheelState currentState = FlywheelState.OFF; @@ -68,10 +68,20 @@ public enum FlywheelState { public Flywheel(FlywheelIO io) { this.io = io; this.goalState = FlywheelState.OFF; - this.flywheelMotorDisconnected = - new Alert("Flywheel Motor Disconnected!", Alert.AlertType.kWarning); - overheatAlert = new Alert("Flywheel motor overheating!", Alert.AlertType.kWarning); + for (int i = 0; i < 4; ++i) { + int id = switch (i) { + case 0 -> Constants.Flywheel.CAN_ID; + case 1 -> Constants.Flywheel.FOLLOWER_CAN_ID; + case 2 -> Constants.Flywheel.OPPOSER_ONE_CAN_ID; + case 3 -> Constants.Flywheel.OPPOSER_TWO_CAN_ID; + default -> -1; + }; + + motorDisconnected[i] = + new Alert("Flywheel Motor (id " + id + ") Disconnected!", Alert.AlertType.kWarning); + overheatAlert[i] = new Alert("Flywheel motor (id " + id + ") overheating!", Alert.AlertType.kWarning); + } } @Override @@ -87,10 +97,12 @@ public void periodic() { Logger.recordOutput("Flywheel/CurrentVelocityRotationsPerSec", inputs.velocityRotationsPerSec); Logger.recordOutput("Flywheel/IsAtSetpoint", isAtSetpoint); - flywheelMotorDisconnected.set(!inputs.motorConnected); + for (int i = 0; i < 4; ++i) { + motorDisconnected[i].set(!inputs.motorConnected[i]); + overheatAlert[i].set(inputs.tempCelsius[i] > Constants.OVERHEAT_THRESHOLD); + } handleCurrentState(); - overheatAlert.set(inputs.tempCelsius > Constants.OVERHEAT_THRESHOLD); } public void runVolts(double volts) { diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIO.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIO.java index ec28f0c..253acc3 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIO.java @@ -21,17 +21,15 @@ public interface FlywheelIO { @AutoLog public static class FlywheelIOInputs { - public boolean motorConnected = true; public double position = 0.0; public double positionRads = 0.0; public double velocityRotationsPerSec = 0.0; public double appliedVoltage = 0.0; public double supplyCurrentAmps = 0.0; public double torqueCurrentAmps = 0.0; - public double tempCelsius = 0.0; - public double followerSupplyCurrentAmps; - public double followerTempCelsius; + public boolean[] motorConnected = {true, true, true, true}; + public double[] tempCelsius = new double[4]; public double setpointVelocityRotationsPerSec; diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java index c3fa25d..3498476 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java @@ -43,13 +43,16 @@ public void updateInputs(FlywheelIOInputs inputs) { if (DriverStation.isDisabled()) stop(); sim.update(Constants.LOOP_PERIODIC_SECONDS); - inputs.motorConnected = true; inputs.positionRads = sim.getAngularPositionRad(); inputs.velocityRotationsPerSec = Units.radiansToRotations(sim.getAngularVelocityRadPerSec()); inputs.appliedVoltage = appliedVoltage; inputs.supplyCurrentAmps = sim.getCurrentDrawAmps(); inputs.setpointVelocityRotationsPerSec = setpoint; - inputs.tempCelsius = 25.0; + + for (int i = 0; i < 4; ++i) { + inputs.motorConnected[i] = true; + inputs.tempCelsius[i] = 25.0; + } } @Override diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java index e191f3e..c6bacb5 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java @@ -34,6 +34,9 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; + +import java.util.ArrayList; + import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; import org.team5924.frc2026.Constants.Flywheel; @@ -81,7 +84,7 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final StatusSignal appliedVoltage; private final StatusSignal supplyCurrent; private final StatusSignal torqueCurrent; - private final StatusSignal tempCelsius; + private final ArrayList> tempCelsius = new ArrayList>(4); private final StatusSignal closedLoopReferenceSlope; private double prevClosedLoopReferenceSlope = 0.0; @@ -139,12 +142,16 @@ public FlywheelIOTalonFX() { appliedVoltage = leaderTalon.getMotorVoltage(); supplyCurrent = leaderTalon.getSupplyCurrent(); torqueCurrent = leaderTalon.getTorqueCurrent(); - tempCelsius = leaderTalon.getDeviceTemp(); + + tempCelsius.set(0, leaderTalon.getDeviceTemp()); + tempCelsius.set(1, followerTalon.getDeviceTemp()); + tempCelsius.set(2, opposerOneTalon.getDeviceTemp()); + tempCelsius.set(3, opposerTwoTalon.getDeviceTemp()); closedLoopReferenceSlope = leaderTalon.getClosedLoopReferenceSlope(); BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, position, velocity, appliedVoltage, supplyCurrent, torqueCurrent, tempCelsius); + 100.0, position, velocity, appliedVoltage, supplyCurrent, torqueCurrent, tempCelsius.get(0), tempCelsius.get(1), tempCelsius.get(2), tempCelsius.get(3)); voltageOut = new VoltageOut(0.0).withEnableFOC(true); motionMagicVelocity = new MotionMagicVelocityVoltage(0.0).withEnableFOC(true).withSlot(0); @@ -154,16 +161,20 @@ public FlywheelIOTalonFX() { @Override public void updateInputs(FlywheelIOInputs inputs) { - inputs.motorConnected = + inputs.motorConnected[0] = BaseStatusSignal.refreshAll( position, velocity, appliedVoltage, supplyCurrent, torqueCurrent, - tempCelsius, + tempCelsius.get(0), closedLoopReferenceSlope) .isOK(); + + for (int i = 1; i < 4; ++i) { + inputs.motorConnected[i] = BaseStatusSignal.refreshAll(tempCelsius.get(i)).isOK(); + } inputs.position = BaseStatusSignal.getLatencyCompensatedValueAsDouble(position, velocity); inputs.positionRads = Units.rotationsToRadians(inputs.position); @@ -172,7 +183,10 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble(); inputs.torqueCurrentAmps = torqueCurrent.getValueAsDouble(); - inputs.tempCelsius = tempCelsius.getValueAsDouble(); + + for (int i = 0; i < 4; ++i) { + inputs.tempCelsius[i] = tempCelsius.get(i).getValueAsDouble(); + } inputs.motionMagicVelocityTarget = motorPositionToRads(leaderTalon.getClosedLoopReferenceSlope().getValueAsDouble()); From 336f771a151be361956781acbacc4e0e3d9be217 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 24 Mar 2026 20:07:38 -0700 Subject: [PATCH 14/24] alert rename --- .../frc2026/subsystems/flywheel/Flywheel.java | 20 ++++++++++--------- .../subsystems/flywheel/FlywheelIOSim.java | 2 +- .../flywheel/FlywheelIOTalonFX.java | 20 +++++++++++++------ .../pivots/intakePivot/IntakePivot.java | 6 +++--- 4 files changed, 29 insertions(+), 19 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java index 9df475e..3951444 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java @@ -70,17 +70,19 @@ public Flywheel(FlywheelIO io) { this.goalState = FlywheelState.OFF; for (int i = 0; i < 4; ++i) { - int id = switch (i) { - case 0 -> Constants.Flywheel.CAN_ID; - case 1 -> Constants.Flywheel.FOLLOWER_CAN_ID; - case 2 -> Constants.Flywheel.OPPOSER_ONE_CAN_ID; - case 3 -> Constants.Flywheel.OPPOSER_TWO_CAN_ID; - default -> -1; - }; + int id = + switch (i) { + case 0 -> Constants.Flywheel.CAN_ID; + case 1 -> Constants.Flywheel.FOLLOWER_CAN_ID; + case 2 -> Constants.Flywheel.OPPOSER_ONE_CAN_ID; + case 3 -> Constants.Flywheel.OPPOSER_TWO_CAN_ID; + default -> -1; + }; motorDisconnected[i] = - new Alert("Flywheel Motor (id " + id + ") Disconnected!", Alert.AlertType.kWarning); - overheatAlert[i] = new Alert("Flywheel motor (id " + id + ") overheating!", Alert.AlertType.kWarning); + new Alert("Flywheel Motor (id " + id + ") Disconnected!", Alert.AlertType.kWarning); + overheatAlert[i] = + new Alert("Flywheel motor (id " + id + ") overheating!", Alert.AlertType.kWarning); } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java index 3498476..43c323b 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java @@ -48,7 +48,7 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.appliedVoltage = appliedVoltage; inputs.supplyCurrentAmps = sim.getCurrentDrawAmps(); inputs.setpointVelocityRotationsPerSec = setpoint; - + for (int i = 0; i < 4; ++i) { inputs.motorConnected[i] = true; inputs.tempCelsius[i] = 25.0; diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java index c6bacb5..f194489 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java @@ -34,9 +34,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; - import java.util.ArrayList; - import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; import org.team5924.frc2026.Constants.Flywheel; @@ -84,7 +82,8 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final StatusSignal appliedVoltage; private final StatusSignal supplyCurrent; private final StatusSignal torqueCurrent; - private final ArrayList> tempCelsius = new ArrayList>(4); + private final ArrayList> tempCelsius = + new ArrayList>(4); private final StatusSignal closedLoopReferenceSlope; private double prevClosedLoopReferenceSlope = 0.0; @@ -151,7 +150,16 @@ public FlywheelIOTalonFX() { closedLoopReferenceSlope = leaderTalon.getClosedLoopReferenceSlope(); BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, position, velocity, appliedVoltage, supplyCurrent, torqueCurrent, tempCelsius.get(0), tempCelsius.get(1), tempCelsius.get(2), tempCelsius.get(3)); + 100.0, + position, + velocity, + appliedVoltage, + supplyCurrent, + torqueCurrent, + tempCelsius.get(0), + tempCelsius.get(1), + tempCelsius.get(2), + tempCelsius.get(3)); voltageOut = new VoltageOut(0.0).withEnableFOC(true); motionMagicVelocity = new MotionMagicVelocityVoltage(0.0).withEnableFOC(true).withSlot(0); @@ -171,7 +179,7 @@ public void updateInputs(FlywheelIOInputs inputs) { tempCelsius.get(0), closedLoopReferenceSlope) .isOK(); - + for (int i = 1; i < 4; ++i) { inputs.motorConnected[i] = BaseStatusSignal.refreshAll(tempCelsius.get(i)).isOK(); } @@ -183,7 +191,7 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble(); inputs.torqueCurrentAmps = torqueCurrent.getValueAsDouble(); - + for (int i = 0; i < 4; ++i) { inputs.tempCelsius[i] = tempCelsius.get(i).getValueAsDouble(); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java index aff7f93..6ab492c 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java @@ -54,7 +54,7 @@ public enum IntakePivotState { private final DoubleSupplier rads; } - private final Alert intakePivotMotorDisconnected; + private final Alert motorDisconnected; protected final Alert overheatAlert; @Getter private IntakePivotState goalState = IntakePivotState.OFF; @@ -67,7 +67,7 @@ public IntakePivot(IntakePivotIO io) { this.io = io; this.goalState = IntakePivotState.OFF; - this.intakePivotMotorDisconnected = + this.motorDisconnected = new Alert("Intake Pivot Motor Disconnected!", Alert.AlertType.kWarning); overheatAlert = new Alert("intake pivot motor overheating!", Alert.AlertType.kWarning); } @@ -78,7 +78,7 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("IntakePivot", inputs); - intakePivotMotorDisconnected.set(!inputs.motorConnected); + motorDisconnected.set(!inputs.motorConnected); overheatAlert.set(inputs.tempCelsius > Constants.OVERHEAT_THRESHOLD); handleCurrentState(); From 89f9b8c74e4cb9515c66b93492878f1dfd6ea09e Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 24 Mar 2026 20:21:17 -0700 Subject: [PATCH 15/24] coderabbit fixes --- src/main/java/org/team5924/frc2026/Robot.java | 6 +++--- .../org/team5924/frc2026/commands/drive/DriveCommands.java | 2 ++ .../frc2026/subsystems/vision/VisionIOPhotonVision.java | 1 + 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Robot.java b/src/main/java/org/team5924/frc2026/Robot.java index 8cbd8a3..cbe17d5 100644 --- a/src/main/java/org/team5924/frc2026/Robot.java +++ b/src/main/java/org/team5924/frc2026/Robot.java @@ -163,6 +163,8 @@ public void robotPeriodic() { } else { lowBatteryAlert.set(false); } + + LaunchCalculator.getInstance().clearLaunchingParameters(); } /** This function is called once when the robot is disabled. */ @@ -210,9 +212,7 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - LaunchCalculator.getInstance().clearLaunchingParameters(); - } + public void teleopPeriodic() {} /** This function is called once when test mode is enabled. */ @Override diff --git a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java index fbe1c0d..23554cb 100644 --- a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java @@ -258,6 +258,8 @@ public static Command joystickDriveWhileLaunching( } } + if (driveLauncherCORMaxErrorDeg.get() < driveLauncherCORMinErrorDeg.get()) return; + // Apply chassis speeds double corScalar = MathUtil.clamp( diff --git a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java index 3adcab6..2b7f01e 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java @@ -117,6 +117,7 @@ private void updateLoggedTunableNumbers() { public void updateInputs(VisionIOInputs inputs) { updateLoggedTunableNumbers(); inputs.connected = camera.isConnected(); + inputs.cameraToTarget = null; // Read new camera observations Set tagIds = new HashSet<>(); From 825fcf91b2198f206f1fa6a5bb44fd453a09a432 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 24 Mar 2026 21:29:49 -0700 Subject: [PATCH 16/24] add intake follower --- .../java/org/team5924/frc2026/Constants.java | 35 +++++++++---------- .../flywheel/FlywheelIOTalonFX.java | 8 +++-- .../subsystems/rollers/intake/Intake.java | 1 - .../rollers/intake/IntakeFollowerIO.java | 21 +++++++++++ .../rollers/intake/IntakeIOTalonFX.java | 18 ++++++++++ 5 files changed, 61 insertions(+), 22 deletions(-) create mode 100644 src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIO.java diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 60d4a59..d7e123c 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -28,6 +28,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -101,7 +102,18 @@ public final class Field { .withVoltageClosedLoopRampPeriod(0.02); public final class GenericRoller { - public static final TalonFXConfiguration CONFIG = + public static final TalonFXConfiguration CLOCKWISE_CONFIG = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(60) + .withStatorCurrentLimit(60)) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake)); + + public static final TalonFXConfiguration COUNTERCLOCKWISE_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() @@ -120,11 +132,12 @@ public final class GenericRoller { public final class Intake { public static final int CAN_ID = 41; + public static final int FOLLOWER_CAN_ID = 42; public static final String BUS = "rio"; public static final double MOTOR_TO_MECHANISM = 36.0 / 16.0; public static final double SIM_MOI = 0.001; - public static final TalonFXConfiguration CONFIG = GenericRoller.CONFIG; + public static final TalonFXConfiguration CONFIG = GenericRoller.COUNTERCLOCKWISE_CONFIG; } public final class IntakePivot { @@ -182,7 +195,7 @@ public final class Hopper { public static final double MOTOR_TO_MECHANISM = (16.0 / 12.0) * (24.0 / 16.0); public static final double SIM_MOI = 0.001; - public static final TalonFXConfiguration CONFIG = GenericRoller.CONFIG; + public static final TalonFXConfiguration CONFIG = GenericRoller.COUNTERCLOCKWISE_CONFIG; } public final class Indexer { @@ -194,7 +207,7 @@ public final class Indexer { public static final double SIM_MOI = 0.001; public static final TalonFXConfiguration CONFIG = - GenericRoller.CONFIG + GenericRoller.COUNTERCLOCKWISE_CONFIG .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive)); @@ -290,20 +303,6 @@ public final class Flywheel { new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast)); - - public static final TalonFXConfiguration OPPOSER_CONFIG = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withSupplyCurrentLimit(60) - .withStatorCurrentLimit(60)) - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast)) - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive)); public static final FeedbackConfigs FEEDBACK_CONFIGS = new FeedbackConfigs() diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java index f194489..184f5be 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java @@ -120,8 +120,8 @@ public FlywheelIOTalonFX() { statusArray[5] = leaderConfig.apply(motionMagicConfigs); statusArray[6] = followerConfig.apply(Flywheel.CONFIG); - statusArray[7] = opposerOneConfig.apply(Flywheel.OPPOSER_CONFIG); - statusArray[8] = opposerTwoConfig.apply(Flywheel.OPPOSER_CONFIG); + statusArray[7] = opposerOneConfig.apply(Flywheel.CONFIG); + statusArray[8] = opposerTwoConfig.apply(Flywheel.CONFIG); boolean isErrorPresent = false; for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; @@ -133,7 +133,9 @@ public FlywheelIOTalonFX() { Logger.recordOutput("Flywheel/InitConfReport", statusArray); - followerTalon.setControl(new Follower(Flywheel.CAN_ID, MotorAlignmentValue.Opposed)); + followerTalon.setControl(new Follower(Flywheel.CAN_ID, MotorAlignmentValue.Aligned)); + opposerOneTalon.setControl(new Follower(Flywheel.CAN_ID, MotorAlignmentValue.Opposed)); + opposerTwoTalon.setControl(new Follower(Flywheel.CAN_ID, MotorAlignmentValue.Opposed)); // Get select status signals and set update frequency position = leaderTalon.getPosition(); diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java index 2484660..259e33f 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java @@ -25,7 +25,6 @@ @Getter public class Intake extends GenericRoller { - @RequiredArgsConstructor @Getter public enum IntakeState implements VoltageState { diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIO.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIO.java new file mode 100644 index 0000000..ef94b7c --- /dev/null +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIO.java @@ -0,0 +1,21 @@ +/* + * IntakeIO.java + */ + +/* + * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2026.subsystems.rollers.intake; + +import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerIO; + +public interface IntakeFollowerIO extends GenericRollerIO {} diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java index 2bf5d01..3a88885 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java @@ -19,13 +19,31 @@ import org.team5924.frc2026.Constants.Intake; import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerIOTalonFX; +import com.ctre.phoenix6.configs.TalonFXConfiguration; + public class IntakeIOTalonFX extends GenericRollerIOTalonFX implements IntakeIO { + private class IntakeFollowerIOTalonFX extends GenericRollerIOTalonFX { + public IntakeFollowerIOTalonFX(int id, String bus, TalonFXConfiguration config, double reduction) { + super(id, bus, config, reduction); + } + } + + private final IntakeFollowerIOTalonFX follower; + public IntakeIOTalonFX() { super(Intake.CAN_ID, Intake.BUS, Intake.CONFIG, Intake.MOTOR_TO_MECHANISM); + follower = new IntakeFollowerIOTalonFX(Intake.FOLLOWER_CAN_ID, Intake.BUS, Intake.CONFIG, Intake.MOTOR_TO_MECHANISM); } @Override public void runVolts(double volts) { super.runVolts(volts); + follower.runVolts(-volts); + } + + @Override + public void stop() { + super.stop(); + follower.stop(); } } From 5eea72d22484651baa112dff386328932d58f177 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Thu, 26 Mar 2026 16:14:40 -0700 Subject: [PATCH 17/24] fixes, intake wip --- .vscode/settings.json | 2 +- .../java/org/team5924/frc2026/Constants.java | 9 ++++--- .../flywheel/FlywheelIOTalonFX.java | 11 ++++----- .../generic/GenericRollerIOTalonFX.java | 2 +- .../rollers/hopper/HopperIOTalonFX.java | 5 ---- .../rollers/indexer/IndexerIOTalonFX.java | 11 --------- .../subsystems/rollers/intake/Intake.java | 8 +++++++ .../subsystems/rollers/intake/IntakeIO.java | 6 ++++- .../rollers/intake/IntakeIOSim.java | 3 +++ .../rollers/intake/IntakeIOTalonFX.java | 24 ++++++------------- 10 files changed, 34 insertions(+), 47 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 1642911..3e78796 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -62,5 +62,5 @@ null ], "java.dependency.enableDependencyCheckup": false, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index d7e123c..d787f85 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -28,7 +28,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -110,7 +109,7 @@ public final class GenericRoller { .withStatorCurrentLimit(60)) .withMotorOutput( new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive) + .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake)); public static final TalonFXConfiguration COUNTERCLOCKWISE_CONFIG = @@ -137,7 +136,7 @@ public final class Intake { public static final double MOTOR_TO_MECHANISM = 36.0 / 16.0; public static final double SIM_MOI = 0.001; - public static final TalonFXConfiguration CONFIG = GenericRoller.COUNTERCLOCKWISE_CONFIG; + public static final TalonFXConfiguration CONFIG = GenericRoller.COUNTERCLOCKWISE_CONFIG.clone(); } public final class IntakePivot { @@ -195,7 +194,7 @@ public final class Hopper { public static final double MOTOR_TO_MECHANISM = (16.0 / 12.0) * (24.0 / 16.0); public static final double SIM_MOI = 0.001; - public static final TalonFXConfiguration CONFIG = GenericRoller.COUNTERCLOCKWISE_CONFIG; + public static final TalonFXConfiguration CONFIG = GenericRoller.COUNTERCLOCKWISE_CONFIG.clone(); } public final class Indexer { @@ -207,7 +206,7 @@ public final class Indexer { public static final double SIM_MOI = 0.001; public static final TalonFXConfiguration CONFIG = - GenericRoller.COUNTERCLOCKWISE_CONFIG + GenericRoller.COUNTERCLOCKWISE_CONFIG.clone() .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive)); diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java index 184f5be..cabf774 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java @@ -82,8 +82,7 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final StatusSignal appliedVoltage; private final StatusSignal supplyCurrent; private final StatusSignal torqueCurrent; - private final ArrayList> tempCelsius = - new ArrayList>(4); + private final ArrayList> tempCelsius = new ArrayList<>(4); private final StatusSignal closedLoopReferenceSlope; private double prevClosedLoopReferenceSlope = 0.0; @@ -144,10 +143,10 @@ public FlywheelIOTalonFX() { supplyCurrent = leaderTalon.getSupplyCurrent(); torqueCurrent = leaderTalon.getTorqueCurrent(); - tempCelsius.set(0, leaderTalon.getDeviceTemp()); - tempCelsius.set(1, followerTalon.getDeviceTemp()); - tempCelsius.set(2, opposerOneTalon.getDeviceTemp()); - tempCelsius.set(3, opposerTwoTalon.getDeviceTemp()); + tempCelsius.add(leaderTalon.getDeviceTemp()); + tempCelsius.add(followerTalon.getDeviceTemp()); + tempCelsius.add(opposerOneTalon.getDeviceTemp()); + tempCelsius.add(opposerTwoTalon.getDeviceTemp()); closedLoopReferenceSlope = leaderTalon.getClosedLoopReferenceSlope(); diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/generic/GenericRollerIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/generic/GenericRollerIOTalonFX.java index 707cc26..e509d69 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/generic/GenericRollerIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/generic/GenericRollerIOTalonFX.java @@ -33,7 +33,7 @@ import org.team5924.frc2026.Constants.GenericRoller; public abstract class GenericRollerIOTalonFX implements GenericRollerIO { - private final TalonFX talon; + protected final TalonFX talon; private final StatusSignal position; private final StatusSignal velocity; diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/HopperIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/HopperIOTalonFX.java index fed7aa3..c51cefe 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/HopperIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/HopperIOTalonFX.java @@ -23,9 +23,4 @@ public class HopperIOTalonFX extends GenericRollerIOTalonFX implements HopperIO public HopperIOTalonFX() { super(Hopper.CAN_ID, Hopper.BUS, Hopper.CONFIG, Hopper.MOTOR_TO_MECHANISM); } - - @Override - public void runVolts(double volts) { - super.runVolts(volts); - } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/IndexerIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/IndexerIOTalonFX.java index 05a88b0..d4638a3 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/IndexerIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/IndexerIOTalonFX.java @@ -20,18 +20,7 @@ import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerIOTalonFX; public class IndexerIOTalonFX extends GenericRollerIOTalonFX implements IndexerIO { - public IndexerIOTalonFX() { super(Indexer.CAN_ID, Indexer.BUS, Indexer.CONFIG, Indexer.MOTOR_TO_MECHANISM); } - - @Override - public void runVolts(double volts) { - super.runVolts(volts); - } - - @Override - public void stop() { - super.stop(); - } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java index 259e33f..5c73f6d 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java @@ -38,10 +38,18 @@ public enum IntakeState implements VoltageState { private IntakeState goalState = IntakeState.OFF; private IntakeState currentState = IntakeState.OFF; + private final GenericRollerIOInputsAutoLogged inputs = new GenericRollerIOInputsAutoLogged(); + public Intake(IntakeIO io) { super("Intake", io); } + @Override + public void periodic() { + super.periodic(); + // io.update(inputs); + } + public void setGoalState(IntakeState goalState) { this.goalState = goalState; currentState = goalState; diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIO.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIO.java index ab0ff28..d55e46d 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIO.java @@ -18,4 +18,8 @@ import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerIO; -public interface IntakeIO extends GenericRollerIO {} +public interface IntakeIO extends GenericRollerIO { +// public GenericRollerIOInputs followerInputs; + + public void updateFollowerInputs(); +} diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOSim.java index c02aaa4..9d672bf 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOSim.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOSim.java @@ -25,4 +25,7 @@ public IntakeIOSim() { super( DCMotor.getKrakenX60Foc(1), Constants.Intake.SIM_MOI, Constants.Intake.MOTOR_TO_MECHANISM); } + + @Override + public void updateFollowerInputs() {} } diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java index 3a88885..bfa0b56 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java @@ -19,31 +19,21 @@ import org.team5924.frc2026.Constants.Intake; import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerIOTalonFX; -import com.ctre.phoenix6.configs.TalonFXConfiguration; - public class IntakeIOTalonFX extends GenericRollerIOTalonFX implements IntakeIO { - private class IntakeFollowerIOTalonFX extends GenericRollerIOTalonFX { - public IntakeFollowerIOTalonFX(int id, String bus, TalonFXConfiguration config, double reduction) { - super(id, bus, config, reduction); - } - } - private final IntakeFollowerIOTalonFX follower; public IntakeIOTalonFX() { super(Intake.CAN_ID, Intake.BUS, Intake.CONFIG, Intake.MOTOR_TO_MECHANISM); - follower = new IntakeFollowerIOTalonFX(Intake.FOLLOWER_CAN_ID, Intake.BUS, Intake.CONFIG, Intake.MOTOR_TO_MECHANISM); + follower = new IntakeFollowerIOTalonFX(Intake.FOLLOWER_CAN_ID, Intake.BUS, Intake.CONFIG, Intake.MOTOR_TO_MECHANISM, Intake.CAN_ID); } @Override - public void runVolts(double volts) { - super.runVolts(volts); - follower.runVolts(-volts); + public void updateFollowerInputs() { + // follower.updateInputs(followerInputs); } - @Override - public void stop() { - super.stop(); - follower.stop(); - } + // @Override + // public void setFollowerInputs(GenericRollerIOInputs followerInputs) { + // this.followerInputs = followerInputs; + // } } From 49ac064e2939901bb4ce752365040854fc3a57bf Mon Sep 17 00:00:00 2001 From: James Guleno Date: Thu, 26 Mar 2026 16:16:09 -0700 Subject: [PATCH 18/24] fixes, intake wip wip --- .../frc2026/subsystems/rollers/intake/IntakeIO.java | 2 +- .../frc2026/subsystems/rollers/intake/IntakeIOSim.java | 4 ++-- .../frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIO.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIO.java index d55e46d..8f9f13d 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIO.java @@ -21,5 +21,5 @@ public interface IntakeIO extends GenericRollerIO { // public GenericRollerIOInputs followerInputs; - public void updateFollowerInputs(); + // public void updateFollowerInputs(); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOSim.java index 9d672bf..7e16e0b 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOSim.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOSim.java @@ -26,6 +26,6 @@ public IntakeIOSim() { DCMotor.getKrakenX60Foc(1), Constants.Intake.SIM_MOI, Constants.Intake.MOTOR_TO_MECHANISM); } - @Override - public void updateFollowerInputs() {} + // @Override + // public void updateFollowerInputs() {} } diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java index bfa0b56..db37d2f 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java @@ -27,10 +27,10 @@ public IntakeIOTalonFX() { follower = new IntakeFollowerIOTalonFX(Intake.FOLLOWER_CAN_ID, Intake.BUS, Intake.CONFIG, Intake.MOTOR_TO_MECHANISM, Intake.CAN_ID); } - @Override - public void updateFollowerInputs() { + // @Override + // public void updateFollowerInputs() { // follower.updateInputs(followerInputs); - } + // } // @Override // public void setFollowerInputs(GenericRollerIOInputs followerInputs) { From a29d265bb31f619522639bcdd0db3600913dc9bc Mon Sep 17 00:00:00 2001 From: James Guleno Date: Fri, 27 Mar 2026 18:26:00 -0700 Subject: [PATCH 19/24] intake should work with two motors --- .../org/team5924/frc2026/RobotContainer.java | 1 - .../java/org/team5924/frc2026/RobotState.java | 1 - .../frc2026/commands/drive/DriveCommands.java | 5 +-- .../pivots/shooterHood/ShooterHood.java | 4 ++- .../subsystems/rollers/indexer/Indexer.java | 1 - .../subsystems/rollers/intake/Intake.java | 3 -- .../rollers/intake/IntakeFollowerIO.java | 2 +- .../intake/IntakeFollowerIOTalonFX.java | 36 +++++++++++++++++++ .../subsystems/rollers/intake/IntakeIO.java | 6 +--- .../rollers/intake/IntakeIOTalonFX.java | 12 +------ 10 files changed, 43 insertions(+), 28 deletions(-) create mode 100644 src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIOTalonFX.java diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 0faadc9..dc0992a 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -31,7 +31,6 @@ import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.team5924.frc2026.commands.drive.DriveCommands; -import org.team5924.frc2026.commands.shooter.AutoScoreCommands; import org.team5924.frc2026.generated.TunerConstants; import org.team5924.frc2026.subsystems.drive.Drive; import org.team5924.frc2026.subsystems.drive.GyroIO; diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index 1e99be0..a597675 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -21,7 +21,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import lombok.Getter; import lombok.Setter; -import org.littletonrobotics.junction.AutoLogOutput; @Getter public class RobotState { diff --git a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java index 0f5a94e..22b60c9 100644 --- a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java @@ -273,10 +273,7 @@ public static Command joystickDriveWhileLaunching( 0.0, 1.0); Translation2d launcherToRobot = - LauncherConstants.robotToLauncher - .getTranslation() - .toTranslation2d() - .unaryMinus(); + LauncherConstants.robotToLauncher.getTranslation().toTranslation2d().unaryMinus(); ChassisSpeeds fieldRelativeSpeedsWithOffset = GeomUtil.transformVelocity( new ChassisSpeeds( diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java index b578b03..a80e2c6 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java @@ -79,7 +79,9 @@ public enum ShooterHoodState { public void setAutoInput(double inputRads) { autoInput = MathUtil.clamp( - inputRads, Constants.ShooterHood.MIN_POSITION_RADS, Constants.ShooterHood.MAX_POSITION_RADS); + inputRads, + Constants.ShooterHood.MIN_POSITION_RADS, + Constants.ShooterHood.MAX_POSITION_RADS); } public ShooterHood(ShooterHoodIO io) { diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java index 9350e6f..9016dc0 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java @@ -19,7 +19,6 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; - import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller.VoltageState; diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java index 5c73f6d..6a55d6a 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java @@ -38,8 +38,6 @@ public enum IntakeState implements VoltageState { private IntakeState goalState = IntakeState.OFF; private IntakeState currentState = IntakeState.OFF; - private final GenericRollerIOInputsAutoLogged inputs = new GenericRollerIOInputsAutoLogged(); - public Intake(IntakeIO io) { super("Intake", io); } @@ -47,7 +45,6 @@ public Intake(IntakeIO io) { @Override public void periodic() { super.periodic(); - // io.update(inputs); } public void setGoalState(IntakeState goalState) { diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIO.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIO.java index ef94b7c..e1e975f 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIO.java @@ -1,5 +1,5 @@ /* - * IntakeIO.java + * IntakeFollowerIO.java */ /* diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIOTalonFX.java new file mode 100644 index 0000000..37ca92d --- /dev/null +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIOTalonFX.java @@ -0,0 +1,36 @@ +/* + * IntakeFollowerIOTalonFX.java + */ + +/* + * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2026.subsystems.rollers.intake; + +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import org.team5924.frc2026.Constants.Intake; +import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerIOTalonFX; + +public class IntakeFollowerIOTalonFX extends GenericRollerIOTalonFX implements IntakeFollowerIO { + public IntakeFollowerIOTalonFX() { + super(Intake.FOLLOWER_CAN_ID, Intake.BUS, Intake.CONFIG, Intake.MOTOR_TO_MECHANISM); + + talon.setControl(new Follower(Intake.CAN_ID, MotorAlignmentValue.Opposed)); + } + + @Override + public void runVolts(double volts) {} + + @Override + public void stop() {} +} diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIO.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIO.java index 8f9f13d..ab0ff28 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIO.java @@ -18,8 +18,4 @@ import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerIO; -public interface IntakeIO extends GenericRollerIO { -// public GenericRollerIOInputs followerInputs; - - // public void updateFollowerInputs(); -} +public interface IntakeIO extends GenericRollerIO {} diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java index db37d2f..2d21457 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java @@ -24,16 +24,6 @@ public class IntakeIOTalonFX extends GenericRollerIOTalonFX implements IntakeIO public IntakeIOTalonFX() { super(Intake.CAN_ID, Intake.BUS, Intake.CONFIG, Intake.MOTOR_TO_MECHANISM); - follower = new IntakeFollowerIOTalonFX(Intake.FOLLOWER_CAN_ID, Intake.BUS, Intake.CONFIG, Intake.MOTOR_TO_MECHANISM, Intake.CAN_ID); + follower = new IntakeFollowerIOTalonFX(); } - - // @Override - // public void updateFollowerInputs() { - // follower.updateInputs(followerInputs); - // } - - // @Override - // public void setFollowerInputs(GenericRollerIOInputs followerInputs) { - // this.followerInputs = followerInputs; - // } } From e521b73cf4eb5cf407e2c71b8f7ecb6d6336fdf7 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Fri, 27 Mar 2026 18:34:41 -0700 Subject: [PATCH 20/24] robot container formatting changes, add toggle for vision --- .../org/team5924/frc2026/RobotContainer.java | 69 +++++++------------ 1 file changed, 25 insertions(+), 44 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index dc0992a..981c663 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -87,6 +87,7 @@ public class RobotContainer { // Real/IO implementation private final boolean realDrive = true; + private final boolean realVision = true; private final boolean realIntake = true; private final boolean realIntakePivot = true; private final boolean realHopper = true; @@ -125,12 +126,14 @@ public RobotContainer() { (pose) -> {}); vision = - new Vision( - drive::addVisionMeasurement, - new VisionIOPhotonVision( - VisionConstants.FRONT_LEFT_NAME, VisionConstants.FRONT_LEFT_TRANSFORM), - new VisionIOPhotonVision( - VisionConstants.FRONT_RIGHT_NAME, VisionConstants.FRONT_RIGHT_TRANSFORM)); + realVision + ? new Vision( + drive::addVisionMeasurement, + new VisionIOPhotonVision( + VisionConstants.FRONT_LEFT_NAME, VisionConstants.FRONT_LEFT_TRANSFORM), + new VisionIOPhotonVision( + VisionConstants.FRONT_RIGHT_NAME, VisionConstants.FRONT_RIGHT_TRANSFORM)) + : null; intake = realIntake ? new Intake(new IntakeIOTalonFX()) : new Intake(new IntakeIO() {}); intakePivot = @@ -138,9 +141,9 @@ public RobotContainer() { ? new IntakePivot(new IntakePivotIOTalonFX()) : new IntakePivot(new IntakePivotIO() {}); hopper = realHopper ? new Hopper(new HopperIOTalonFX()) : new Hopper(new HopperIO() {}); + indexer = realIndexer ? new Indexer(new IndexerIOTalonFX()) : new Indexer(new IndexerIO() {}); - shooterHood = realShooterHood ? new ShooterHood(new ShooterHoodIOTalonFX()) @@ -182,8 +185,8 @@ public RobotContainer() { intake = new Intake(new IntakeIOSim()); intakePivot = new IntakePivot(new IntakePivotIOSim()); hopper = new Hopper(new HopperIOSim()); - indexer = new Indexer(new IndexerIOSim()); + indexer = new Indexer(new IndexerIOSim()); shooterHood = new ShooterHood(new ShooterHoodIOSim()); flywheel = new Flywheel(new FlywheelIOSim()); @@ -200,13 +203,13 @@ public RobotContainer() { new ModuleIO() {}, (pose) -> {}); - vision = null; // no vision implementation for replay + vision = null; intake = new Intake(new IntakeIO() {}); intakePivot = new IntakePivot(new IntakePivotIO() {}); - hopper = new Hopper(new HopperIO() {}); // TODO: Add replay IO implementation - indexer = new Indexer(new IndexerIO() {}); + hopper = new Hopper(new HopperIO() {}); + indexer = new Indexer(new IndexerIO() {}); shooterHood = new ShooterHood(new ShooterHoodIO() {}); flywheel = new Flywheel(new FlywheelIO() {}); break; @@ -291,18 +294,18 @@ private void configureButtonBindings() { () -> -driveController.getLeftX() * Constants.SLOW_MODE_MULTI, () -> -driveController.getRightX() * Constants.SLOW_MODE_MULTI)); - // // [driver] 0-DEGREE MODE - // driveController - // .a() - // .onTrue( - // DriveCommands.joystickDriveAtAngle( - // drive, - // () -> -driveController.getLeftY(), - // () -> -driveController.getLeftX(), - // () -> Rotation2d.kZero)); + // [driver] 0-DEGREE MODE + driveController + .a() + .onTrue( + DriveCommands.joystickDriveAtAngle( + drive, + () -> -driveController.getLeftY(), + () -> -driveController.getLeftX(), + () -> Rotation2d.kZero)); - // // [driver] Switch to X pattern when X button is pressed - // driveController.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + // [driver] Switch to X pattern when X button is pressed + driveController.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); final Runnable resetGyro = Constants.currentMode == Constants.Mode.SIM @@ -355,28 +358,6 @@ private void configureButtonBindings() { }, intakePivot)); - // // ### intake pivot spit - // driveController - // .leftTrigger() - // .onTrue( - // Commands.runOnce( - // () -> { - // intakePivot.setGoalState(IntakePivotState.DOWN); - // intake.setGoalState(IntakeState.SPITOUT); - // }, - // intakePivot, - // intake)); - // driveController - // .leftTrigger() - // .onFalse( - // Commands.runOnce( - // () -> { - // intakePivot.setGoalState(IntakePivotState.STOW); - // intake.setGoalState(IntakeState.OFF); - // }, - // intakePivot, - // intake)); - // TODO: shooting, auto shooting } From b3809de213c167a11733ea769cc6e540d1ae9417 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Fri, 27 Mar 2026 18:39:19 -0700 Subject: [PATCH 21/24] add shooting bindings --- .../org/team5924/frc2026/RobotContainer.java | 35 ++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 981c663..9e01042 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -358,7 +358,40 @@ private void configureButtonBindings() { }, intakePivot)); - // TODO: shooting, auto shooting + + // shooter + driveController + .leftBumper() + .onTrue( + Commands.runOnce( + () -> { + flywheel.setGoalState(Flywheel.FlywheelState.B8); + indexer.setGoalState(Indexer.IndexerState.INDEXING); + }, + flywheel, + indexer)); + + driveController + .leftBumper() + .onFalse( + Commands.runOnce( + () -> { + flywheel.setGoalState(Flywheel.FlywheelState.OFF); + indexer.setGoalState(Indexer.IndexerState.OFF); + }, + flywheel, + indexer)); + + shooterHood.setDefaultCommand( + Commands.runOnce( + () -> shooterHood.setGoalState(ShooterHood.ShooterHoodState.MANUAL), shooterHood)); + + driveController + .rightStick() + .onTrue( + Commands.runOnce(() -> shooterHood.setInput(driveController.getRightY()), shooterHood)); + + // TODO: auto shooting } /** From f430c17ab08b511060a4c49f7a1979f0d73d4134 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Fri, 27 Mar 2026 19:03:09 -0700 Subject: [PATCH 22/24] fix shooter hood states and bindings --- src/main/java/org/team5924/frc2026/RobotContainer.java | 4 ++-- .../subsystems/pivots/shooterHood/ShooterHood.java | 8 ++++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 9e01042..e818f7b 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -328,7 +328,7 @@ private void configureButtonBindings() { /* ### intake ### */ driveController - .leftBumper() + .rightBumper() .onTrue( Commands.runOnce( () -> { @@ -389,7 +389,7 @@ private void configureButtonBindings() { driveController .rightStick() .onTrue( - Commands.runOnce(() -> shooterHood.setInput(driveController.getRightY()), shooterHood)); + Commands.run(() -> shooterHood.setInput(() -> driveController.getRightY()), shooterHood)); // TODO: auto shooting } diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java index a80e2c6..b650da5 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java @@ -73,9 +73,13 @@ public enum ShooterHoodState { private double lastStateChange = 0.0; private double timeSinceLastStateChange = 0.0; - @Setter private double input; + private double input; private double autoInput = 0.0; + public void setInput(DoubleSupplier inputSupplier) { + input = inputSupplier.getAsDouble(); + } + public void setAutoInput(double inputRads) { autoInput = MathUtil.clamp( @@ -136,7 +140,7 @@ public void setGoalState(ShooterHoodState goalState) { case MOVING -> DriverStation.reportError( "Shooter Hood: MOVING is an invalid goal state; it is a transition state!!", null); - case AUTO -> currentState = ShooterHoodState.MOVING; + case AUTO -> currentState = ShooterHoodState.AUTO; case OFF -> currentState = ShooterHoodState.OFF; default -> currentState = ShooterHoodState.MOVING; } From cf6d953c9684ec283e46a93cbb58dd3958df3c44 Mon Sep 17 00:00:00 2001 From: Gio Bueno Date: Fri, 27 Mar 2026 19:13:13 -0700 Subject: [PATCH 23/24] added documentation so we can merge --- .../team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java index cabf774..0aa3acf 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java @@ -266,6 +266,7 @@ private void updateLoggedTunableNumbers() { } @Override + // runs motor at volts public void runVolts(double volts) { leaderTalon.setControl(voltageOut.withOutput(volts)); } From ea8b214c1d83761e7c003f313cd54e7a4f16b3b2 Mon Sep 17 00:00:00 2001 From: Gio Bueno Date: Fri, 27 Mar 2026 19:32:29 -0700 Subject: [PATCH 24/24] fixed build error --- .../frc2026/commands/shooter/ManualShooterCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java b/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java index 962f264..4737b7b 100644 --- a/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java @@ -30,7 +30,7 @@ public static Command manualShooterHood(ShooterHood hood, DoubleSupplier inputSu return Commands.run( () -> { hood.setGoalState(ShooterHoodState.MANUAL); - hood.setInput(inputSupplier.getAsDouble()); + hood.setInput(inputSupplier); }, hood); }