From 9e01ed897905af7976152ea352af31d44640e732 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 17 Mar 2026 19:34:39 -0700 Subject: [PATCH 01/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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 69524a9a4bed679e362ea86f8027fd768befd486 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 24 Mar 2026 16:47:59 -0700 Subject: [PATCH 08/11] coderabbit fixes --- src/main/java/org/team5924/frc2026/Robot.java | 12 ++--- .../org/team5924/frc2026/RobotContainer.java | 52 ++++++++++++++----- .../frc2026/commands/drive/DriveCommands.java | 3 ++ .../frc2026/generated/TunerConstants.java | 2 +- .../frc2026/subsystems/flywheel/Flywheel.java | 4 +- .../pivots/shooterHood/ShooterHood.java | 2 +- .../frc2026/subsystems/vision/VisionIO.java | 1 - .../frc2026/util/LaunchCalculator.java | 6 +++ 8 files changed, 57 insertions(+), 25 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Robot.java b/src/main/java/org/team5924/frc2026/Robot.java index 8cbd8a3..d0fe751 100644 --- a/src/main/java/org/team5924/frc2026/Robot.java +++ b/src/main/java/org/team5924/frc2026/Robot.java @@ -126,12 +126,6 @@ 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(); @@ -205,6 +199,12 @@ public void teleopInit() { } updateMatchShift.startPeriodic(0.2); + // Low battery alert + if (DriverStation.isEnabled()) { + disabledTimer.reset(); + lowBatteryAlert.set(false); + } + Elastic.selectTab("Teleoperated"); } diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 5f504f3..7c58fe8 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -72,6 +72,7 @@ import org.team5924.frc2026.subsystems.vision.VisionConstants; import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVision; import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVisionSim; +import org.team5924.frc2026.util.LaunchCalculator; public class RobotContainer { // Subsystems @@ -348,27 +349,27 @@ private void configureButtonBindings() { hopper.setDefaultCommand( Commands.run(() -> hopper.setGoalState(Hopper.HopperState.ON), hopper)); - // // ### indexing + // // ### intaking driveController .rightBumper() .onTrue( Commands.runOnce( () -> { - intakePivot.setGoalState(IntakePivotState.DOWN); + intakePivot.setGoalState(IntakePivotState.DOWN); intake.setGoalState(IntakeState.INTAKE); }, intakePivot, - indexer)); + intake)); driveController .rightBumper() .onFalse( Commands.runOnce( () -> { - intakePivot.setGoalState(IntakePivotState.STOW); + intakePivot.setGoalState(IntakePivotState.STOW); intake.setGoalState(IntakeState.OFF); }, intakePivot, - indexer)); + intake)); // driveController // .leftBumper() @@ -415,6 +416,28 @@ private void configureButtonBindings() { // flywheelLeft, // indexer)); + // driveController + // .leftBumper() + // .onTrue( + // Commands.runOnce( + // () -> { + // flywheelLeft.setGoalState(FlywheelState.SLOW_LAUNCH); + // indexer.setGoalState(IndexerState.INDEXING); + // }, + // flywheelLeft, + // indexer)); + + // driveController + // .leftBumper() + // .onFalse( + // Commands.runOnce( + // () -> { + // flywheelLeft.setGoalState(FlywheelState.OFF); + // indexer.setGoalState(IndexerState.OFF); + // }, + // flywheelLeft, + // indexer)); + driveController .leftBumper() .onTrue( @@ -424,26 +447,27 @@ private void configureButtonBindings() { () -> { indexer.setGoalState(IndexerState.INDEXING); }, - indexer))); - // driveController - // .rightTrigger() - // .whileTrue( - // DriveCommands.joystickDriveWhileLaunching( - // drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); - + indexer))) + .and(() -> LaunchCalculator.getInstance().getParameters().isValid()); driveController .leftBumper() .onFalse( Commands.runOnce( () -> { flywheelLeft.setGoalState(FlywheelState.OFF); - flywheelRight.setGoalState(FlywheelState.OFF); indexer.setGoalState(IndexerState.OFF); }, flywheelLeft, - flywheelRight, indexer)); + // driveController + // .rightTrigger() + // .whileTrue( + // DriveCommands.joystickDriveWhileLaunching( + // drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); + + // ------- + // // driveController // // .rightTrigger() // // .onTrue( 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..35a6a11 100644 --- a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java @@ -204,6 +204,9 @@ public static Command joystickDriveWhileLaunching( () -> { // Run PID controller final var parameters = LaunchCalculator.getInstance().getParameters(); + + if (!parameters.isValid()) return; + double omegaOutput = parameters.driveVelocity() + (parameters diff --git a/src/main/java/org/team5924/frc2026/generated/TunerConstants.java b/src/main/java/org/team5924/frc2026/generated/TunerConstants.java index aad94c2..8aaeba9 100644 --- a/src/main/java/org/team5924/frc2026/generated/TunerConstants.java +++ b/src/main/java/org/team5924/frc2026/generated/TunerConstants.java @@ -118,7 +118,7 @@ public class TunerConstants { .withMountPose( new MountPoseConfigs() .withMountPosePitch(87.24925231933594) - .withMountPoseYaw(-0.7844216227531433) + .withMountPoseYaw(-0.7844216227531433 + 180.0) .withMountPoseRoll(-179.69129943847656)); // CAN bus that the devices are located on; 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 6bcf31b..b2e0f15 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java @@ -73,7 +73,7 @@ public enum FlywheelState { private double autoInput = 0.0; public void setAutoInput(double inputRads) { - autoInput = MathUtil.clamp(inputRads, 0.0, 100.0); + autoInput = MathUtil.clamp(inputRads, 0.0, 150.0); } public Flywheel(FlywheelIO io, boolean isLeft) { @@ -164,7 +164,7 @@ private void handleCurrentState() { case OFF -> stop(); case B4, B6, B8, B12 -> runVolts(getTargetVelocityRotationsPerSec()); case AUTO -> { - autoInput = LaunchCalculator.getInstance().getParameters(isLeft).flywheelSpeed(); + setAutoInput(LaunchCalculator.getInstance().getParameters(isLeft).flywheelSpeed()); setVelocity(autoInput); } default -> setVelocity(getTargetVelocityRotationsPerSec()); 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 5f0fd28..6853431 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 @@ -182,7 +182,7 @@ private void handleCurrentState() { case MANUAL -> handleManualState(); case OFF -> stop(); case AUTO -> { - autoInput = LaunchCalculator.getInstance().getParameters(isLeft).hoodAngle(); + setAutoInput(LaunchCalculator.getInstance().getParameters(isLeft).hoodAngle()); if (!isAtSetpoint) setPosition(autoInput); } default -> { 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 93cf0e2..c2e350f 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIO.java @@ -24,7 +24,6 @@ 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]; diff --git a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java index 59869bb..8e2cca0 100644 --- a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java +++ b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java @@ -16,6 +16,7 @@ package org.team5924.frc2026.util; +import static edu.wpi.first.units.Units.Rotation; import static org.team5924.frc2026.util.LauncherConstants.*; import edu.wpi.first.math.MathUtil; @@ -319,6 +320,11 @@ public LaunchingParameters getParameters(Transform3d robotToLauncher) { boolean passing = AllianceFlipUtil.applyX(RobotState.getInstance().getOdometryPose().getX()) > FieldConstants.LinesVertical.hubCenter; + + // TODO: passing not implemented yet + if (passing) + return new LaunchingParameters(false, Rotation2d.kZero, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, true); + if (latestParameters != null) { return latestParameters; } From 1901b4b11ad798e5853fe0a594fed3bad4bed05c Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 24 Mar 2026 17:13:47 -0700 Subject: [PATCH 09/11] fix bindings, stale vision camera to target logging, zeroed offset in launch calc --- .../org/team5924/frc2026/RobotContainer.java | 4 ++-- .../frc2026/subsystems/flywheel/Flywheel.java | 2 +- .../vision/VisionIOPhotonVision.java | 19 ++++++++++--------- .../frc2026/util/LaunchCalculator.java | 4 +--- 4 files changed, 14 insertions(+), 15 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 7c58fe8..46d813c 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -440,6 +440,7 @@ private void configureButtonBindings() { driveController .leftBumper() + .and(() -> LaunchCalculator.getInstance().getParameters().isValid()) .onTrue( AutoScoreCommands.runTrackTargetCommand(shooterHoodLeft, flywheelLeft, true) .alongWith( @@ -447,8 +448,7 @@ private void configureButtonBindings() { () -> { indexer.setGoalState(IndexerState.INDEXING); }, - indexer))) - .and(() -> LaunchCalculator.getInstance().getParameters().isValid()); + indexer))); driveController .leftBumper() .onFalse( 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 b2e0f15..cfd1ffd 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java @@ -183,7 +183,7 @@ private void handleManualState() { } public void updateSetpointState(double change) { - autoInput = inputs.setpointVelocityRotationsPerSec + change; + setAutoInput(inputs.setpointVelocityRotationsPerSec + change); setGoalState(FlywheelState.SETPOINT); } 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..607e8e2 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java @@ -122,6 +122,8 @@ public void updateInputs(VisionIOInputs inputs) { Set tagIds = new HashSet<>(); List poseObservations = new LinkedList<>(); for (var result : camera.getAllUnreadResults()) { + Transform3d cameraToTarget = Transform3d.kZero; + // Add pose observation if (result.multitagResult.isPresent()) { // Multitag result var multitagResult = result.multitagResult.get(); @@ -158,15 +160,7 @@ public void updateInputs(VisionIOInputs inputs) { if (tagPose.isPresent()) { 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())); + cameraToTarget = target.bestCameraToTarget; Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); @@ -185,6 +179,13 @@ public void updateInputs(VisionIOInputs inputs) { cameraToTarget.getTranslation().getNorm(), // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type } + Rotation3d cameraToTargetRotation = cameraToTarget.getRotation(); + inputs.cameraToTarget = + new TranslationRotation( + cameraToTarget.getTranslation(), + Units.radiansToDegrees(cameraToTargetRotation.getX()), + Units.radiansToDegrees(cameraToTargetRotation.getY()), + Units.radiansToDegrees(cameraToTargetRotation.getZ())); } } diff --git a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java index 8e2cca0..f319e1c 100644 --- a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java +++ b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java @@ -16,7 +16,6 @@ package org.team5924.frc2026.util; -import static edu.wpi.first.units.Units.Rotation; import static org.team5924.frc2026.util.LauncherConstants.*; import edu.wpi.first.math.MathUtil; @@ -348,7 +347,7 @@ public LaunchingParameters getParameters(Transform3d robotToLauncher) { double launcherToTargetDistance = target.getDistance(launcherPosition.getTranslation()); // Calculate field relative launcher velocity - var robotVelocity = new ChassisSpeeds(); // RobotState.getInstance().getFieldSetpointVelocity(); + var robotVelocity = RobotState.getInstance().getFieldSetpointVelocity(); var robotAngle = RobotState.getInstance().getRotation(); ChassisSpeeds launcherVelocity = DriverStation.isAutonomous() @@ -451,7 +450,6 @@ private static Rotation2d getTurretAngle(Pose2d turretPose, Translation2d target private static Rotation2d getDriveAngleWithLauncherOffset( Pose2d robotPose, Translation2d target, Transform3d robotToLauncher) { - ; Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle(); Rotation2d hubAngle = From 67ee32e525a43320d0a7e044d40f4331ee5c8739 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 24 Mar 2026 17:30:12 -0700 Subject: [PATCH 10/11] binding, vision logging + logic fixes --- .../org/team5924/frc2026/RobotContainer.java | 1 + .../vision/VisionIOPhotonVision.java | 52 +++++++++---------- 2 files changed, 25 insertions(+), 28 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 46d813c..848dc95 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -451,6 +451,7 @@ private void configureButtonBindings() { indexer))); driveController .leftBumper() + .and(() -> LaunchCalculator.getInstance().getParameters().isValid()) .onFalse( Commands.runOnce( () -> { 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 607e8e2..09fafed 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java @@ -79,22 +79,22 @@ public VisionIOPhotonVision(String name, Transform3d robotToCamera) { "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()))); + updateRobotToCamera(); } private void updateLoggedTunableNumbers() { LoggedTunableNumber.ifChanged( hashCode(), - () -> + this::updateRobotToCamera, + xInches, + yInches, + zInches, + rollDegrees, + pitchDegrees, + yawDegrees); + } + + private void updateRobotToCamera() { robotToCamera = new Transform3d( new Translation3d( @@ -104,13 +104,7 @@ private void updateLoggedTunableNumbers() { new Rotation3d( Units.degreesToRadians(rollDegrees.getAsDouble()), Units.degreesToRadians(pitchDegrees.getAsDouble()), - Units.degreesToRadians(yawDegrees.getAsDouble()))), - xInches, - yInches, - zInches, - rollDegrees, - pitchDegrees, - yawDegrees); + Units.degreesToRadians(yawDegrees.getAsDouble()))); } @Override @@ -122,8 +116,6 @@ public void updateInputs(VisionIOInputs inputs) { Set tagIds = new HashSet<>(); List poseObservations = new LinkedList<>(); for (var result : camera.getAllUnreadResults()) { - Transform3d cameraToTarget = Transform3d.kZero; - // Add pose observation if (result.multitagResult.isPresent()) { // Multitag result var multitagResult = result.multitagResult.get(); @@ -133,6 +125,9 @@ public void updateInputs(VisionIOInputs inputs) { Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + // no single target, so log invalid cameraToTarget value + inputs.cameraToTarget = new TranslationRotation(Translation3d.kZero, -1, -1, -1); + // Calculate average tag distance double totalTagDistance = 0.0; for (var target : result.targets) { @@ -160,7 +155,15 @@ public void updateInputs(VisionIOInputs inputs) { if (tagPose.isPresent()) { Transform3d fieldToTarget = new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); - cameraToTarget = target.bestCameraToTarget; + 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()); @@ -179,13 +182,6 @@ public void updateInputs(VisionIOInputs inputs) { cameraToTarget.getTranslation().getNorm(), // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type } - Rotation3d cameraToTargetRotation = cameraToTarget.getRotation(); - inputs.cameraToTarget = - new TranslationRotation( - cameraToTarget.getTranslation(), - Units.radiansToDegrees(cameraToTargetRotation.getX()), - Units.radiansToDegrees(cameraToTargetRotation.getY()), - Units.radiansToDegrees(cameraToTargetRotation.getZ())); } } From 8f31100ee28eb239d79720df677ce4f9cc088437 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 24 Mar 2026 17:43:45 -0700 Subject: [PATCH 11/11] indentation --- .../org/team5924/frc2026/RobotContainer.java | 4 ++-- .../vision/VisionIOPhotonVision.java | 20 +++++++++---------- .../frc2026/util/LaunchCalculator.java | 5 +++-- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 848dc95..d728874 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -355,7 +355,7 @@ private void configureButtonBindings() { .onTrue( Commands.runOnce( () -> { - intakePivot.setGoalState(IntakePivotState.DOWN); + intakePivot.setGoalState(IntakePivotState.DOWN); intake.setGoalState(IntakeState.INTAKE); }, intakePivot, @@ -365,7 +365,7 @@ private void configureButtonBindings() { .onFalse( Commands.runOnce( () -> { - intakePivot.setGoalState(IntakePivotState.STOW); + intakePivot.setGoalState(IntakePivotState.STOW); intake.setGoalState(IntakeState.OFF); }, intakePivot, 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 09fafed..4460382 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java @@ -95,16 +95,16 @@ private void updateLoggedTunableNumbers() { } private void updateRobotToCamera() { - 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()))); + 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()))); } @Override diff --git a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java index f319e1c..fe7cded 100644 --- a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java +++ b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java @@ -320,9 +320,10 @@ public LaunchingParameters getParameters(Transform3d robotToLauncher) { AllianceFlipUtil.applyX(RobotState.getInstance().getOdometryPose().getX()) > FieldConstants.LinesVertical.hubCenter; - // TODO: passing not implemented yet + // TODO: passing not implemented yet if (passing) - return new LaunchingParameters(false, Rotation2d.kZero, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, true); + return new LaunchingParameters( + false, Rotation2d.kZero, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, true); if (latestParameters != null) { return latestParameters;