From f78ffec8bec7f1fd60182540e2413c03a5694748 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Thu, 16 Apr 2026 08:58:53 -0500 Subject: [PATCH 01/25] Feat: autos speed up to predicted pose speed. Feat: add a button binding for shooting to the hub instead of switching out to feeding. --- src/main/java/frc/robot/Orchestrator.java | 3 +-- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/commands/auto/Autos.java | 8 +++++--- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 37135b86b..f32a35226 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -352,8 +352,7 @@ public Command aimToHub() { new Pose2d( drive.getPose().getX(), drive.getPose().getY(), - AllianceFlipUtil.apply(Hub.blueCenter) - .plus(new Translation2d(-0.4, 0)) + AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()) .minus( drive .getPose() diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a205ec02e..202d66423 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -125,7 +125,7 @@ public RobotContainer() { indexer = new Indexer(new IndexerIOSparkMax()); intakeRollers = new IntakeRollers(new IntakeRollersIOKraken()); intakeExtend = - new IntakeExtend(new IntakeExtendIOSparkMax(), operatorController.rightTrigger()); + new IntakeExtend(new IntakeExtendIOSparkMax(), () -> false); // climber = new Climber(new ClimberIOPhysical()); } @@ -337,7 +337,7 @@ private void configureButtonBindings() { .and(operatorController.pov(180)) .whileTrue(intakeExtend.runIntakeExtendVolts(4)) .onFalse(intakeExtend.stopExtendingCommand()); - operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest()); + operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())); operatorController .rightTrigger(0.1) .and(() -> !operatorController.pov(0).getAsBoolean()) diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 06eb4f0a5..d098be0ab 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -15,6 +15,8 @@ import frc.robot.subsystems.shooter.ShooterConstants; import java.util.function.Supplier; +import static edu.wpi.first.units.Units.Meters; + /** Contains all autos */ public class Autos { @@ -154,7 +156,7 @@ private Command ppCycleRegression(String path, Supplier startPose) { drive.getAutonomousCommand(path), intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), Commands.waitSeconds(5) - .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) + .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(2.62)))) .withTimeout(15.5), Commands.deadline( orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), @@ -175,8 +177,8 @@ private Command ppCycleRegressionDepo(String path, Supplier startPose) { drive.getAutonomousCommand(path), intake.extendToAngleAndIntake(IntakeConstants.FUNNEL_POS).withTimeout(5.5), Commands.waitSeconds(5) - .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) - .withTimeout(15.5), + .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(2.62)))) + .withTimeout(16.5), Commands.deadline( orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) From 1edd8665c72b48e255e61d0bd0bda8bc6ffb8c21 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Thu, 16 Apr 2026 13:30:13 -0500 Subject: [PATCH 02/25] dcmp before match 2 --- src/main/java/frc/robot/Orchestrator.java | 12 ++++++------ src/main/java/frc/robot/RobotContainer.java | 14 +++++++++++--- src/main/java/frc/robot/commands/auto/Autos.java | 4 ++-- .../java/frc/robot/subsystems/drive/Drive.java | 2 +- .../frc/robot/subsystems/drive/DriveConstants.java | 2 +- .../java/frc/robot/subsystems/intake/Intake.java | 13 ++++++++++++- .../robot/subsystems/intake/IntakeConstants.java | 2 +- .../java/frc/robot/subsystems/shooter/Shooter.java | 3 ++- .../robot/subsystems/vision/VisionConstants.java | 2 +- 9 files changed, 37 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index f32a35226..a95049840 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -5,6 +5,8 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Rotations; import static frc.robot.subsystems.shooter.ShooterConstants.CLOSE_HUB_SHOOTER_RPM; +import static frc.robot.subsystems.shooter.ShooterConstants.kShooterOffsetFromRobotCenter; +import static frc.robot.subsystems.vision.VisionConstants.*; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; @@ -233,7 +235,7 @@ public Supplier getHubDistance() { .getDistance( drive .getPose() - .transformBy(ShooterConstants.kShooterOffsetFromRobotCenter) + .transformBy(kShooterOffsetFromRobotCenter) .getTranslation())); } @@ -352,12 +354,10 @@ public Command aimToHub() { new Pose2d( drive.getPose().getX(), drive.getPose().getY(), - AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()) + AllianceFlipUtil.apply(Hub.innerCenterPoint) + .toTranslation2d() .minus( - drive - .getPose() - .transformBy(ShooterConstants.kShooterOffsetFromRobotCenter) - .getTranslation()) + drive.getPose().transformBy(kShooterOffsetFromRobotCenter).getTranslation()) .getAngle())); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 202d66423..40dcad6f1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -124,8 +124,7 @@ public RobotContainer() { magicCarpet = new MagicCarpet(new MagicCarpetSparkMax()); indexer = new Indexer(new IndexerIOSparkMax()); intakeRollers = new IntakeRollers(new IntakeRollersIOKraken()); - intakeExtend = - new IntakeExtend(new IntakeExtendIOSparkMax(), () -> false); + intakeExtend = new IntakeExtend(new IntakeExtendIOSparkMax(), () -> false); // climber = new Climber(new ClimberIOPhysical()); } @@ -311,7 +310,14 @@ private void configureButtonBindings() { driverController.leftBumper(), () -> RobotState.getInstance().intakePosition == IntakePosition.DEPLOYED) .and(() -> !operatorController.pov(180).getAsBoolean()) + .and(() -> !RobotState.getInstance().shooterAtSpeed) .toggleOnTrue(intakeExtend.extendToAngle(IntakeConstants.COLLAPSE_POS)); + CustomTriggers.toggleIntakeUp( + driverController.leftBumper(), + () -> RobotState.getInstance().intakePosition == IntakePosition.DEPLOYED) + .and(() -> !operatorController.pov(180).getAsBoolean()) + .and(() -> RobotState.getInstance().shooterAtSpeed) + .toggleOnTrue(intake.slowlyBringInIntakeWithoutRollers()); CustomTriggers.toggleIntakeDown( driverController.leftBumper(), () -> RobotState.getInstance().intakePosition == IntakePosition.STOWED) @@ -337,7 +343,9 @@ private void configureButtonBindings() { .and(operatorController.pov(180)) .whileTrue(intakeExtend.runIntakeExtendVolts(4)) .onFalse(intakeExtend.stopExtendingCommand()); - operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())); + operatorController + .leftTrigger(0.1) + .toggleOnTrue(orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())); operatorController .rightTrigger(0.1) .and(() -> !operatorController.pov(0).getAsBoolean()) diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index d098be0ab..ea0c59b27 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -1,5 +1,7 @@ package frc.robot.commands.auto; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -15,8 +17,6 @@ import frc.robot.subsystems.shooter.ShooterConstants; import java.util.function.Supplier; -import static edu.wpi.first.units.Units.Meters; - /** Contains all autos */ public class Autos { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 4d405d38d..3add398c2 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -98,7 +98,7 @@ public Drive( @Override public void periodic() { - logCameraPositions(); // uncomment to show camera positions in advantage scope +// logCameraPositions(); // uncomment to show camera positions in advantage scope odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index a469e5c73..98c726bb5 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -61,7 +61,7 @@ public class DriveConstants { public static final SwerveModuleConstants.ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; public static final int driveMotorCurrentLimit = 50; - public static final double wheelRadiusMeters = Units.inchesToMeters(2); + public static final double wheelRadiusMeters = Units.inchesToMeters(1.97); public static final double driveMotorReduction = 8.16; public static final DCMotor driveGearbox = DCMotor.getKrakenX60Foc(1); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 4d41fba74..1a58f9403 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -30,7 +30,18 @@ public Command runIntakeMotor() { } public Command slowlyBringInIntake() { - return Commands.parallel(rollers.intake(), extend.runIntakeExtendVolts(SLOW_VOLTS)) + return Commands.parallel( + rollers.intake(), + extend + .runIntakeExtendVolts(SLOW_VOLTS) + .until(extend::isHopperCollapsed) + .andThen(extend.extendToAngle(COLLAPSE_POS).repeatedly())); + } + + public Command slowlyBringInIntakeWithoutRollers() { + return extend + .runIntakeExtendVolts(SLOW_VOLTS) + .until(extend::isHopperCollapsed) .andThen(extend.extendToAngle(COLLAPSE_POS).repeatedly()); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 263ce3b4d..c78453011 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -29,7 +29,7 @@ public class IntakeConstants { public static final double SHAKE_POS_OFFSET = 1; public static final double HOME_VOLTAGE = 3; public static final double COLLAPSE_POS = 4.0; - public static final double SLOW_VOLTS = 2; + public static final double SLOW_VOLTS = 1.2; // How close we need to be to collapse position to stop the intake rollers public static final double SAFETY_TOLERANCE = 2; public static final double POSITION_TOLERANCE = 7; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 80c857bb0..5ca4b2f92 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -245,7 +245,8 @@ public Command setTargetVelocityRepeatedly(AngularVelocity velocity) { */ public boolean isAtSetpoint() { return Math.abs(inputs.shooterWheelVelocityRadPerSec - (targetSpeed.in(RadiansPerSecond))) - < TOLERANCE; + < TOLERANCE + && targetSpeed.gt(RadiansPerSecond.of(0)); } // TODO: empirically determine the relationship between distance and air time diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 463e6ce91..1454e21dc 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -33,7 +33,7 @@ public class VisionConstants { new Rotation3d( Units.degreesToRadians(0.0), Units.degreesToRadians(0.0), - Units.degreesToRadians(-158.771))); + Units.degreesToRadians(201.229))); public static Transform3d robotToCamera1 = new Transform3d( new Translation3d( From 157c6141be5109cc96da1ff823d569eb7cb37a08 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Thu, 16 Apr 2026 13:31:15 -0500 Subject: [PATCH 03/25] less far A cycle 1 --- .../deploy/pathplanner/paths/A-Cycle1.path | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index 6c49fd076..8c77e4679 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -16,32 +16,32 @@ }, { "anchor": { - "x": 7.783237924865832, - "y": 7.028586762075134 + "x": 7.546467236467235, + "y": 7.01957264957265 }, "prevControl": { - "x": 7.767012522361359, - "y": 7.531574239713775 + "x": 7.530241833962762, + "y": 7.522560127211291 }, "nextControl": { - "x": 7.817452345049281, - "y": 5.967939736388225 + "x": 7.580681656650684, + "y": 5.958925623885741 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.783237924865832, - "y": 4.805706618962433 + "x": 7.546467236467235, + "y": 4.861894586894587 }, "prevControl": { - "x": 7.77903133903134, - "y": 5.171980056980058 + "x": 7.542260650632743, + "y": 5.228168024912211 }, "nextControl": { - "x": 7.786250707881794, - "y": 4.543379297786823 + "x": 7.549480019483197, + "y": 4.599567265718976 }, "isLocked": false, "linkedName": null From f8e24d03632431a3015da24ccfbc7eef1a2b75c6 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Thu, 16 Apr 2026 13:33:20 -0500 Subject: [PATCH 04/25] B cycles 2.0 --- .../deploy/pathplanner/paths/B-Cycle1.path | 40 +++++++++---------- .../deploy/pathplanner/paths/B-Cycle2.path | 16 ++++---- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index 632dc384e..51035c4ba 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -8,55 +8,55 @@ }, "prevControl": null, "nextControl": { - "x": 5.438196256563948, - "y": 0.6560842522924792 + "x": 5.46631054131054, + "y": 0.7920227920227922 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.783237924865832, - "y": 1.0404132379248665 + "x": 7.533547008547009, + "y": 1.1021082621082632 }, "prevControl": { - "x": 7.767012522361359, - "y": 0.5374257602862258 + "x": 7.517321606042536, + "y": 0.5991207844696225 }, "nextControl": { - "x": 7.817452345049281, - "y": 2.1010602636117754 + "x": 7.567761428730458, + "y": 2.1627552877951723 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.783237924865832, - "y": 3.2632933810375677 + "x": 7.533547008547009, + "y": 3.2339458689458693 }, "prevControl": { - "x": 7.77903133903134, - "y": 2.8970199430199433 + "x": 7.529340422712517, + "y": 2.867672430928245 }, "nextControl": { - "x": 7.786250707881794, - "y": 3.5256207022131782 + "x": 7.536559791562971, + "y": 3.49627319012148 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.797094017094016, + "x": 6.6291310541310535, "y": 2.4577321937321948 }, "prevControl": { - "x": 7.802016105528991, + "x": 7.634053142566028, "y": 2.5084858345622454 }, "nextControl": { - "x": 5.517991452991453, + "x": 5.35002849002849, "y": 2.3931310541310538 }, "isLocked": false, @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 2.533418803418803, + "x": 2.287934472934473, "y": 2.4577321937321948 }, "prevControl": { - "x": 3.334472934472934, + "x": 3.0889886039886036, "y": 2.3931310541310538 }, "nextControl": null, @@ -142,7 +142,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -37.18470645323313 + "rotation": 38.53360770053578 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index a28ef683d..060685c06 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.533418803418803, + "x": 2.287934472934473, "y": 2.4577321937321948 }, "prevControl": null, "nextControl": { - "x": 2.267699809926946, + "x": 2.0222154794426155, "y": 1.9082942897883557 }, "isLocked": false, @@ -112,12 +112,12 @@ }, { "anchor": { - "x": 2.533418803418803, - "y": 2.4577321937321948 + "x": 2.287934472934473, + "y": 2.458000000000001 }, "prevControl": { - "x": 3.476099715099715, - "y": 2.4445441595441597 + "x": 3.2306153846153847, + "y": 2.444811965811966 }, "nextControl": null, "isLocked": false, @@ -207,13 +207,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 52.804773309737485 + "rotation": 40.52112194243585 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -37.18470645323313 + "rotation": 38.53360770053578 }, "useDefaultConstraints": true } \ No newline at end of file From 024cbe44b83f6d123a9498e6265b0f8db4c17dc0 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Thu, 16 Apr 2026 13:34:24 -0500 Subject: [PATCH 05/25] B cycles 2.0 2.0 --- .../deploy/pathplanner/paths/B-Cycle1.path | 40 +++++++++---------- .../deploy/pathplanner/paths/B-Cycle2.path | 16 ++++---- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index 51035c4ba..cce173afd 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -8,55 +8,55 @@ }, "prevControl": null, "nextControl": { - "x": 5.46631054131054, - "y": 0.7920227920227922 + "x": 5.438196256563948, + "y": 0.6560842522924792 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.533547008547009, - "y": 1.1021082621082632 + "x": 7.783237924865832, + "y": 1.0404132379248665 }, "prevControl": { - "x": 7.517321606042536, - "y": 0.5991207844696225 + "x": 7.767012522361359, + "y": 0.5374257602862258 }, "nextControl": { - "x": 7.567761428730458, - "y": 2.1627552877951723 + "x": 7.817452345049281, + "y": 2.1010602636117754 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.533547008547009, - "y": 3.2339458689458693 + "x": 7.783237924865832, + "y": 3.2632933810375677 }, "prevControl": { - "x": 7.529340422712517, - "y": 2.867672430928245 + "x": 7.77903133903134, + "y": 2.8970199430199433 }, "nextControl": { - "x": 7.536559791562971, - "y": 3.49627319012148 + "x": 7.786250707881794, + "y": 3.5256207022131782 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.6291310541310535, + "x": 6.797094017094016, "y": 2.4577321937321948 }, "prevControl": { - "x": 7.634053142566028, + "x": 7.802016105528991, "y": 2.5084858345622454 }, "nextControl": { - "x": 5.35002849002849, + "x": 5.517991452991453, "y": 2.3931310541310538 }, "isLocked": false, @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 2.287934472934473, + "x": 2.533418803418803, "y": 2.4577321937321948 }, "prevControl": { - "x": 3.0889886039886036, + "x": 3.334472934472934, "y": 2.3931310541310538 }, "nextControl": null, @@ -142,7 +142,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 38.53360770053578 + "rotation": 42.70997766084689 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index 060685c06..bea52f0a1 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.287934472934473, + "x": 2.533418803418803, "y": 2.4577321937321948 }, "prevControl": null, "nextControl": { - "x": 2.0222154794426155, + "x": 2.267699809926946, "y": 1.9082942897883557 }, "isLocked": false, @@ -112,12 +112,12 @@ }, { "anchor": { - "x": 2.287934472934473, - "y": 2.458000000000001 + "x": 2.533418803418803, + "y": 2.4577321937321948 }, "prevControl": { - "x": 3.2306153846153847, - "y": 2.444811965811966 + "x": 3.476099715099715, + "y": 2.4445441595441597 }, "nextControl": null, "isLocked": false, @@ -207,13 +207,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 40.52112194243585 + "rotation": 40.99869009692334 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 38.53360770053578 + "rotation": 42.70997766084689 }, "useDefaultConstraints": true } \ No newline at end of file From f54498ec8883b6e95be8acaf6740aa8cedb759ca Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Thu, 16 Apr 2026 13:48:50 -0500 Subject: [PATCH 06/25] B cycles 2.0 2.0 --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 40dcad6f1..9a2deb461 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -299,7 +299,7 @@ private void configureButtonBindings() { .ignoringDisable(true)); new Trigger(() -> driverController.getHID().getPOV() != -1) .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV())); - driverController.x().toggleOnTrue(orchestrator.zoneBasedAim()); + driverController.x().toggleOnTrue(orchestrator.aimToHub()); driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS)); driverController .leftBumper() From 93d63ceaaab213c435b255ca2b96d4de5459eccc Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Thu, 16 Apr 2026 17:05:18 -0500 Subject: [PATCH 07/25] general changes --- .../deploy/pathplanner/paths/A-Cycle1.path | 4 ++-- .../deploy/pathplanner/paths/A-Cycle2.path | 24 +++++++++++++++---- src/main/java/frc/robot/Orchestrator.java | 7 +----- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../subsystems/drive/DriveConstants.java | 2 +- 6 files changed, 26 insertions(+), 15 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index 8c77e4679..7cd3a5125 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 2.533418803418803, + "x": 2.300854700854701, "y": 5.611267806267806 }, "prevControl": { - "x": 3.334472934472934, + "x": 3.101908831908832, "y": 5.675868945868947 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index 811891d20..ab5b62cb6 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.533418803418803, + "x": 2.300854700854701, "y": 5.611267806267806 }, "prevControl": null, "nextControl": { - "x": 2.267699809926946, + "x": 2.0351357073628438, "y": 6.160705710211645 }, "isLocked": false, @@ -112,13 +112,29 @@ }, { "anchor": { - "x": 2.533418803418803, + "x": 2.1974928774928775, "y": 5.611267806267806 }, "prevControl": { - "x": 3.476099715099715, + "x": 3.1401737891737893, "y": 5.624455840455841 }, + "nextControl": { + "x": 1.2548119658119656, + "y": 5.598079772079771 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.300854700854701, + "y": 5.611267806267806 + }, + "prevControl": { + "x": 1.8295142450142452, + "y": 5.585293447293447 + }, "nextControl": null, "isLocked": false, "linkedName": null diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index a95049840..09ac24292 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -12,7 +12,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -33,7 +32,6 @@ import frc.robot.subsystems.intake.rollers.IntakeRollers; import frc.robot.subsystems.magicCarpet.MagicCarpet; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.util.ShooterLeadCompensator; import frc.robot.util.Zone; import frc.robot.util.Zone.Tuple2d; @@ -233,10 +231,7 @@ public Supplier getHubDistance() { Meters.of( AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()) .getDistance( - drive - .getPose() - .transformBy(kShooterOffsetFromRobotCenter) - .getTranslation())); + drive.getPose().transformBy(kShooterOffsetFromRobotCenter).getTranslation())); } private Rotation2d filteredHubAngle(Rotation2d raw) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9a2deb461..40dcad6f1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -299,7 +299,7 @@ private void configureButtonBindings() { .ignoringDisable(true)); new Trigger(() -> driverController.getHID().getPOV() != -1) .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV())); - driverController.x().toggleOnTrue(orchestrator.aimToHub()); + driverController.x().toggleOnTrue(orchestrator.zoneBasedAim()); driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS)); driverController .leftBumper() diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3add398c2..09d8613f1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -98,7 +98,7 @@ public Drive( @Override public void periodic() { -// logCameraPositions(); // uncomment to show camera positions in advantage scope + // logCameraPositions(); // uncomment to show camera positions in advantage scope odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 98c726bb5..219a39371 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -61,7 +61,7 @@ public class DriveConstants { public static final SwerveModuleConstants.ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; public static final int driveMotorCurrentLimit = 50; - public static final double wheelRadiusMeters = Units.inchesToMeters(1.97); + public static final double wheelRadiusMeters = Units.inchesToMeters(1.98); public static final double driveMotorReduction = 8.16; public static final DCMotor driveGearbox = DCMotor.getKrakenX60Foc(1); From e8bf50c6d3e22454fcffb263cc97efd25bb060d2 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Thu, 16 Apr 2026 18:21:28 -0400 Subject: [PATCH 08/25] hub distance align 70 in --- src/main/java/frc/robot/Orchestrator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 09ac24292..895d17556 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -54,7 +54,7 @@ public enum ZoneId { ZONE_4 } - private final double FRONT_HUB_OFFSET = Units.inchesToMeters(58.0); + private final double FRONT_HUB_OFFSET = Units.inchesToMeters(70.0); private final Drive drive; private final Shooter shooter; private final MagicCarpet magicCarpet; From 397a842d2f0b35d36cf7b6e6dac2ac50a34e8268 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Thu, 16 Apr 2026 18:21:59 -0500 Subject: [PATCH 09/25] new regression --- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 5ca4b2f92..9f97aea82 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -267,7 +267,7 @@ public Supplier calculateSetpoint(Supplier distance) AngularVelocity velocity = RPM.of( 25.30184 * Math.pow(distance.get().in(Meters), 2) - - 47.12642 * distance.get().in(Meters) + - 65.12642 * distance.get().in(Meters) + 1001.70713); if (velocity.gt(RPM.of(1550))) { return RPM.of(1550); From a267b9f343422624b9b326801a2ac5f2d52116f9 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Thu, 16 Apr 2026 19:24:05 -0400 Subject: [PATCH 10/25] new paths --- .../deploy/pathplanner/paths/A-Cycle1.path | 42 ++++++++--------- .../deploy/pathplanner/paths/B-Cycle1.path | 46 +++++++++---------- .../deploy/pathplanner/paths/B-Cycle2.path | 10 ++-- 3 files changed, 49 insertions(+), 49 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index 7cd3a5125..ab4db31fa 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -16,32 +16,32 @@ }, { "anchor": { - "x": 7.546467236467235, - "y": 7.01957264957265 + "x": 6.797094017094016, + "y": 7.077262969588551 }, "prevControl": { - "x": 7.530241833962762, - "y": 7.522560127211291 + "x": 6.780868614589544, + "y": 7.580250447227192 }, "nextControl": { - "x": 7.580681656650684, - "y": 5.958925623885741 + "x": 6.831308437277466, + "y": 6.016615943901642 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.546467236467235, - "y": 4.861894586894587 + "x": 6.797094017094016, + "y": 4.49742397137746 }, "prevControl": { - "x": 7.542260650632743, - "y": 5.228168024912211 + "x": 6.792887431259524, + "y": 4.863697409395084 }, "nextControl": { - "x": 7.549480019483197, - "y": 4.599567265718976 + "x": 6.800106800109979, + "y": 4.235096650201849 }, "isLocked": false, "linkedName": null @@ -52,23 +52,23 @@ "y": 5.611267806267806 }, "prevControl": { - "x": 7.802016105528991, - "y": 5.5605141654377555 + "x": 7.069320214665453, + "y": 5.616976744193628 }, "nextControl": { - "x": 5.517991452991453, - "y": 5.675868945868947 + "x": 5.516642687480301, + "y": 5.584415074153799 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.300854700854701, + "x": 2.533, "y": 5.611267806267806 }, "prevControl": { - "x": 3.101908831908832, + "x": 3.3340541310541307, "y": 5.675868945868947 }, "nextControl": null, @@ -91,15 +91,15 @@ }, { "waypointRelativePos": 3.0, - "rotationDegrees": 0.0 + "rotationDegrees": -139.08745511543628 }, { - "waypointRelativePos": 3.45, + "waypointRelativePos": 3.2406417112299466, "rotationDegrees": 0.0 }, { "waypointRelativePos": 3.7526652452025653, - "rotationDegrees": 0.0 + "rotationDegrees": -0.15502232751999445 } ], "constraintZones": [ diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index cce173afd..b0625131f 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -16,32 +16,32 @@ }, { "anchor": { - "x": 7.783237924865832, - "y": 1.0404132379248665 + "x": 6.923291592128802, + "y": 0.6996797853309484 }, "prevControl": { - "x": 7.767012522361359, - "y": 0.5374257602862258 + "x": 6.923291592128802, + "y": 0.44967978533094843 }, "nextControl": { - "x": 7.817452345049281, - "y": 2.1010602636117754 + "x": 6.923291592128802, + "y": 1.7608785130191102 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.783237924865832, - "y": 3.2632933810375677 + "x": 6.923291592128802, + "y": 3.6699284436493738 }, "prevControl": { - "x": 7.77903133903134, - "y": 2.8970199430199433 + "x": 6.91908500629431, + "y": 3.3036550056317493 }, "nextControl": { - "x": 7.786250707881794, - "y": 3.5256207022131782 + "x": 6.926304375144764, + "y": 3.9322557648249843 }, "isLocked": false, "linkedName": null @@ -52,28 +52,28 @@ "y": 2.4577321937321948 }, "prevControl": { - "x": 7.802016105528991, - "y": 2.5084858345622454 + "x": 7.625712719298246, + "y": 2.4387061403508765 }, "nextControl": { - "x": 5.517991452991453, - "y": 2.3931310541310538 + "x": 5.516698627557278, + "y": 2.4871315676127463 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.533418803418803, + "x": 2.533, "y": 2.4577321937321948 }, "prevControl": { - "x": 3.334472934472934, + "x": 3.3340541310541307, "y": 2.3931310541310538 }, "nextControl": null, "isLocked": false, - "linkedName": "A-Cycle1-End" + "linkedName": "B-Cycle-End" } ], "rotationTargets": [ @@ -90,11 +90,11 @@ "rotationDegrees": 90.0 }, { - "waypointRelativePos": 3, - "rotationDegrees": 0.0 + "waypointRelativePos": 3.1069182389937122, + "rotationDegrees": 138.54887123860425 }, { - "waypointRelativePos": 3.45, + "waypointRelativePos": 3.2327044025157234, "rotationDegrees": 0.0 }, { @@ -142,7 +142,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 42.70997766084689 + "rotation": 38.60859722705431 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index bea52f0a1..63a54f5a5 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 2.533418803418803, + "x": 2.533, "y": 2.4577321937321948 }, "prevControl": null, "nextControl": { - "x": 2.267699809926946, + "x": 2.2672810065081426, "y": 1.9082942897883557 }, "isLocked": false, - "linkedName": "A-Cycle1-End" + "linkedName": "B-Cycle-End" }, { "anchor": { @@ -207,13 +207,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 40.99869009692334 + "rotation": 52.804773309737485 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 42.70997766084689 + "rotation": 38.60859722705431 }, "useDefaultConstraints": true } \ No newline at end of file From 033809921a2ea64aa5040abd5bba1b902df40f4d Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Thu, 16 Apr 2026 19:37:25 -0400 Subject: [PATCH 11/25] new paths --- .../deploy/pathplanner/paths/A-Cycle1.path | 10 ++-- .../deploy/pathplanner/paths/A-Cycle2.path | 26 ++------- .../deploy/pathplanner/paths/B-Cycle1.path | 54 +++++++++---------- .../java/frc/robot/commands/auto/Autos.java | 2 +- 4 files changed, 38 insertions(+), 54 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index ab4db31fa..eb836a792 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -65,15 +65,15 @@ { "anchor": { "x": 2.533, - "y": 5.611267806267806 + "y": 5.611 }, "prevControl": { "x": 3.3340541310541307, - "y": 5.675868945868947 + "y": 5.675601139601141 }, "nextControl": null, "isLocked": false, - "linkedName": "A-Cycle1-End" + "linkedName": "ACycleEnd" } ], "rotationTargets": [ @@ -99,7 +99,7 @@ }, { "waypointRelativePos": 3.7526652452025653, - "rotationDegrees": -0.15502232751999445 + "rotationDegrees": -0.15502232751999448 } ], "constraintZones": [ @@ -142,7 +142,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -37.18470645323313 + "rotation": -38.82347239526609 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index ab5b62cb6..8b6b2ff57 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 2.300854700854701, + "x": 2.533, "y": 5.611267806267806 }, "prevControl": null, "nextControl": { - "x": 2.0351357073628438, + "x": 2.2672810065081426, "y": 6.160705710211645 }, "isLocked": false, - "linkedName": "A-Cycle1-End" + "linkedName": "ACycleEnd" }, { "anchor": { @@ -110,22 +110,6 @@ "isLocked": false, "linkedName": null }, - { - "anchor": { - "x": 2.1974928774928775, - "y": 5.611267806267806 - }, - "prevControl": { - "x": 3.1401737891737893, - "y": 5.624455840455841 - }, - "nextControl": { - "x": 1.2548119658119656, - "y": 5.598079772079771 - }, - "isLocked": false, - "linkedName": null - }, { "anchor": { "x": 2.300854700854701, @@ -200,7 +184,7 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 4.442943956785955, - "maxWaypointRelativePos": 6.4280891289669135, + "maxWaypointRelativePos": 6.2, "constraints": { "maxVelocity": 2.3, "maxAcceleration": 6.0, @@ -229,7 +213,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -37.18470645323313 + "rotation": -38.82347239526609 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index b0625131f..555506510 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -16,32 +16,32 @@ }, { "anchor": { - "x": 6.923291592128802, - "y": 0.6996797853309484 + "x": 6.797094017094016, + "y": 0.9917370304114499 }, "prevControl": { - "x": 6.923291592128802, - "y": 0.44967978533094843 + "x": 6.780868614589544, + "y": 0.4887495527728092 }, "nextControl": { - "x": 6.923291592128802, - "y": 1.7608785130191102 + "x": 6.831308437277466, + "y": 2.052384056098359 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.923291592128802, - "y": 3.6699284436493738 + "x": 6.797094017094016, + "y": 3.571576028622541 }, "prevControl": { - "x": 6.91908500629431, - "y": 3.3036550056317493 + "x": 6.792887431259524, + "y": 3.2053025906049166 }, "nextControl": { - "x": 6.926304375144764, - "y": 3.9322557648249843 + "x": 6.800106800109979, + "y": 3.8339033497981516 }, "isLocked": false, "linkedName": null @@ -52,12 +52,12 @@ "y": 2.4577321937321948 }, "prevControl": { - "x": 7.625712719298246, - "y": 2.4387061403508765 + "x": 7.069320214665453, + "y": 2.452023255806373 }, "nextControl": { - "x": 5.516698627557278, - "y": 2.4871315676127463 + "x": 5.516642687480301, + "y": 2.4845849258462023 }, "isLocked": false, "linkedName": null @@ -73,33 +73,33 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "B-Cycle-End" + "linkedName": null } ], "rotationTargets": [ { "waypointRelativePos": 0.29946524064171126, - "rotationDegrees": 0.0 + "rotationDegrees": 0 }, { "waypointRelativePos": 1.171122994652407, - "rotationDegrees": 90.0 + "rotationDegrees": -90 }, { "waypointRelativePos": 1.9411764705882377, - "rotationDegrees": 90.0 + "rotationDegrees": -90 }, { - "waypointRelativePos": 3.1069182389937122, - "rotationDegrees": 138.54887123860425 + "waypointRelativePos": 3, + "rotationDegrees": -139.08745511543628 }, { - "waypointRelativePos": 3.2327044025157234, - "rotationDegrees": 0.0 + "waypointRelativePos": 3.2406417112299466, + "rotationDegrees": 0 }, { "waypointRelativePos": 3.7526652452025653, - "rotationDegrees": 0.0 + "rotationDegrees": -0.15502232751999448 } ], "constraintZones": [ @@ -142,13 +142,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 38.60859722705431 + "rotation": 52.81529354676687 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index ea0c59b27..71575c5c0 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -156,7 +156,7 @@ private Command ppCycleRegression(String path, Supplier startPose) { drive.getAutonomousCommand(path), intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), Commands.waitSeconds(5) - .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(2.62)))) + .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(2.6201)))) .withTimeout(15.5), Commands.deadline( orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), From c95cba8eb5ba59bf132ff198311a39afa98ac453 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Fri, 17 Apr 2026 00:12:09 -0400 Subject: [PATCH 12/25] made first cycle go further back in the end --- .../deploy/pathplanner/paths/A-Cycle1.path | 4 +- .../deploy/pathplanner/paths/A-Cycle2.path | 16 +++---- .../deploy/pathplanner/paths/B-Cycle1.path | 44 +++++++++---------- .../deploy/pathplanner/paths/B-Cycle2.path | 18 ++++---- 4 files changed, 41 insertions(+), 41 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index eb836a792..cdd4da55c 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 2.533, + "x": 2.1, "y": 5.611 }, "prevControl": { - "x": 3.3340541310541307, + "x": 2.901054131054131, "y": 5.675601139601141 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index 8b6b2ff57..a3586e1c8 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -3,28 +3,28 @@ "waypoints": [ { "anchor": { - "x": 2.533, - "y": 5.611267806267806 + "x": 2.1, + "y": 5.611 }, "prevControl": null, "nextControl": { - "x": 2.2672810065081426, - "y": 6.160705710211645 + "x": 1.8342810065081427, + "y": 6.160437903943839 }, "isLocked": false, "linkedName": "ACycleEnd" }, { "anchor": { - "x": 2.5075783475783475, + "x": 3.2401252236135956, "y": 7.447760964912282 }, "prevControl": { - "x": 2.0694924799575967, + "x": 2.8020393559928447, "y": 7.351408549885447 }, "nextControl": { - "x": 2.7903904157463497, + "x": 3.5229372917815978, "y": 7.509962528077773 }, "isLocked": false, @@ -207,7 +207,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -37.195226690262515 + "rotation": -33.52008653915619 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index 555506510..06799bedd 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -4,12 +4,12 @@ { "anchor": { "x": 4.424579606440071, - "y": 0.6996797853309484 + "y": 0.6443797853309476 }, "prevControl": null, "nextControl": { "x": 5.438196256563948, - "y": 0.6560842522924792 + "y": 0.6007842522924784 }, "isLocked": false, "linkedName": null @@ -17,15 +17,15 @@ { "anchor": { "x": 6.797094017094016, - "y": 0.9917370304114499 + "y": 0.9364370304114491 }, "prevControl": { "x": 6.780868614589544, - "y": 0.4887495527728092 + "y": 0.4334495527728084 }, "nextControl": { "x": 6.831308437277466, - "y": 2.052384056098359 + "y": 1.997084056098358 }, "isLocked": false, "linkedName": null @@ -33,15 +33,15 @@ { "anchor": { "x": 6.797094017094016, - "y": 3.571576028622541 + "y": 3.5162760286225403 }, "prevControl": { "x": 6.792887431259524, - "y": 3.2053025906049166 + "y": 3.150002590604916 }, "nextControl": { "x": 6.800106800109979, - "y": 3.8339033497981516 + "y": 3.778603349798151 }, "isLocked": false, "linkedName": null @@ -49,57 +49,57 @@ { "anchor": { "x": 6.797094017094016, - "y": 2.4577321937321948 + "y": 2.402432193732194 }, "prevControl": { "x": 7.069320214665453, - "y": 2.452023255806373 + "y": 2.3967232558063722 }, "nextControl": { "x": 5.516642687480301, - "y": 2.4845849258462023 + "y": 2.4292849258462015 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.533, + "x": 2.1, "y": 2.4577321937321948 }, "prevControl": { - "x": 3.3340541310541307, + "x": 2.901054131054131, "y": 2.3931310541310538 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "BCycleEnd" } ], "rotationTargets": [ { "waypointRelativePos": 0.29946524064171126, - "rotationDegrees": 0 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 1.171122994652407, - "rotationDegrees": -90 + "rotationDegrees": 90.0 }, { "waypointRelativePos": 1.9411764705882377, - "rotationDegrees": -90 + "rotationDegrees": 90.0 }, { "waypointRelativePos": 3, - "rotationDegrees": -139.08745511543628 + "rotationDegrees": 139.08745511543628 }, { "waypointRelativePos": 3.2406417112299466, - "rotationDegrees": 0 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 3.7526652452025653, - "rotationDegrees": -0.15502232751999448 + "rotationDegrees": 0.15502232751999448 } ], "constraintZones": [ @@ -142,13 +142,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 52.81529354676687 + "rotation": 30.68666362424798 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index 63a54f5a5..afa07a318 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -3,28 +3,28 @@ "waypoints": [ { "anchor": { - "x": 2.533, + "x": 2.1, "y": 2.4577321937321948 }, "prevControl": null, "nextControl": { - "x": 2.2672810065081426, + "x": 1.8342810065081427, "y": 1.9082942897883557 }, "isLocked": false, - "linkedName": "B-Cycle-End" + "linkedName": "BCycleEnd" }, { "anchor": { - "x": 2.5075783475783475, + "x": 3.094096601073345, "y": 0.6212390350877186 }, "prevControl": { - "x": 2.0694924799575967, + "x": 2.656010733452594, "y": 0.717591450114554 }, "nextControl": { - "x": 2.7903904157463497, + "x": 3.376908669241347, "y": 0.5590374719222275 }, "isLocked": false, @@ -150,7 +150,7 @@ "rotationDegrees": 0.0 }, { - "waypointRelativePos": 5.734276729559759, + "waypointRelativePos": 6.344919786096259, "rotationDegrees": 0.0 } ], @@ -207,13 +207,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 52.804773309737485 + "rotation": 32.614505469234345 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 38.60859722705431 + "rotation": 30.68666362424798 }, "useDefaultConstraints": true } \ No newline at end of file From 57a3472c634a3477406a9e08cc8ac6a8f61c5ca9 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Fri, 17 Apr 2026 00:20:42 -0400 Subject: [PATCH 13/25] updates pre-spin assumed distance from 2.6 -> 2.977. Accoutns for further back ending pose on cycle 1 --- src/main/java/frc/robot/commands/auto/Autos.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 71575c5c0..cd62ea680 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -156,7 +156,7 @@ private Command ppCycleRegression(String path, Supplier startPose) { drive.getAutonomousCommand(path), intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), Commands.waitSeconds(5) - .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(2.6201)))) + .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(2.9774)))) .withTimeout(15.5), Commands.deadline( orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), From 5f4669f5c81991b060f3ece58b72bc804c57c943 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Fri, 17 Apr 2026 00:37:25 -0400 Subject: [PATCH 14/25] added easily adjustable regression variable (tunable numbers) --- .../java/frc/robot/subsystems/shooter/Shooter.java | 12 +++++++++--- .../robot/subsystems/shooter/ShooterConstants.java | 3 +++ 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9f97aea82..38a683f05 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -3,6 +3,9 @@ import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Volts; +import static frc.robot.subsystems.shooter.ShooterConstants.ACONST; +import static frc.robot.subsystems.shooter.ShooterConstants.BCONST; +import static frc.robot.subsystems.shooter.ShooterConstants.CCONST; import static frc.robot.subsystems.shooter.ShooterConstants.FF_SCALAR; import static frc.robot.subsystems.shooter.ShooterConstants.KA; import static frc.robot.subsystems.shooter.ShooterConstants.KS; @@ -57,6 +60,9 @@ public Shooter(ShooterIO io) { SmartDashboard.putNumber("Shooter/KA", KA * FF_SCALAR); SmartDashboard.putNumber("Shooter/KV", KV * FF_SCALAR); SmartDashboard.putNumber("Shooter/FeedForward_Scalar", FF_SCALAR); + SmartDashboard.putNumber("Shooter/AConst",ACONST ); + SmartDashboard.putNumber("Shooter/BCONST", BCONST); + SmartDashboard.putNumber("Shooter/CCONST", CCONST); feedForwardScalar = FF_SCALAR; this.io = io; sysId = @@ -266,9 +272,9 @@ public Supplier calculateSetpoint(Supplier distance) return () -> { AngularVelocity velocity = RPM.of( - 25.30184 * Math.pow(distance.get().in(Meters), 2) - - 65.12642 * distance.get().in(Meters) - + 1001.70713); + SmartDashboard.getNumber("Shooter/ACONST",25.30184) * Math.pow(distance.get().in(Meters), 2) + - SmartDashboard.getNumber("Shooter/BCONST",- 65.12642) * distance.get().in(Meters) + + SmartDashboard.getNumber("Shooter/CCONST",1001.70713)); if (velocity.gt(RPM.of(1550))) { return RPM.of(1550); } else return velocity; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index f784e5bea..54860af3b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -9,6 +9,9 @@ public class ShooterConstants { public static final double ENCODER_POSITION_CONVERSION = 1; public static final double ENCODER_VELOCITY_CONVERSION = (2 * Math.PI) / 60; + public static final double ACONST =25.30184; + public static final double BCONST = - 65.12642; + public static final double CCONST = 1001.70713; public static final double VOLTAGE_COMPENSATION = 12.0; public static final int CURRENT_LIMIT = 35; From d0ff9572e93ad2cbe25f3f7c6be6bf505ef34e60 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Fri, 17 Apr 2026 00:39:27 -0400 Subject: [PATCH 15/25] built formatting --- .../java/frc/robot/subsystems/shooter/Shooter.java | 10 ++++++---- .../frc/robot/subsystems/shooter/ShooterConstants.java | 4 ++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 38a683f05..5feb1d721 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -60,7 +60,7 @@ public Shooter(ShooterIO io) { SmartDashboard.putNumber("Shooter/KA", KA * FF_SCALAR); SmartDashboard.putNumber("Shooter/KV", KV * FF_SCALAR); SmartDashboard.putNumber("Shooter/FeedForward_Scalar", FF_SCALAR); - SmartDashboard.putNumber("Shooter/AConst",ACONST ); + SmartDashboard.putNumber("Shooter/AConst", ACONST); SmartDashboard.putNumber("Shooter/BCONST", BCONST); SmartDashboard.putNumber("Shooter/CCONST", CCONST); feedForwardScalar = FF_SCALAR; @@ -272,9 +272,11 @@ public Supplier calculateSetpoint(Supplier distance) return () -> { AngularVelocity velocity = RPM.of( - SmartDashboard.getNumber("Shooter/ACONST",25.30184) * Math.pow(distance.get().in(Meters), 2) - - SmartDashboard.getNumber("Shooter/BCONST",- 65.12642) * distance.get().in(Meters) - + SmartDashboard.getNumber("Shooter/CCONST",1001.70713)); + SmartDashboard.getNumber("Shooter/ACONST", 25.30184) + * Math.pow(distance.get().in(Meters), 2) + - SmartDashboard.getNumber("Shooter/BCONST", -65.12642) + * distance.get().in(Meters) + + SmartDashboard.getNumber("Shooter/CCONST", 1001.70713)); if (velocity.gt(RPM.of(1550))) { return RPM.of(1550); } else return velocity; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 54860af3b..da40c170b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -9,8 +9,8 @@ public class ShooterConstants { public static final double ENCODER_POSITION_CONVERSION = 1; public static final double ENCODER_VELOCITY_CONVERSION = (2 * Math.PI) / 60; - public static final double ACONST =25.30184; - public static final double BCONST = - 65.12642; + public static final double ACONST = 25.30184; + public static final double BCONST = -65.12642; public static final double CCONST = 1001.70713; public static final double VOLTAGE_COMPENSATION = 12.0; From 573497f14b991a37fe61905634d7bc0a02c4a70a Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Fri, 17 Apr 2026 08:30:34 -0400 Subject: [PATCH 16/25] decrease path time --- src/main/deploy/pathplanner/paths/A-Cycle1.path | 4 ++-- src/main/deploy/pathplanner/paths/B-Cycle1.path | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index cdd4da55c..4c5e87b3f 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -95,11 +95,11 @@ }, { "waypointRelativePos": 3.2406417112299466, - "rotationDegrees": 0.0 + "rotationDegrees": 180.0 }, { "waypointRelativePos": 3.7526652452025653, - "rotationDegrees": -0.15502232751999448 + "rotationDegrees": 180.0 } ], "constraintZones": [ diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index 06799bedd..0bd2ab0b5 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -95,11 +95,11 @@ }, { "waypointRelativePos": 3.2406417112299466, - "rotationDegrees": 0.0 + "rotationDegrees": 180.0 }, { "waypointRelativePos": 3.7526652452025653, - "rotationDegrees": 0.15502232751999448 + "rotationDegrees": 180.0 } ], "constraintZones": [ From 109c68a9381aaf906e54ec1f5a30a378f7972460 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Fri, 17 Apr 2026 09:34:05 -0400 Subject: [PATCH 17/25] prevent beaching --- .../deploy/pathplanner/paths/A-Cycle2.path | 134 ++++++++---------- .../deploy/pathplanner/paths/B-Cycle2.path | 12 +- 2 files changed, 69 insertions(+), 77 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index a3586e1c8..81bfed561 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -4,154 +4,146 @@ { "anchor": { "x": 2.1, - "y": 5.611 + "y": 5.555967806267805 }, "prevControl": null, "nextControl": { "x": 1.8342810065081427, - "y": 6.160437903943839 + "y": 6.105405710211644 }, - "isLocked": false, - "linkedName": "ACycleEnd" + "isLocked": false }, { "anchor": { - "x": 3.2401252236135956, - "y": 7.447760964912282 + "x": 3.094096601073345, + "y": 7.392460964912281 }, "prevControl": { - "x": 2.8020393559928447, - "y": 7.351408549885447 + "x": 2.656010733452594, + "y": 7.296108549885446 }, "nextControl": { - "x": 3.5229372917815978, - "y": 7.509962528077773 + "x": 3.376908669241347, + "y": 7.4546625280777725 }, - "isLocked": false, - "linkedName": null + "isLocked": false }, { "anchor": { "x": 5.634273504273503, - "y": 7.447760964912282 + "y": 7.392460964912281 }, "prevControl": { "x": 4.144916933849193, - "y": 7.530332248967907 + "y": 7.475032248967906 }, "nextControl": { "x": 6.494219837010534, - "y": 7.400084757398864 + "y": 7.344784757398863 }, - "isLocked": false, - "linkedName": null + "isLocked": false }, { "anchor": { - "x": 6.202763532763532, - "y": 4.060840455840457 + "x": 6.1444722719141325, + "y": 4.700730411449015 }, "prevControl": { - "x": 6.215683760683759, - "y": 3.569871794871795 + "x": 6.157392499834359, + "y": 4.209761750480354 }, "nextControl": { - "x": 6.1825608953181, - "y": 4.828540678766911 + "x": 6.124269634468701, + "y": 5.468430634375471 }, - "isLocked": false, - "linkedName": null + "isLocked": false }, { "anchor": { "x": 5.866837606837606, - "y": 5.611 + "y": 5.555699999999999 }, "prevControl": { "x": 6.590370370370371, - "y": 5.624188034188035 + "y": 5.568888034188034 }, "nextControl": { "x": 5.351084085527113, - "y": 5.601599216214575 + "y": 5.546299216214575 }, - "isLocked": false, - "linkedName": null + "isLocked": false }, { "anchor": { "x": 4.755698005698005, - "y": 5.611 + "y": 5.555699999999999 }, "prevControl": { "x": 5.905598290598291, - "y": 5.598079772079772 + "y": 5.542779772079771 }, "nextControl": { "x": 4.148936797920834, - "y": 5.6178175416604175 + "y": 5.562517541660417 }, - "isLocked": false, - "linkedName": null + "isLocked": false }, { "anchor": { "x": 3.760840455840455, - "y": 5.611 + "y": 5.555699999999999 }, "prevControl": { "x": 4.6006552706552695, - "y": 5.598347578347578 + "y": 5.543047578347577 }, "nextControl": { "x": 3.34446165048371, - "y": 5.61727304986715 + "y": 5.56197304986715 }, - "isLocked": false, - "linkedName": null + "isLocked": false }, { "anchor": { - "x": 2.300854700854701, - "y": 5.611267806267806 + "x": 2.533418803418803, + "y": 5.555967806267805 }, "prevControl": { - "x": 1.8295142450142452, - "y": 5.585293447293447 + "x": 3.476099715099715, + "y": 5.56915584045584 }, "nextControl": null, - "isLocked": false, - "linkedName": null + "isLocked": false } ], "rotationTargets": [ { "waypointRelativePos": 1.1886792452830188, - "rotationDegrees": 0.0 + "rotationDegrees": 0 }, { "waypointRelativePos": 1.7264150943396226, - "rotationDegrees": 0.0 + "rotationDegrees": 0 }, { "waypointRelativePos": 2.0754716981132075, - "rotationDegrees": -90.0 + "rotationDegrees": -90 }, { "waypointRelativePos": 2.9433962264150932, - "rotationDegrees": -90.0 + "rotationDegrees": -90 }, { "waypointRelativePos": 3.9622641509434033, - "rotationDegrees": 0.0 + "rotationDegrees": 0 }, { "waypointRelativePos": 4.688679245283029, - "rotationDegrees": 0.0 + "rotationDegrees": 0 }, { - "waypointRelativePos": 5.734276729559759, - "rotationDegrees": 0.0 + "waypointRelativePos": 6.344919786096259, + "rotationDegrees": 0 } ], "constraintZones": [ @@ -174,23 +166,23 @@ "maxWaypointRelativePos": 1.380148548278184, "constraints": { "maxVelocity": 1.5, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false } }, { "name": "Constraints Zone", "minWaypointRelativePos": 4.442943956785955, - "maxWaypointRelativePos": 6.2, + "maxWaypointRelativePos": 6.4280891289669135, "constraints": { "maxVelocity": 2.3, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false } } @@ -198,22 +190,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxVelocity": 8, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -33.52008653915619 + "rotation": -32.614505469234345 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -38.82347239526609 + "rotation": 30.68666362424798 }, "useDefaultConstraints": true -} \ No newline at end of file +} diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index afa07a318..68c19e04d 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -48,16 +48,16 @@ }, { "anchor": { - "x": 6.202763532763532, - "y": 4.008159544159544 + "x": 6.1444722719141325, + "y": 3.3129695885509842 }, "prevControl": { - "x": 6.215683760683759, - "y": 4.499128205128206 + "x": 6.157392499834359, + "y": 3.803938249519646 }, "nextControl": { - "x": 6.1825608953181, - "y": 3.2404593212330894 + "x": 6.124269634468701, + "y": 2.5452693656245295 }, "isLocked": false, "linkedName": null From 8b78f48ecfee36f479a19aa38e95a44e6d4c1a7d Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Fri, 17 Apr 2026 09:57:03 -0400 Subject: [PATCH 18/25] prevent beaching --- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 5feb1d721..c5ae95899 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -274,7 +274,7 @@ public Supplier calculateSetpoint(Supplier distance) RPM.of( SmartDashboard.getNumber("Shooter/ACONST", 25.30184) * Math.pow(distance.get().in(Meters), 2) - - SmartDashboard.getNumber("Shooter/BCONST", -65.12642) + + SmartDashboard.getNumber("Shooter/BCONST", -65.12642) * distance.get().in(Meters) + SmartDashboard.getNumber("Shooter/CCONST", 1001.70713)); if (velocity.gt(RPM.of(1550))) { From ecd73b953901f91de709cf05e572186ed29d5cb0 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Fri, 17 Apr 2026 11:20:48 -0400 Subject: [PATCH 19/25] less --- src/main/deploy/pathplanner/paths/A-Cycle1.path | 4 ++-- src/main/deploy/pathplanner/paths/B-Cycle1.path | 8 ++++---- src/main/deploy/pathplanner/paths/B-Cycle2.path | 4 ++-- src/main/java/frc/robot/commands/auto/Autos.java | 8 ++++---- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index 4c5e87b3f..88d2f4567 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 2.1, + "x": 2.35, "y": 5.611 }, "prevControl": { - "x": 2.901054131054131, + "x": 3.151054131054131, "y": 5.675601139601141 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index 0bd2ab0b5..98ffcdb1e 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.424579606440071, + "x": 4.425, "y": 0.6443797853309476 }, "prevControl": null, "nextControl": { - "x": 5.438196256563948, + "x": 5.438616650123877, "y": 0.6007842522924784 }, "isLocked": false, @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 2.1, + "x": 2.35, "y": 2.4577321937321948 }, "prevControl": { - "x": 2.901054131054131, + "x": 3.151054131054131, "y": 2.3931310541310538 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index 68c19e04d..3f763a17c 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.1, + "x": 2.35, "y": 2.4577321937321948 }, "prevControl": null, "nextControl": { - "x": 1.8342810065081427, + "x": 2.0842810065081427, "y": 1.9082942897883557 }, "isLocked": false, diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index cd62ea680..9cd86c3c8 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -155,15 +155,15 @@ private Command ppCycleRegression(String path, Supplier startPose) { Commands.deadline( drive.getAutonomousCommand(path), intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), - Commands.waitSeconds(5) - .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(2.9774)))) + Commands.waitSeconds(5.7) + .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(2.769)))) .withTimeout(15.5), Commands.deadline( orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) .andThen( Commands.parallel( - Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()), + Commands.waitSeconds(0.4).andThen(intake.slowlyBringInIntake()), orchestrator.aimToHub().withTimeout(1).repeatedly(), orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())) @@ -184,7 +184,7 @@ private Command ppCycleRegressionDepo(String path, Supplier startPose) { orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) .andThen( Commands.parallel( - Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()), + Commands.waitSeconds(0.4).andThen(intake.slowlyBringInIntake()), orchestrator.aimToHub().withTimeout(1).repeatedly(), orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())) From 07f4c5b2af172b2119b00d308dfa97bcc6d1c4ef Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Fri, 17 Apr 2026 14:32:55 -0400 Subject: [PATCH 20/25] have robot get vision then go over bump for second cycle --- .../deploy/pathplanner/paths/A-Cycle2.path | 40 ++++++++------ .../deploy/pathplanner/paths/B-Cycle2.path | 52 ++++++++----------- .../java/frc/robot/commands/auto/Autos.java | 4 +- .../subsystems/intake/IntakeConstants.java | 2 +- 4 files changed, 49 insertions(+), 49 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index 81bfed561..edfc7ccfb 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -11,7 +11,8 @@ "x": 1.8342810065081427, "y": 6.105405710211644 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { @@ -26,7 +27,8 @@ "x": 3.376908669241347, "y": 7.4546625280777725 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { @@ -41,7 +43,8 @@ "x": 6.494219837010534, "y": 7.344784757398863 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { @@ -56,7 +59,8 @@ "x": 6.124269634468701, "y": 5.468430634375471 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { @@ -71,7 +75,8 @@ "x": 5.351084085527113, "y": 5.546299216214575 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { @@ -86,7 +91,8 @@ "x": 4.148936797920834, "y": 5.562517541660417 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { @@ -101,7 +107,8 @@ "x": 3.34446165048371, "y": 5.56197304986715 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { @@ -113,37 +120,38 @@ "y": 5.56915584045584 }, "nextControl": null, - "isLocked": false + "isLocked": false, + "linkedName": null } ], "rotationTargets": [ { "waypointRelativePos": 1.1886792452830188, - "rotationDegrees": 0 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 1.7264150943396226, - "rotationDegrees": 0 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 2.0754716981132075, - "rotationDegrees": -90 + "rotationDegrees": -90.0 }, { "waypointRelativePos": 2.9433962264150932, - "rotationDegrees": -90 + "rotationDegrees": -90.0 }, { "waypointRelativePos": 3.9622641509434033, - "rotationDegrees": 0 + "rotationDegrees": 180.0 }, { "waypointRelativePos": 4.688679245283029, - "rotationDegrees": 0 + "rotationDegrees": 180.0 }, { "waypointRelativePos": 6.344919786096259, - "rotationDegrees": 0 + "rotationDegrees": 180.0 } ], "constraintZones": [ @@ -208,4 +216,4 @@ "rotation": 30.68666362424798 }, "useDefaultConstraints": true -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index 3f763a17c..a44a93bc4 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -3,16 +3,15 @@ "waypoints": [ { "anchor": { - "x": 2.35, + "x": 2.1, "y": 2.4577321937321948 }, "prevControl": null, "nextControl": { - "x": 2.0842810065081427, + "x": 1.8342810065081427, "y": 1.9082942897883557 }, - "isLocked": false, - "linkedName": "BCycleEnd" + "isLocked": false }, { "anchor": { @@ -21,14 +20,13 @@ }, "prevControl": { "x": 2.656010733452594, - "y": 0.717591450114554 + "y": 0.7175914501145542 }, "nextControl": { "x": 3.376908669241347, "y": 0.5590374719222275 }, - "isLocked": false, - "linkedName": null + "isLocked": false }, { "anchor": { @@ -37,30 +35,28 @@ }, "prevControl": { "x": 4.144916933849193, - "y": 0.5386677510320939 + "y": 0.5386677510320936 }, "nextControl": { "x": 6.494219837010534, "y": 0.6689152426011367 }, - "isLocked": false, - "linkedName": null + "isLocked": false }, { "anchor": { "x": 6.1444722719141325, - "y": 3.3129695885509842 + "y": 3.3129695885509847 }, "prevControl": { "x": 6.157392499834359, - "y": 3.803938249519646 + "y": 3.8039382495196463 }, "nextControl": { "x": 6.124269634468701, - "y": 2.5452693656245295 + "y": 2.545269365624529 }, - "isLocked": false, - "linkedName": null + "isLocked": false }, { "anchor": { @@ -75,8 +71,7 @@ "x": 5.351084085527113, "y": 2.4674007837854255 }, - "isLocked": false, - "linkedName": null + "isLocked": false }, { "anchor": { @@ -91,8 +86,7 @@ "x": 4.148936797920834, "y": 2.4511824583395834 }, - "isLocked": false, - "linkedName": null + "isLocked": false }, { "anchor": { @@ -107,8 +101,7 @@ "x": 3.34446165048371, "y": 2.4517269501328505 }, - "isLocked": false, - "linkedName": null + "isLocked": false }, { "anchor": { @@ -120,38 +113,37 @@ "y": 2.4445441595441597 }, "nextControl": null, - "isLocked": false, - "linkedName": null + "isLocked": false } ], "rotationTargets": [ { "waypointRelativePos": 1.1886792452830188, - "rotationDegrees": 0.0 + "rotationDegrees": 0 }, { "waypointRelativePos": 1.7264150943396226, - "rotationDegrees": 0.0 + "rotationDegrees": 0 }, { "waypointRelativePos": 2.0754716981132075, - "rotationDegrees": 90.0 + "rotationDegrees": 90 }, { "waypointRelativePos": 2.9433962264150932, - "rotationDegrees": 90.0 + "rotationDegrees": 90 }, { "waypointRelativePos": 3.9622641509434033, - "rotationDegrees": 0.0 + "rotationDegrees": -180 }, { "waypointRelativePos": 4.688679245283029, - "rotationDegrees": 0.0 + "rotationDegrees": -180 }, { "waypointRelativePos": 6.344919786096259, - "rotationDegrees": 0.0 + "rotationDegrees": -180 } ], "constraintZones": [ diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 9cd86c3c8..629c3f90d 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -196,8 +196,8 @@ private Command ppCycleRegressionConnect(String path) { Commands.deadline( drive.getAutonomousCommand(path), intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), - Commands.waitSeconds(5) - .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) + Commands.waitSeconds(5.5) + .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(2.869)))) .withTimeout(14.5), Commands.deadline( orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index c78453011..22d24bb90 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -29,7 +29,7 @@ public class IntakeConstants { public static final double SHAKE_POS_OFFSET = 1; public static final double HOME_VOLTAGE = 3; public static final double COLLAPSE_POS = 4.0; - public static final double SLOW_VOLTS = 1.2; + public static final double SLOW_VOLTS = 2.1; // How close we need to be to collapse position to stop the intake rollers public static final double SAFETY_TOLERANCE = 2; public static final double POSITION_TOLERANCE = 7; From 69b733ebaea531fd10c40a9274a9fe94e3144c70 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Fri, 17 Apr 2026 16:24:26 -0400 Subject: [PATCH 21/25] prevent bump crash --- .../deploy/pathplanner/paths/A-Cycle2.path | 64 +++++++------- .../deploy/pathplanner/paths/B-Cycle2.path | 86 ++++++++++--------- .../java/frc/robot/commands/auto/Autos.java | 1 - 3 files changed, 79 insertions(+), 72 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index edfc7ccfb..08981c41b 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -32,15 +32,15 @@ }, { "anchor": { - "x": 5.634273504273503, + "x": 6.0146690518783545, "y": 7.392460964912281 }, "prevControl": { - "x": 4.144916933849193, + "x": 4.525312481454044, "y": 7.475032248967906 }, "nextControl": { - "x": 6.494219837010534, + "x": 6.874615384615385, "y": 7.344784757398863 }, "isLocked": false, @@ -48,76 +48,76 @@ }, { "anchor": { - "x": 6.1444722719141325, - "y": 4.700730411449015 + "x": 6.566332737030411, + "y": 4.692128801431127 }, "prevControl": { - "x": 6.157392499834359, - "y": 4.209761750480354 + "x": 6.579252964950638, + "y": 4.201160140462465 }, "nextControl": { - "x": 6.124269634468701, - "y": 5.468430634375471 + "x": 6.546130099584979, + "y": 5.459829024357583 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.866837606837606, - "y": 5.555699999999999 + "x": 6.355402504472272, + "y": 5.828 }, "prevControl": { - "x": 6.590370370370371, - "y": 5.568888034188034 + "x": 7.312701252236136, + "y": 5.746779964221824 }, "nextControl": { - "x": 5.351084085527113, - "y": 5.546299216214575 + "x": 5.841409940149118, + "y": 5.871608637911162 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.755698005698005, - "y": 5.555699999999999 + "x": 4.684186046511628, + "y": 5.828 }, "prevControl": { - "x": 5.905598290598291, - "y": 5.542779772079771 + "x": 5.722611806797854, + "y": 5.828 }, "nextControl": { - "x": 4.148936797920834, - "y": 5.562517541660417 + "x": 4.077386539147374, + "y": 5.828 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.760840455840455, - "y": 5.555699999999999 + "x": 3.6619856887298745, + "y": 5.8279069767441865 }, "prevControl": { - "x": 4.6006552706552695, - "y": 5.543047578347577 + "x": 4.501800503544689, + "y": 5.815254555091765 }, "nextControl": { - "x": 3.34446165048371, - "y": 5.56197304986715 + "x": 3.2456068833731293, + "y": 5.834180026611337 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.533418803418803, - "y": 5.555967806267805 + "x": 2.542432915921288, + "y": 5.8279069767441865 }, "prevControl": { - "x": 3.476099715099715, - "y": 5.56915584045584 + "x": 3.46006956867422, + "y": 5.824014304644539 }, "nextControl": null, "isLocked": false, @@ -207,7 +207,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -32.614505469234345 + "rotation": -49.70785224437223 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index a44a93bc4..3bff2d9bf 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -11,7 +11,8 @@ "x": 1.8342810065081427, "y": 1.9082942897883557 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { @@ -20,13 +21,14 @@ }, "prevControl": { "x": 2.656010733452594, - "y": 0.7175914501145542 + "y": 0.717591450114554 }, "nextControl": { "x": 3.376908669241347, "y": 0.5590374719222275 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { @@ -35,13 +37,14 @@ }, "prevControl": { "x": 4.144916933849193, - "y": 0.5386677510320936 + "y": 0.5386677510320939 }, "nextControl": { "x": 6.494219837010534, "y": 0.6689152426011367 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { @@ -56,94 +59,99 @@ "x": 6.124269634468701, "y": 2.545269365624529 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { - "x": 5.866837606837606, - "y": 2.458000000000001 + "x": 6.1444722719141325, + "y": 2.4577321937321948 }, "prevControl": { - "x": 6.590370370370371, - "y": 2.444811965811966 + "x": 6.868005035446897, + "y": 2.4445441595441597 }, "nextControl": { - "x": 5.351084085527113, - "y": 2.4674007837854255 + "x": 5.628718750603639, + "y": 2.467132977517619 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { - "x": 4.755698005698005, - "y": 2.458000000000001 + "x": 4.684186046511628, + "y": 2.4577321937321948 }, "prevControl": { - "x": 5.905598290598291, - "y": 2.470920227920229 + "x": 5.722611806797854, + "y": 2.4577321937321948 }, "nextControl": { - "x": 4.148936797920834, - "y": 2.4511824583395834 + "x": 4.077386539147374, + "y": 2.4577321937321948 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { - "x": 3.760840455840455, - "y": 2.458000000000001 + "x": 3.613309481216458, + "y": 2.4577321937321948 }, "prevControl": { - "x": 4.6006552706552695, - "y": 2.4706524216524226 + "x": 4.453124296031272, + "y": 2.4703846153846163 }, "nextControl": { - "x": 3.34446165048371, - "y": 2.4517269501328505 + "x": 3.1969306758597127, + "y": 2.451459143865044 }, - "isLocked": false + "isLocked": false, + "linkedName": null }, { "anchor": { - "x": 2.533418803418803, + "x": 2.542432915921288, "y": 2.4577321937321948 }, "prevControl": { - "x": 3.476099715099715, - "y": 2.4445441595441597 + "x": 3.46006956867422, + "y": 2.4616248658318423 }, "nextControl": null, - "isLocked": false + "isLocked": false, + "linkedName": null } ], "rotationTargets": [ { "waypointRelativePos": 1.1886792452830188, - "rotationDegrees": 0 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 1.7264150943396226, - "rotationDegrees": 0 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 2.0754716981132075, - "rotationDegrees": 90 + "rotationDegrees": 90.0 }, { "waypointRelativePos": 2.9433962264150932, - "rotationDegrees": 90 + "rotationDegrees": 90.0 }, { "waypointRelativePos": 3.9622641509434033, - "rotationDegrees": -180 + "rotationDegrees": -180.0 }, { "waypointRelativePos": 4.688679245283029, - "rotationDegrees": -180 + "rotationDegrees": -180.0 }, { "waypointRelativePos": 6.344919786096259, - "rotationDegrees": -180 + "rotationDegrees": -180.0 } ], "constraintZones": [ @@ -199,7 +207,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 32.614505469234345 + "rotation": 36.48792547566371 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 629c3f90d..bcfbebea0 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -14,7 +14,6 @@ import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.rollers.IntakeRollers; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; import java.util.function.Supplier; /** Contains all autos */ From 135b12476ca4b91868fc9ed0099ef88e4028fd75 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Fri, 17 Apr 2026 18:50:44 -0400 Subject: [PATCH 22/25] stuff man. stressful --- .../deploy/pathplanner/paths/A-Cycle2.path | 52 +++++++------- .../deploy/pathplanner/paths/B-Cycle2.path | 70 +++++++++---------- src/main/java/frc/robot/RobotContainer.java | 4 -- .../intake/extend/IntakeExtendIOSparkMax.java | 4 +- 4 files changed, 64 insertions(+), 66 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index 08981c41b..6869a3fda 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -64,60 +64,60 @@ }, { "anchor": { - "x": 6.355402504472272, - "y": 5.828 + "x": 6.566332737030411, + "y": 5.438497316636852 }, "prevControl": { - "x": 7.312701252236136, - "y": 5.746779964221824 + "x": 6.825939177101969, + "y": 5.42227191413238 }, "nextControl": { - "x": 5.841409940149118, - "y": 5.871608637911162 + "x": 6.051498103813295, + "y": 5.470674481212921 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.684186046511628, - "y": 5.828 + "x": 4.667960644007155, + "y": 5.438497316636852 }, "prevControl": { - "x": 5.722611806797854, - "y": 5.828 + "x": 5.706386404293381, + "y": 5.438497316636852 }, "nextControl": { - "x": 4.077386539147374, - "y": 5.828 + "x": 4.061161136642901, + "y": 5.438497316636852 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.6619856887298745, - "y": 5.8279069767441865 + "x": 3.5646332737030413, + "y": 5.438497316636852 }, "prevControl": { - "x": 4.501800503544689, - "y": 5.815254555091765 + "x": 4.404448088517856, + "y": 5.42584489498443 }, "nextControl": { - "x": 3.2456068833731293, - "y": 5.834180026611337 + "x": 3.148254468346296, + "y": 5.444770366504002 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.542432915921288, - "y": 5.8279069767441865 + "x": 2.396404293381037, + "y": 5.438497316636852 }, "prevControl": { - "x": 3.46006956867422, - "y": 5.824014304644539 + "x": 3.3140409461339693, + "y": 5.434604644537204 }, "nextControl": null, "isLocked": false, @@ -143,10 +143,10 @@ }, { "waypointRelativePos": 3.9622641509434033, - "rotationDegrees": 180.0 + "rotationDegrees": -134.71789215346988 }, { - "waypointRelativePos": 4.688679245283029, + "waypointRelativePos": 4.398395721925132, "rotationDegrees": 180.0 }, { @@ -160,7 +160,7 @@ "minWaypointRelativePos": 2.0644067796610166, "maxWaypointRelativePos": 3.22711864406781, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.3, "maxAcceleration": 6, "maxAngularVelocity": 540, "maxAngularAcceleration": 720, @@ -207,7 +207,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -49.70785224437223 + "rotation": -41.308614013548784 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index 3bff2d9bf..d010fc67e 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -32,15 +32,15 @@ }, { "anchor": { - "x": 5.634273504273503, + "x": 6.0146690518783545, "y": 0.6212390350877186 }, "prevControl": { - "x": 4.144916933849193, + "x": 4.525312481454044, "y": 0.5386677510320939 }, "nextControl": { - "x": 6.494219837010534, + "x": 6.874615384615385, "y": 0.6689152426011367 }, "isLocked": false, @@ -48,76 +48,76 @@ }, { "anchor": { - "x": 6.1444722719141325, - "y": 3.3129695885509847 + "x": 6.566332737030411, + "y": 3.321571198568873 }, "prevControl": { - "x": 6.157392499834359, - "y": 3.8039382495196463 + "x": 6.579252964950638, + "y": 3.8125398595375346 }, "nextControl": { - "x": 6.124269634468701, - "y": 2.545269365624529 + "x": 6.546130099584979, + "y": 2.5538709756424174 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.1444722719141325, - "y": 2.4577321937321948 + "x": 6.566332737030411, + "y": 2.5752026833631483 }, "prevControl": { - "x": 6.868005035446897, - "y": 2.4445441595441597 + "x": 6.825939177101969, + "y": 2.59142808586762 }, "nextControl": { - "x": 5.628718750603639, - "y": 2.467132977517619 + "x": 6.051498103813295, + "y": 2.543025518787079 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.684186046511628, - "y": 2.4577321937321948 + "x": 4.667960644007155, + "y": 2.5752026833631483 }, "prevControl": { - "x": 5.722611806797854, - "y": 2.4577321937321948 + "x": 5.706386404293381, + "y": 2.5752026833631483 }, "nextControl": { - "x": 4.077386539147374, - "y": 2.4577321937321948 + "x": 4.061161136642901, + "y": 2.5752026833631483 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.613309481216458, - "y": 2.4577321937321948 + "x": 3.5646332737030413, + "y": 2.5752026833631483 }, "prevControl": { - "x": 4.453124296031272, - "y": 2.4703846153846163 + "x": 4.404448088517856, + "y": 2.58785510501557 }, "nextControl": { - "x": 3.1969306758597127, - "y": 2.451459143865044 + "x": 3.148254468346296, + "y": 2.5689296334959977 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.542432915921288, - "y": 2.4577321937321948 + "x": 2.396404293381037, + "y": 2.5752026833631483 }, "prevControl": { - "x": 3.46006956867422, - "y": 2.4616248658318423 + "x": 3.3140409461339693, + "y": 2.579095355462796 }, "nextControl": null, "isLocked": false, @@ -143,10 +143,10 @@ }, { "waypointRelativePos": 3.9622641509434033, - "rotationDegrees": -180.0 + "rotationDegrees": 134.71789215346988 }, { - "waypointRelativePos": 4.688679245283029, + "waypointRelativePos": 4.398395721925132, "rotationDegrees": -180.0 }, { @@ -160,7 +160,7 @@ "minWaypointRelativePos": 2.0644067796610166, "maxWaypointRelativePos": 3.22711864406781, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.3, "maxAcceleration": 6, "maxAngularVelocity": 540, "maxAngularAcceleration": 720, @@ -207,7 +207,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 36.48792547566371 + "rotation": 41.308614013548784 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 40dcad6f1..5442ce1ac 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -100,8 +100,6 @@ public RobotContainer() { vision = new Vision( drive::addVisionMeasurement, - new VisionIOPhotonVision(camera0Name, robotToCamera0), - new VisionIOPhotonVision(camera1Name, robotToCamera1), new VisionIOPhotonVision(camera2Name, robotToCamera2)); leds = new Leds(); } @@ -116,8 +114,6 @@ public RobotContainer() { vision = new Vision( drive::addVisionMeasurement, - new VisionIOPhotonVision(camera0Name, robotToCamera0), - new VisionIOPhotonVision(camera1Name, robotToCamera1), new VisionIOPhotonVision(camera2Name, robotToCamera2)); leds = new Leds(); shooter = new Shooter(new ShooterIOSparkMax()); diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java index c21bc77d9..5df9579ae 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java @@ -10,6 +10,7 @@ import static frc.robot.subsystems.intake.IntakeConstants.PID_P; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; @@ -54,7 +55,8 @@ public SparkMaxConfig getSparkMaxConfig() { .smartCurrentLimit((int) Math.round(INTAKE_EXTEND_MOTOR_CURRENT_LIMIT)) .closedLoop .pid(PID_P, PID_I, PID_D) - .feedbackSensor(FeedbackSensor.kPrimaryEncoder); + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .allowedClosedLoopError(0.5, ClosedLoopSlot.kSlot0); extendConfig.encoder.apply(extendEnc); return extendConfig; } From f0bb8d499fb9f126feaa7404b49fad9cf66651ea Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Sat, 18 Apr 2026 06:49:16 -0400 Subject: [PATCH 23/25] added depot auto --- .../pathplanner/paths/Depot+Outpost.path | 225 ++++++++++++++++++ src/main/deploy/pathplanner/paths/Depot.path | 113 --------- src/main/java/frc/robot/RobotContainer.java | 1 + .../java/frc/robot/commands/auto/Autos.java | 29 ++- 4 files changed, 254 insertions(+), 114 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Depot+Outpost.path delete mode 100644 src/main/deploy/pathplanner/paths/Depot.path diff --git a/src/main/deploy/pathplanner/paths/Depot+Outpost.path b/src/main/deploy/pathplanner/paths/Depot+Outpost.path new file mode 100644 index 000000000..81400486a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot+Outpost.path @@ -0,0 +1,225 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.532, + "y": 4.027 + }, + "prevControl": null, + "nextControl": { + "x": 1.812107334532951, + "y": 3.79984436493161 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2016994633273703, + "y": 5.990161001788908 + }, + "prevControl": { + "x": 3.369928443649373, + "y": 5.909033989266546 + }, + "nextControl": { + "x": 1.0334704830053674, + "y": 6.07128801431127 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.6927370304184608, + "y": 5.990161001788908 + }, + "prevControl": { + "x": 1.8610786817134128, + "y": 5.990161001818615 + }, + "nextControl": { + "x": 0.4427370304184608, + "y": 5.9901610017825515 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9036672629766005, + "y": 5.243792486577408 + }, + "prevControl": { + "x": 0.5230906562912437, + "y": 5.187974584263555 + }, + "nextControl": { + "x": 2.120572450812021, + "y": 5.422271914126602 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8447406082359925, + "y": 4.2215921287956535 + }, + "prevControl": { + "x": 1.8285152057315202, + "y": 4.659677996416406 + }, + "nextControl": { + "x": 1.8608544946251664, + "y": 3.786517196287962 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8447406082359925, + "y": 2.550375670835009 + }, + "prevControl": { + "x": 2.029725626584978, + "y": 2.7185438693340873 + }, + "nextControl": { + "x": 1.4957446623815076, + "y": 2.233106629149113 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.579159212887155, + "y": 1.8364579606382292 + }, + "prevControl": { + "x": 0.6116100178960995, + "y": 2.566601073339482 + }, + "nextControl": { + "x": 0.5680590594819749, + "y": 1.5867045090216771 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.579159212887155, + "y": 0.9278354203877826 + }, + "prevControl": { + "x": 0.579159212887155, + "y": 1.365921288008534 + }, + "nextControl": { + "x": 0.579159212887155, + "y": 0.489749552767031 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.845, + "y": 1.204 + }, + "prevControl": { + "x": 1.8816371652723654, + "y": 0.9566991344115322 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.02139037433155, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 3.1256684491978635, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 5.3903743315508015, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 6.994652406417122, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 7.336898395721934, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.1389830508474574, + "maxWaypointRelativePos": 2.3728813559322037, + "constraints": { + "maxVelocity": 0.6, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 5.776271186440677, + "maxWaypointRelativePos": 7.21355932203389, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.62, + "y": 4.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 3.48813559322034, + "maxWaypointRelativePos": 4.864406779661017, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 46.29216688624992 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 178.72696997994333 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot.path b/src/main/deploy/pathplanner/paths/Depot.path deleted file mode 100644 index 90ed3dfca..000000000 --- a/src/main/deploy/pathplanner/paths/Depot.path +++ /dev/null @@ -1,113 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.63, - "y": 5.844 - }, - "prevControl": null, - "nextControl": { - "x": 2.9647584973166365, - "y": 5.90890161001789 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.282826475849731, - "y": 5.9577101967799635 - }, - "prevControl": { - "x": 3.2798809613513087, - "y": 5.881013697895228 - }, - "nextControl": { - "x": 1.8609660107334522, - "y": 5.990161001788907 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.685826210826211, - "y": 5.9577101967799635 - }, - "prevControl": { - "x": 1.7065242165242167, - "y": 5.947193732193732 - }, - "nextControl": { - "x": 0.3447247623997972, - "y": 5.9612246361956025 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.9480679785330945, - "y": 4.611001788908765 - }, - "prevControl": { - "x": 2.365455840455841, - "y": 6.218518518518518 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0160427807486627, - "rotationDegrees": 180.0 - }, - { - "waypointRelativePos": 1.9358288770053491, - "rotationDegrees": 180.0 - }, - { - "waypointRelativePos": 2.660980810234532, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.799999999999997, - "maxWaypointRelativePos": 2.450847457627116, - "constraints": { - "maxVelocity": 3.58, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -27.897271030947596 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5442ce1ac..e2bb9f2a3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -257,6 +257,7 @@ public RobotContainer() { autoChooser.addOption("A-side", autos.ppA2CycleRightRegression()); autoChooser.addOption("B-side", autos.ppB2CycleRightRegression()); + autoChooser.addOption("Depot+Outpost", autos.ppDepotOutpost()); // Configure the button bindings configureButtonBindings(); } diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index bcfbebea0..fb79f854c 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -88,7 +88,6 @@ public Autos( this.shooter = shooter; } - private static final Supplier center = () -> new Pose2d(3.504, 4.019, new Rotation2d(0)); private static final Supplier depotPoseStopPoint = () -> new Pose2d(1.7998201847076416, 5.983600616455078, Rotation2d.fromDegrees(180)); private static final Supplier depotPose = @@ -102,6 +101,8 @@ public Autos( () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000))); private static final Supplier startDepot = () -> AllianceFlipUtil.apply(new Pose2d(3.630, 5.844, new Rotation2d(0.000))); + private static final Supplier center = + () -> AllianceFlipUtil.apply(new Pose2d(3.532, 4.027, Rotation2d.k180deg)); /** * Helper function for a single cycle auto with a constant shooting speed @@ -169,6 +170,28 @@ private Command ppCycleRegression(String path, Supplier startPose) { .withTimeout(1.6))); } + private Command ppCycleRegressionCustom( + String path, Supplier startPose, double endDistance) { + return Commands.sequence( + Commands.runOnce(() -> drive.setPose(startPose.get())), + Commands.deadline( + drive.getAutonomousCommand(path), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + Commands.waitSeconds(5.7) + .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(endDistance)))) + .withTimeout(15.5), + Commands.deadline( + orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) + .andThen( + Commands.parallel( + Commands.waitSeconds(0.4).andThen(intake.slowlyBringInIntake()), + orchestrator.aimToHub().withTimeout(1).repeatedly(), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())) + .withTimeout(1.6))); + } + private Command ppCycleRegressionDepo(String path, Supplier startPose) { return Commands.sequence( Commands.runOnce(() -> drive.setPose(startPose.get())), @@ -286,6 +309,10 @@ public Command ppACycleLeftRegression() { return ppCycleRegression("A-Cycle-LeftSweep", startAside); } + public Command ppDepotOutpost() { + return ppCycleRegressionCustom("Depot+Outpost", center, 4); + } + /** * Gets relevant poses for the autos * From 681b0888321bed3da69c43a10b30ca78e8233407 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Sat, 18 Apr 2026 09:51:51 -0400 Subject: [PATCH 24/25] adjusted autos --- .../pathplanner/paths/Depot+Outpost.path | 140 +++++------------- src/main/java/frc/robot/RobotContainer.java | 3 +- .../java/frc/robot/commands/auto/Autos.java | 38 ++--- 3 files changed, 63 insertions(+), 118 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Depot+Outpost.path b/src/main/deploy/pathplanner/paths/Depot+Outpost.path index 81400486a..192420be5 100644 --- a/src/main/deploy/pathplanner/paths/Depot+Outpost.path +++ b/src/main/deploy/pathplanner/paths/Depot+Outpost.path @@ -3,137 +3,73 @@ "waypoints": [ { "anchor": { - "x": 3.532, + "x": 2.2, "y": 4.027 }, "prevControl": null, "nextControl": { - "x": 1.812107334532951, - "y": 3.79984436493161 + "x": 1.6987119856887296, + "y": 3.9133094812164577 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.2016994633273703, - "y": 5.990161001788908 + "x": 1.7311627906976743, + "y": 5.9577101967799635 }, "prevControl": { - "x": 3.369928443649373, - "y": 5.909033989266546 + "x": 2.039445438282647, + "y": 5.9739355992844345 }, "nextControl": { - "x": 1.0334704830053674, - "y": 6.07128801431127 + "x": 0.5617388688105667, + "y": 5.896161569312226 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.6927370304184608, - "y": 5.990161001788908 + "x": 0.8549910554561715, + "y": 5.9577101967799635 }, "prevControl": { - "x": 1.8610786817134128, - "y": 5.990161001818615 + "x": 2.0233327067511238, + "y": 5.95771019680967 }, "nextControl": { - "x": 0.4427370304184608, - "y": 5.9901610017825515 + "x": 0.6049910554561715, + "y": 5.957710196773607 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.9036672629766005, - "y": 5.243792486577408 + "x": 1.2606261180679783, + "y": 5.9577101967799635 }, "prevControl": { - "x": 0.5230906562912437, - "y": 5.187974584263555 + "x": 0.8761262902266167, + "y": 5.947029646006592 }, "nextControl": { - "x": 2.120572450812021, - "y": 5.422271914126602 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8447406082359925, - "y": 4.2215921287956535 - }, - "prevControl": { - "x": 1.8285152057315202, - "y": 4.659677996416406 - }, - "nextControl": { - "x": 1.8608544946251664, - "y": 3.786517196287962 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8447406082359925, - "y": 2.550375670835009 - }, - "prevControl": { - "x": 2.029725626584978, - "y": 2.7185438693340873 - }, - "nextControl": { - "x": 1.4957446623815076, - "y": 2.233106629149113 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.579159212887155, - "y": 1.8364579606382292 - }, - "prevControl": { - "x": 0.6116100178960995, - "y": 2.566601073339482 - }, - "nextControl": { - "x": 0.5680590594819749, - "y": 1.5867045090216771 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.579159212887155, - "y": 0.9278354203877826 - }, - "prevControl": { - "x": 0.579159212887155, - "y": 1.365921288008534 - }, - "nextControl": { - "x": 0.579159212887155, - "y": 0.489749552767031 + "x": 2.428855098389982, + "y": 5.990161001788908 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.845, - "y": 1.204 + "x": 1.893416815742397, + "y": 4.027 }, "prevControl": { - "x": 1.8816371652723654, - "y": 0.9566991344115322 + "x": 1.8771914132379246, + "y": 4.465085867620752 }, "nextControl": null, "isLocked": false, @@ -141,6 +77,10 @@ } ], "rotationTargets": [ + { + "waypointRelativePos": 1.1310160427807487, + "rotationDegrees": 180.0 + }, { "waypointRelativePos": 2.02139037433155, "rotationDegrees": 180.0 @@ -150,16 +90,16 @@ "rotationDegrees": 180.0 }, { - "waypointRelativePos": 5.3903743315508015, + "waypointRelativePos": 4.5, "rotationDegrees": -90.0 }, { - "waypointRelativePos": 6.994652406417122, + "waypointRelativePos": 4.75, "rotationDegrees": -90.0 }, { - "waypointRelativePos": 7.336898395721934, - "rotationDegrees": -90.0 + "waypointRelativePos": 4.75, + "rotationDegrees": 29.42136089373414 } ], "constraintZones": [ @@ -178,8 +118,8 @@ }, { "name": "Constraints Zone", - "minWaypointRelativePos": 5.776271186440677, - "maxWaypointRelativePos": 7.21355932203389, + "minWaypointRelativePos": 4, + "maxWaypointRelativePos": 4.75, "constraints": { "maxVelocity": 1.2, "maxAcceleration": 6.0, @@ -197,8 +137,8 @@ "y": 4.0 }, "rotationOffset": 0.0, - "minWaypointRelativePos": 3.48813559322034, - "maxWaypointRelativePos": 4.864406779661017, + "minWaypointRelativePos": 0.013559322033898305, + "maxWaypointRelativePos": 0.6237288135593221, "name": "Point Towards Zone" } ], @@ -213,13 +153,13 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 46.29216688624992 + "rotation": 0.0 }, "reversed": false, "folder": null, "idealStartingState": { - "velocity": 0, - "rotation": 178.72696997994333 + "velocity": 0.0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e2bb9f2a3..8115a63d0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -257,7 +257,8 @@ public RobotContainer() { autoChooser.addOption("A-side", autos.ppA2CycleRightRegression()); autoChooser.addOption("B-side", autos.ppB2CycleRightRegression()); - autoChooser.addOption("Depot+Outpost", autos.ppDepotOutpost()); + autoChooser.addOption("Depot+Outpost", autos.ppDepot()); + // Configure the button bindings configureButtonBindings(); } diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index fb79f854c..641e5a99d 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -102,7 +102,7 @@ public Autos( private static final Supplier startDepot = () -> AllianceFlipUtil.apply(new Pose2d(3.630, 5.844, new Rotation2d(0.000))); private static final Supplier center = - () -> AllianceFlipUtil.apply(new Pose2d(3.532, 4.027, Rotation2d.k180deg)); + () -> AllianceFlipUtil.apply(new Pose2d(3.581, 4.027, Rotation2d.k180deg)); /** * Helper function for a single cycle auto with a constant shooting speed @@ -176,7 +176,10 @@ private Command ppCycleRegressionCustom( Commands.runOnce(() -> drive.setPose(startPose.get())), Commands.deadline( drive.getAutonomousCommand(path), - intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + intake + .extendToAngleAndIntake(IntakeConstants.FUNNEL_POS) + .withTimeout(4.5) + .andThen(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)), Commands.waitSeconds(5.7) .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(endDistance)))) .withTimeout(15.5), @@ -192,25 +195,22 @@ private Command ppCycleRegressionCustom( .withTimeout(1.6))); } - private Command ppCycleRegressionDepo(String path, Supplier startPose) { + private Command ppCycleRegressionDepo(String path) { return Commands.sequence( - Commands.runOnce(() -> drive.setPose(startPose.get())), Commands.deadline( drive.getAutonomousCommand(path), - intake.extendToAngleAndIntake(IntakeConstants.FUNNEL_POS).withTimeout(5.5), - Commands.waitSeconds(5) - .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(2.62)))) - .withTimeout(16.5), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS), + orchestrator.spinUpShooterDistance(() -> Meters.of(3.185))) + .withTimeout(14.5), Commands.deadline( orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) .andThen( Commands.parallel( - Commands.waitSeconds(0.4).andThen(intake.slowlyBringInIntake()), - orchestrator.aimToHub().withTimeout(1).repeatedly(), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())) - .withTimeout(1.6))); + Commands.waitSeconds(0.9).andThen(intake.slowlyBringInIntake()), + orchestrator.aimToHub().withTimeout(1).repeatedly(), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(0.9).andThen(orchestrator.feedUp())))); } private Command ppCycleRegressionConnect(String path) { @@ -251,7 +251,7 @@ private Command pp2CycleRegression(String path1, String path2, Supplier /** Depot Auto */ public Command ppDepot() { - return ppCycleRegression("A-Cycle-LeftSweep", startDepot); + return Commands.sequence(EightBalls().withTimeout(8.2), ppCycleRegressionDepo("Depot+Outpost")); } ; @@ -310,7 +310,7 @@ public Command ppACycleLeftRegression() { } public Command ppDepotOutpost() { - return ppCycleRegressionCustom("Depot+Outpost", center, 4); + return ppCycleRegressionCustom("Depot+Outpost", center, 4.285); } /** @@ -480,7 +480,11 @@ public Command BCCManualAutoOverBump() { public Command EightBalls() { return Commands.sequence( Commands.deadline( - orchestrator.driveToHub().withTimeout(3.0), orchestrator.spinUpShooterHub()), - Commands.parallel(orchestrator.spinUpShooterHub(), orchestrator.feedUp())); + orchestrator.driveToHub().withTimeout(3.0), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())), + Commands.parallel( + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(1).andThen(orchestrator.feedUp())) + .withTimeout(5.2)); } } From 9330feb6c5c8906d09afa62b9ea55239366be920 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Sun, 21 Jun 2026 16:11:41 -0400 Subject: [PATCH 25/25] Formatting, major cleanup of code, adding comments to susbsystem methods --- src/main/java/frc/robot/RobotContainer.java | 14 +-- .../robot/commands/drive/DriveWithDpad.java | 5 +- .../frc/robot/subsystems/intake/Intake.java | 9 +- .../intake/extend/IntakeExtend.java | 46 +++++----- .../intake/extend/IntakeExtendIO.java | 17 +++- .../intake/extend/IntakeExtendIOKraken.java | 11 ++- .../intake/extend/IntakeExtendIOSparkMax.java | 10 +-- .../intake/rollers/IntakeRollers.java | 11 ++- .../intake/rollers/IntakeRollersIO.java | 5 +- .../intake/rollers/IntakeRollersIOKraken.java | 2 +- .../rollers/IntakeRollersIOSparkMax.java | 7 +- .../robot/subsystems/leds/LedPatterns.java | 16 ++++ .../java/frc/robot/subsystems/leds/Leds.java | 86 ++++++++++--------- 13 files changed, 134 insertions(+), 105 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8115a63d0..62fa60ceb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -74,8 +74,6 @@ public class RobotContainer { private Intake intake; private IntakeRollers intakeRollers; private IntakeExtend intakeExtend; - private RobotState robotState = RobotState.getInstance(); - private boolean isRobotOriented = true; // Workaround, change if needed // Controller private final CommandXboxController driverController = new CommandXboxController(0); @@ -120,7 +118,7 @@ public RobotContainer() { magicCarpet = new MagicCarpet(new MagicCarpetSparkMax()); indexer = new Indexer(new IndexerIOSparkMax()); intakeRollers = new IntakeRollers(new IntakeRollersIOKraken()); - intakeExtend = new IntakeExtend(new IntakeExtendIOSparkMax(), () -> false); + intakeExtend = new IntakeExtend(new IntakeExtendIOSparkMax()); // climber = new Climber(new ClimberIOPhysical()); } @@ -159,7 +157,7 @@ public RobotContainer() { intakeRollers = new IntakeRollers(new IntakeRollersIO() {}); } if (intakeExtend == null) { - intakeExtend = new IntakeExtend(new IntakeExtendIO() {}, () -> false); + intakeExtend = new IntakeExtend(new IntakeExtendIO() {}); } intake = new Intake(intakeRollers, intakeExtend); if (magicCarpet == null) { @@ -369,12 +367,4 @@ public void robotPeriodic() { RobotState.getInstance().updateLEDState(); orchestrator.orchestratorPeriodic(); } - - private Command rumblePulse(double intensity, double seconds) { - return Commands.sequence( - Commands.runOnce( - () -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)), - Commands.waitSeconds(seconds), - Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0))); - } } diff --git a/src/main/java/frc/robot/commands/drive/DriveWithDpad.java b/src/main/java/frc/robot/commands/drive/DriveWithDpad.java index 62d1f7a2a..6a23da9a6 100644 --- a/src/main/java/frc/robot/commands/drive/DriveWithDpad.java +++ b/src/main/java/frc/robot/commands/drive/DriveWithDpad.java @@ -7,16 +7,19 @@ import java.util.function.Supplier; public class DriveWithDpad extends Command { + public static final double SLOW_SPEED = Units.inchesToMeters(20); + private final Drive drive; private final Supplier povSupplier; - public static final double SLOW_SPEED = Units.inchesToMeters(20); + /** Creates a slow cardinal-direction drive command controlled by the driver's D-pad angle. */ public DriveWithDpad(Drive drive, Supplier povSupplier) { this.drive = drive; this.povSupplier = povSupplier; addRequirements(drive); } + /** Converts the current POV angle into a low-speed chassis movement. */ @Override public void execute() { int pov = povSupplier.get(); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 1a58f9403..51254c99e 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -11,24 +11,29 @@ public class Intake { private final IntakeRollers rollers; private final IntakeExtend extend; + /** Creates the intake facade from the roller and extension subsystems. */ public Intake(IntakeRollers rollers, IntakeExtend extend) { this.rollers = rollers; this.extend = extend; } + /** Extends to the requested angle while running the rollers inward. */ public Command extendToAngleAndIntake(double angle) { return Commands.parallel(extend.extendToAngle(angle), runIntakeMotor()) .withName("extendToAngleAndIntake"); } + /** Moves the intake extension to the requested angle. */ public Command extendToAngle(double angle) { return extend.extendToAngle(angle); } + /** Runs the intake rollers inward. */ public Command runIntakeMotor() { return rollers.intake(); } + /** Collapses the intake slowly while keeping the rollers running. */ public Command slowlyBringInIntake() { return Commands.parallel( rollers.intake(), @@ -38,6 +43,7 @@ public Command slowlyBringInIntake() { .andThen(extend.extendToAngle(COLLAPSE_POS).repeatedly())); } + /** Collapses the intake slowly without moving the rollers. */ public Command slowlyBringInIntakeWithoutRollers() { return extend .runIntakeExtendVolts(SLOW_VOLTS) @@ -45,7 +51,7 @@ public Command slowlyBringInIntakeWithoutRollers() { .andThen(extend.extendToAngle(COLLAPSE_POS).repeatedly()); } - // Jack's Chugga Chugga mode + /** Alternates the extension around the funnel position while intaking game pieces. */ public Command shakeAndIntake() { return Commands.repeatingSequence( Commands.deadline( @@ -55,6 +61,7 @@ public Command shakeAndIntake() { .withName("shakeAndIntake"); } + /** Alternates the extension around the funnel position without running the rollers. */ public Command shake() { return Commands.repeatingSequence( extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET), diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java index bbf147252..5b9880701 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; import frc.robot.RobotState.IntakePosition; -import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -26,17 +25,10 @@ public class IntakeExtend extends SubsystemBase { private boolean stalledExtend = false; private boolean stalledCollapse = false; private boolean isStowed = false; - private boolean isExtended = false; private boolean hasPose = false; - private double targetExtendPosition = 0; - - /** - * @param limitSwitchDisabled A supplier to return whether the limit switch is currently disabled - * or not. If disabled, uses slipping to control intake. - */ - private final BooleanSupplier limitSwitchDisabled; - + /** Updates logged inputs and keeps robot-wide intake state in sync with the extension encoder. */ + @Override public void periodic() { inputs.stalledExtended = stalledExtend; inputs.stalledCollapsed = stalledCollapse; @@ -49,10 +41,10 @@ public void periodic() { Logger.recordOutput("Intake/IntakeExtended/StalledCollapse", stalledCollapse); Logger.recordOutput("Intake/stallingExtend", isStallingExtend()); Logger.recordOutput("Intake/stallingCollapse", isStallingCollapse()); - if (inputs.getExtendPos > EXTEND_POS / 2) { + if (inputs.extendPosition > EXTEND_POS / 2) { RobotState.getInstance().intakePosition = IntakePosition.STOWED; } - if (inputs.getExtendPos <= EXTEND_POS / 2) { + if (inputs.extendPosition <= EXTEND_POS / 2) { RobotState.getInstance().intakePosition = IntakePosition.DEPLOYED; } if (inputs.isCollapsed) { @@ -66,64 +58,71 @@ public void periodic() { Logger.recordOutput("Intake/IntakeExtend/HasPose", hasPose); } - public IntakeExtend(IntakeExtendIO io, BooleanSupplier limitSwitchDisabled) { + /** Creates an intake extension subsystem using the selected hardware abstraction. */ + public IntakeExtend(IntakeExtendIO io) { this.io = io; - this.limitSwitchDisabled = limitSwitchDisabled; } + /** Returns whether the extension is currently on the collapsed limit switch. */ public boolean isHopperCollapsed() { return io.isCollapsed(); } + /** Supplies the current extension position for commands that need a live sensor reading. */ public DoubleSupplier getAngle() { - return () -> inputs.getExtendPos; + return () -> inputs.extendPosition; } + /** Sets the motor idle behavior through the IO layer. */ public void setIdleMode(boolean idleMode) { io.setIdleMode(idleMode); } + /** Resets the extension encoder to a known pose. */ public Command setPose(double pose) { return Commands.runOnce(() -> io.resetExtendEncoder(pose), this); } + /** Returns true when the extension is being driven outward but the encoder is barely moving. */ private boolean isStallingExtend() { - // For our volts, we are not getting the right velocity return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts < -0.01; } + /** Returns true when the extension is being collapsed but the encoder is barely moving. */ private boolean isStallingCollapse() { return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts > 0.01; } - private void setPercentExtend(double extendPercent) { - io.setPercentExtend(extendPercent); - } - + /** Drives the extension motor with open-loop voltage. */ public void setVoltageExtend(double extendVolts) { io.setVoltageExtend(extendVolts); } + /** Stops the extension motor immediately. */ public void stopExtend() { io.setVoltageExtend(0); } + /** Creates a one-shot command that zeros the extension encoder. */ public Command resetExtendPosition() { return Commands.runOnce(() -> io.resetExtendEncoder(0), this); } + /** Moves the extension to the configured deployed position and finishes at tolerance. */ public Command moveToExtendedPosition() { return Commands.run(() -> io.extendToPosition(EXTEND_POS)) - .until(() -> Math.abs(inputs.getExtendPos - EXTEND_POS) <= POSITION_TOLERANCE) + .until(() -> Math.abs(inputs.extendPosition - EXTEND_POS) <= POSITION_TOLERANCE) .withName("moveToExtendedPosition"); } + /** Moves the extension to the configured collapsed position and finishes at tolerance. */ public Command moveToCollapsedPosition() { return Commands.run(() -> io.extendToPosition(COLLAPSE_POS)) - .until(() -> Math.abs(inputs.getExtendPos - COLLAPSE_POS) <= POSITION_TOLERANCE) + .until(() -> Math.abs(inputs.extendPosition - COLLAPSE_POS) <= POSITION_TOLERANCE) .withName("moveToCollapsedPosition"); } + /** Creates a manual voltage command for operator-controlled extension movement. */ public Command runIntakeExtendVolts(double volts) { return Commands.run( () -> { @@ -133,10 +132,12 @@ public Command runIntakeExtendVolts(double volts) { this); } + /** Creates a command that keeps the extension stopped while scheduled. */ public Command stopExtendingCommand() { return Commands.run(this::stopExtend, this); } + /** Homes the extension by driving until the collapsed limit switch is reached. */ public Command homeExtend() { return Commands.run( () -> { @@ -150,6 +151,7 @@ public Command homeExtend() { .withName("homeExtend"); } + /** Homes first if needed, then moves the extension to the requested encoder angle. */ public Command extendToAngle(double angle) { return new ConditionalCommand(homeExtend(), Commands.none(), () -> !hasPose) .andThen( diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java index 6a8695012..5f2697d3f 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java @@ -11,11 +11,10 @@ class IntakeExtendIOInputs { public double extendVelocity = 0.0; public double extendAmps = 0.0; public boolean isCollapsed = false; - // public boolean manualModeInput; public boolean atSetpoint = false; public boolean hasSetpoint = false; public double setpointValue = 0.0; - public double getExtendPos = 0.0; + public double extendPosition = 0.0; public boolean stalledExtended = false; public boolean stalledCollapsed = false; public double stallExtendTimer = 0.0; @@ -24,30 +23,40 @@ class IntakeExtendIOInputs { public boolean hasPose = false; } + /** Refreshes telemetry from the active extension hardware. */ default void updateInputs(IntakeExtendIOInputs inputs) {} + /** Drives the extension by percent output. */ default void setPercentExtend(double extendPercent) {} + /** Drives the extension by voltage. */ default void setVoltageExtend(double extendVolts) {} + /** Stops the extension if the hardware implementation has a dedicated stop path. */ default void stop() {} + /** Requests a closed-loop extension position. */ default void goToPos(double pos) {} + /** Enables or disables closed-loop extension control. */ default void setPIDEnabled(boolean enabled) {} - default void setIdleMode(boolean cost) {} - ; + /** Sets the extension motor idle behavior. */ + default void setIdleMode(boolean coast) {} + /** Returns whether closed-loop extension control is currently enabled. */ default boolean getPIDEnabled() { return false; } + /** Resets the extension encoder to the provided position. */ default void resetExtendEncoder(double position) {} + /** Returns whether the extension is on the collapsed limit switch. */ default boolean isCollapsed() { return false; } + /** Requests a closed-loop extension position without changing PID enabled state. */ default void extendToPosition(double position) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java index 814cfc72f..59b608544 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java @@ -29,9 +29,9 @@ public class IntakeExtendIOKraken implements IntakeExtendIO { private final DigitalInput collapsedLimitSwitch; private final RelativeEncoder extendMotorEncoder; private final SparkClosedLoopController pidController; - private double setPos = 0; private boolean usingPID = false; + /** Configures the extension motor, encoder, PID, and limit switch for the Kraken variant. */ public IntakeExtendIOKraken() { extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless); @@ -70,7 +70,7 @@ public void updateInputs(IntakeExtendIOInputs inputs) { inputs.extendVolts = extendMotor.getAppliedOutput(); inputs.extendAmps = extendMotor.getOutputCurrent(); inputs.isCollapsed = !collapsedLimitSwitch.get(); - inputs.getExtendPos = extendMotorEncoder.getPosition(); + inputs.extendPosition = extendMotorEncoder.getPosition(); inputs.hasSetpoint = usingPID; inputs.setpointValue = extendMotor.getClosedLoopController().getSetpoint(); inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint(); @@ -92,10 +92,9 @@ public void setVoltageExtend(double extendVolts) { } @Override - public void goToPos(double setPos) { - this.setPos = setPos; - if (usingPID && !((isCollapsed() && (setPos > 0)))) { - pidController.setSetpoint(setPos, ControlType.kPosition); + public void goToPos(double targetPosition) { + if (usingPID && !(isCollapsed() && targetPosition > 0)) { + pidController.setSetpoint(targetPosition, ControlType.kPosition); } } diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java index 5df9579ae..9fb6f075a 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java @@ -29,9 +29,9 @@ public class IntakeExtendIOSparkMax implements IntakeExtendIO { private final DigitalInput collapsedLimitSwitch; private final RelativeEncoder extendMotorEncoder; private final SparkClosedLoopController pidController; - private double setPos = 0; private boolean usingPID = false; + /** Configures the Spark Max-backed extension motor, encoder, PID, and limit switch. */ public IntakeExtendIOSparkMax() { extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless); @@ -43,6 +43,7 @@ public IntakeExtendIOSparkMax() { collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID); } + /** Builds the Spark Max configuration shared by startup and idle-mode changes. */ public SparkMaxConfig getSparkMaxConfig() { EncoderConfig extendEnc = new EncoderConfig(); extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION); @@ -68,7 +69,7 @@ public void updateInputs(IntakeExtendIOInputs inputs) { inputs.extendVolts = extendMotor.getAppliedOutput(); inputs.extendAmps = extendMotor.getOutputCurrent(); inputs.isCollapsed = isCollapsed(); - inputs.getExtendPos = extendMotorEncoder.getPosition(); + inputs.extendPosition = extendMotorEncoder.getPosition(); inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint() && usingPID; inputs.hasSetpoint = usingPID; inputs.setpointValue = extendMotor.getClosedLoopController().getSetpoint(); @@ -95,10 +96,9 @@ public void setVoltageExtend(double extendVolts) { } @Override - public void goToPos(double setPos) { - this.setPos = setPos; + public void goToPos(double targetPosition) { if (usingPID) { - pidController.setSetpoint(setPos, ControlType.kPosition); + pidController.setSetpoint(targetPosition, ControlType.kPosition); } } diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java index 0cec68ba6..d5942dcb6 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java @@ -17,36 +17,39 @@ public IntakeRollers(IntakeRollersIO io) { this.io = io; } + /** Updates roller telemetry and exposes the active intake state to robot-wide logic. */ + @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake/IntakeRollers", inputs); RobotState.getInstance().intaking = inputs.intakeVolts > 0; } - private void setPercentIntake(double intakePercent) { - io.setPercentIntake(intakePercent); - } - + /** Drives the intake rollers with open-loop voltage. */ public void setVoltageIntake(double intakeVolts) { io.setVoltageIntake(intakeVolts); } + /** Stops the intake rollers immediately. */ private void stopIntake() { io.setVoltageIntake(0); } + /** Runs the rollers inward until the command ends. */ public Command intake() { return Commands.run(() -> setVoltageIntake(INTAKE_VOLTS), this) .finallyDo(this::stopIntake) .withName("intake"); } + /** Runs the rollers outward until the command ends. */ public Command outtake() { return Commands.run(() -> setVoltageIntake(OUTTAKE_VOLTS), this) .finallyDo(this::stopIntake) .withName("outtake"); } + /** Creates a command that keeps the rollers stopped while scheduled. */ public Command stopIntakeCommand() { return Commands.run(this::stopIntake).withName("stopIntakeCommand"); } diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java index 305898aa5..00c4670cd 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java @@ -9,14 +9,17 @@ class IntakeRollersIOInputs { public double intakeVolts = 0.0; public double intakeAmps = 0.0; public double intakeRPM = 0.0; - // public boolean manualModeInput; } + /** Refreshes telemetry from the active roller hardware. */ default void updateInputs(IntakeRollersIOInputs inputs) {} + /** Drives the rollers by percent output. */ default void setPercentIntake(double intakePercent) {} + /** Drives the rollers by voltage. */ default void setVoltageIntake(double intakeVolts) {} + /** Stops the rollers if the hardware implementation has a dedicated stop path. */ default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java index cbe116e01..50d83e8b9 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java @@ -16,11 +16,11 @@ public class IntakeRollersIOKraken implements IntakeRollersIO { private final TalonFX intakeMotor; - private double setPos = 0; private final StatusSignal intakeAppliedVolts; private final StatusSignal intakeCurrent; + /** Configures the TalonFX-backed intake roller motor and cached status signals. */ public IntakeRollersIOKraken() { intakeMotor = new TalonFX(intakeMotorCanId); diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java index 39b4ded2e..ea75ede72 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java @@ -13,9 +13,8 @@ public class IntakeRollersIOSparkMax implements IntakeRollersIO { private final SparkMax intakeMotor; - private double setPos = 0; - private boolean usingPID = false; + /** Configures the Spark Max-backed intake roller motor. */ public IntakeRollersIOSparkMax() { intakeMotor = new SparkMax(intakeMotorCanId, MotorType.kBrushless); @@ -49,8 +48,4 @@ public void setPercentIntake(double intakePercent) { public void setVoltageIntake(double intakeVolts) { intakeMotor.setVoltage(intakeVolts); } - - public void setPIDEnabled(boolean enabled) { - this.usingPID = enabled; - } } diff --git a/src/main/java/frc/robot/subsystems/leds/LedPatterns.java b/src/main/java/frc/robot/subsystems/leds/LedPatterns.java index 21b4f6f7b..77e51b9d1 100644 --- a/src/main/java/frc/robot/subsystems/leds/LedPatterns.java +++ b/src/main/java/frc/robot/subsystems/leds/LedPatterns.java @@ -65,43 +65,59 @@ public enum LedPatterns { private final LEDPattern colorPattern; + /** Returns the base LED pattern without adding animation. */ public LEDPattern colorPatternOnly() { return colorPattern; } + /** Blinks the pattern at the default strobe timing. */ public LEDPattern blink() { return colorPattern.blink(Seconds.of(STROBE_TIME)); } + /** + * Blinks the pattern at the requested timing, falling back to default timing for invalid input. + */ public LEDPattern blink(double time) { return colorPattern.blink(Seconds.of(time > 0.0 ? time : STROBE_TIME)); } + /** Breathes the pattern at the default timing. */ public LEDPattern breathe() { return colorPattern.breathe(Seconds.of(BREATH_TIME)); } + /** + * Breathes the pattern at the requested timing, falling back to default timing for invalid input. + */ public LEDPattern breathe(double time) { return colorPattern.breathe(Seconds.of(time > 0.0 ? time : BREATH_TIME)); } + /** Scrolls the pattern at the default speed. */ public LEDPattern scroll() { return colorPattern.scrollAtAbsoluteSpeed(MetersPerSecond.of(1), LED_SPACING); } + /** + * Scrolls the pattern at the requested speed, falling back to default speed for invalid input. + */ public LEDPattern scroll(double metersPerSecond) { return colorPattern.scrollAtAbsoluteSpeed( MetersPerSecond.of(metersPerSecond > 0.0 ? metersPerSecond : 1), LED_SPACING); } + /** Overlays this pattern on top of another pattern. */ public LEDPattern overlayOn(LEDPattern base) { return colorPattern.overlayOn(base); } + /** Blends this pattern with another pattern. */ public LEDPattern blend(LEDPattern other) { return colorPattern.blend(other); } + /** Stores the underlying WPILib LED pattern for this enum value. */ private LedPatterns(LEDPattern colorPattern) { this.colorPattern = colorPattern; } diff --git a/src/main/java/frc/robot/subsystems/leds/Leds.java b/src/main/java/frc/robot/subsystems/leds/Leds.java index c4f7d94e1..3899393d8 100644 --- a/src/main/java/frc/robot/subsystems/leds/Leds.java +++ b/src/main/java/frc/robot/subsystems/leds/Leds.java @@ -18,10 +18,10 @@ import frc.robot.RobotState.Mode; public class Leds extends SubsystemBase { - private static final boolean debug = false; - private RobotState state = RobotState.getInstance(); + private static final boolean DEBUG = false; + private final RobotState state = RobotState.getInstance(); private final AddressableLED leds; - public static final AddressableLEDBuffer buffer = + private static final AddressableLEDBuffer buffer = new AddressableLEDBuffer(LedConstants.FULL_LENGTH); public enum Animations { @@ -71,42 +71,45 @@ public enum Sections { LedConstants.BASE2_FOURTH_QUARTER_START, LedConstants.BASE2_FOURTH_QUARTER_END); - private final AddressableLEDBufferView buf_view_1; - private final AddressableLEDBufferView buf_view_2; + private final AddressableLEDBufferView primaryBufferView; + private final AddressableLEDBufferView secondaryBufferView; - private Sections(int start_1, int end_1, int start_2, int end_2) { - if (debug) { + /** Creates a section with mirrored LED ranges. */ + private Sections(int primaryStart, int primaryEnd, int secondaryStart, int secondaryEnd) { + if (DEBUG) { System.out.println( "create section " + toString() - + " start_1: " - + start_1 - + " end_1: " - + end_1 - + " start_2: " - + start_2 - + " end_2:" - + end_2); + + " primaryStart: " + + primaryStart + + " primaryEnd: " + + primaryEnd + + " secondaryStart: " + + secondaryStart + + " secondaryEnd:" + + secondaryEnd); } - this.buf_view_1 = buffer.createView(start_1, end_1); - this.buf_view_2 = buffer.createView(start_2, end_2); + primaryBufferView = buffer.createView(primaryStart, primaryEnd); + secondaryBufferView = buffer.createView(secondaryStart, secondaryEnd); } - private Sections(int start_1, int end_1) { - if (debug) { - System.out.println( - "create section " + toString() + " start_1: " + start_1 + " end_1: " + end_1); + /** Creates a section with one contiguous LED range. */ + private Sections(int start, int end) { + if (DEBUG) { + System.out.println("create section " + toString() + " start: " + start + " end: " + end); } - this.buf_view_1 = buffer.createView(start_1, end_1); - this.buf_view_2 = null; + primaryBufferView = buffer.createView(start, end); + secondaryBufferView = null; } - public AddressableLEDBufferView getBufferView_1() { - return buf_view_1; + /** Returns the first LED range for this section. */ + public AddressableLEDBufferView getPrimaryBufferView() { + return primaryBufferView; } - public AddressableLEDBufferView getBufferView_2() { - return buf_view_2; + /** Returns the mirrored LED range, or null when this section only has one range. */ + public AddressableLEDBufferView getSecondaryBufferView() { + return secondaryBufferView; } } @@ -122,10 +125,10 @@ public AddressableLEDBufferView getBufferView_2() { private LEDPattern currentPattern = LedPatterns.BLACK.colorPatternOnly(); private Sections applySection = Sections.FULL; - private Boolean isReversed = false; + private boolean isReversed = false; + /** Creates and starts the addressable LED strip and Shuffleboard controls. */ public Leds() { - leds = new AddressableLED(LedConstants.LED_CHANNEL); leds.setLength(LedConstants.FULL_LENGTH); leds.setColorOrder(ColorOrder.kRGB); @@ -135,15 +138,14 @@ public Leds() { initLedModeTabInShuffleboard(); } + /** Selects the current LED pattern from test controls or the robot state, then applies it. */ @Override public void periodic() { - if (enableTestEntry.getBoolean(false)) { processLedPatternTestInputs(); } else if (enableLedModeTestEntry.getBoolean(false)) { processLedModeTestInputs(); } else { - /* get from robot state */ Mode ledMode = state.getMode(); currentPattern = ledMode.ledPattern; applySection = ledMode.ledSection; @@ -154,11 +156,9 @@ public void periodic() { loadLedPatterns(); leds.setData(buffer); } - // else no op for simulation - } - // tests led patterns from shuffleboard gui + /** Reads Shuffleboard controls that directly test LED colors and animations. */ private void processLedPatternTestInputs() { try { LedPatterns pattern = testPattern.getSelected(); @@ -178,36 +178,37 @@ private void processLedPatternTestInputs() { case BREATHE_OVERLAY -> currentPattern = pattern.breathe().overlayOn(pattern2.breathe()); default -> currentPattern = pattern.colorPatternOnly(); } - } catch (IllegalArgumentException E) { + } catch (IllegalArgumentException exception) { currentPattern = LedPatterns.RED.blink(); } } - // process test inputs for led mode based on robot state + /** Reads Shuffleboard controls that preview a full robot LED mode. */ private void processLedModeTestInputs() { try { Mode mode = testMode.getSelected(); applySection = mode.ledSection; currentPattern = mode.ledPattern; - } catch (IllegalArgumentException E) { + } catch (IllegalArgumentException exception) { currentPattern = LedPatterns.RED.blink(); } } + /** Clears the strip and applies the selected pattern to the selected section. */ private void loadLedPatterns() { LedPatterns.BLACK.colorPatternOnly().applyTo(buffer); if (isReversed) { - currentPattern.applyTo(applySection.getBufferView_1().reversed()); + currentPattern.applyTo(applySection.getPrimaryBufferView().reversed()); } else { - currentPattern.applyTo(applySection.getBufferView_1()); + currentPattern.applyTo(applySection.getPrimaryBufferView()); } - if (applySection.getBufferView_2() != null) { - currentPattern.applyTo(applySection.getBufferView_2().reversed()); + if (applySection.getSecondaryBufferView() != null) { + currentPattern.applyTo(applySection.getSecondaryBufferView().reversed()); } } + /** Builds Shuffleboard controls for manually testing LED patterns. */ private void initLedTabInShuffleboard() { - ShuffleboardTab ledTab = Shuffleboard.getTab("LEDs"); ShuffleboardLayout ledTestingLayout = ledTab @@ -277,6 +278,7 @@ private void initLedTabInShuffleboard() { .getEntry(); } + /** Builds Shuffleboard controls for previewing robot LED modes. */ private void initLedModeTabInShuffleboard() { ShuffleboardTab ledModeTab = Shuffleboard.getTab("LED Modes"); ShuffleboardLayout ledModeTestingLayout =