From 95a18f862712470dca1ee4eb0fff2b13a0a8218a Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 6 Apr 2026 15:57:58 -0400 Subject: [PATCH 01/58] feat: digestion --- src/main/java/com/stuypulse/robot/RobotContainer.java | 5 +++++ .../com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 4bc4b230..f0aeac35 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -17,6 +17,7 @@ import com.stuypulse.robot.commands.hood.HomingRoutineUpper; import com.stuypulse.robot.commands.hood.SeedHoodRelativeEncoderAtLowerHardstop; import com.stuypulse.robot.commands.hood.SeedHoodRelativeEncoderAtUpperHardstop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeOuttake; import com.stuypulse.robot.commands.intake.IntakeRunRollers; @@ -180,6 +181,10 @@ private void configureButtonBindings() { ) ); + // driver.getTopButton() + // .whileTrue(new IntakeAutoDigest()) + // .onFalse(new IntakeDeploy()); + // Intake Stow driver.getLeftTriggerButton() .onTrue(new IntakeStow()); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java index 8085ffbb..3d0e744a 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -65,7 +65,7 @@ public SwerveDriveSOTM(Gamepad driver) { isIdle = BStream.create( () -> getDriverInputAsVelocity().magnitude() <= Drive.DEADBAND && Math.abs(driver.getRightX()) <= Turn.DEADBAND) - .filtered(new BDebounce.Rising(2.0), new BDebounce.Falling(0.1)); + .filtered(new BDebounce.Rising(0.5), new BDebounce.Falling(0.1)); this.driver = driver; From 929677a868548a9cf0054b9b9c8a6d91f22e7bdc Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 6 Apr 2026 16:03:03 -0400 Subject: [PATCH 02/58] fix: repeatedly digest --- src/main/java/com/stuypulse/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index f0aeac35..35395d93 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -182,7 +182,7 @@ private void configureButtonBindings() { ); // driver.getTopButton() - // .whileTrue(new IntakeAutoDigest()) + // .whileTrue(new IntakeAutoDigest().repeatedly()) // .onFalse(new IntakeDeploy()); // Intake Stow From 1ff281452cf2aa17f6aa7a55079325f280362fdc Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Mon, 6 Apr 2026 17:11:47 -0400 Subject: [PATCH 03/58] feat: fix portforwarding commands --- src/main/java/com/stuypulse/robot/Robot.java | 15 --------------- .../commands/vision/EnableBackLimelight.java | 2 +- .../commands/vision/EnableLeftLimelight.java | 2 +- .../commands/vision/EnablePortForwarding.java | 6 ++++-- .../commands/vision/EnableRightLimelight.java | 2 +- 5 files changed, 7 insertions(+), 20 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 6bceaaef..d8b26a4b 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -100,21 +100,6 @@ public void robotInit() { energyUtil = new EnergyUtil(); fmsUtil = new FMSUtil(true); gcStatsCollector = new GcStatsCollector(); - for (Camera camera : Cameras.LimelightCameras) { - cameras.setDefaultOption(camera.getName(), camera); - } - selected = cameras.getSelected(); - PortForwarder.add(5801, selected + ".local:5801", 5801); - SmartDashboard.putData("Selected Camera",cameras); - - try { - Field watchdogField = IterativeRobotBase.class.getDeclaredField("m_watchdog"); - watchdogField.setAccessible(true); - Watchdog watchdog = (Watchdog) watchdogField.get(this); - watchdog.setTimeout(Settings.LOOP_OVERRUN_WARNING_TIME_SEC); - } catch (Exception e) { - DriverStation.reportError("Failed to disable loop overrun warnings.", e.getStackTrace()); - } DataLogManager.start(); SignalLogger.start(); diff --git a/src/main/java/com/stuypulse/robot/commands/vision/EnableBackLimelight.java b/src/main/java/com/stuypulse/robot/commands/vision/EnableBackLimelight.java index 5c222ead..02a8a163 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/EnableBackLimelight.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/EnableBackLimelight.java @@ -2,7 +2,7 @@ public class EnableBackLimelight extends EnablePortForwarding { public EnableBackLimelight() { - super("limelight-back"); + super("limelight-back.local"); } } diff --git a/src/main/java/com/stuypulse/robot/commands/vision/EnableLeftLimelight.java b/src/main/java/com/stuypulse/robot/commands/vision/EnableLeftLimelight.java index 01594c67..0a0567d9 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/EnableLeftLimelight.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/EnableLeftLimelight.java @@ -2,6 +2,6 @@ public class EnableLeftLimelight extends EnablePortForwarding { public EnableLeftLimelight() { - super("limelight-left"); + super("limelight-left.local"); } } diff --git a/src/main/java/com/stuypulse/robot/commands/vision/EnablePortForwarding.java b/src/main/java/com/stuypulse/robot/commands/vision/EnablePortForwarding.java index a836cc62..ca4bc3d0 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/EnablePortForwarding.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/EnablePortForwarding.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.commands.vision; import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; public class EnablePortForwarding extends InstantCommand { @@ -12,8 +13,9 @@ public EnablePortForwarding(String hostname) { @Override public void initialize() { - PortForwarder.remove(5801); - PortForwarder.add(5801, hostname, 5801); + PortForwarder.remove(10000); + PortForwarder.add(10000, hostname, 5801); + SmartDashboard.putString("Limelight/Limelight Being Port Forwarded", hostname); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/vision/EnableRightLimelight.java b/src/main/java/com/stuypulse/robot/commands/vision/EnableRightLimelight.java index 96f4908b..f14bd871 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/EnableRightLimelight.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/EnableRightLimelight.java @@ -2,6 +2,6 @@ public class EnableRightLimelight extends EnablePortForwarding { public EnableRightLimelight() { - super("limelight-right"); + super("limelight-right.local"); } } From 8196b6731c0bd3bb7d342e596dab9d85568be636 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 6 Apr 2026 18:11:53 -0400 Subject: [PATCH 04/58] FEAT: reset pose on init --- src/main/java/com/stuypulse/robot/Robot.java | 4 ++-- .../commands/auton/regular/DepotAuton.java | 4 +++- .../commands/auton/regular/EightFuel.java | 3 ++- .../commands/auton/regular/LeftTwoCycle.java | 3 +++ .../commands/auton/regular/RightTwoCycle.java | 4 +++- .../commands/swerve/SwerveResetPose.java | 21 +++++++++++++++++++ 6 files changed, 34 insertions(+), 5 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index d8b26a4b..36a59bc2 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -179,10 +179,10 @@ public void disabledPeriodic() { auto = robot.getAutonomousCommand(); switch (auto.getName()) { case "LeftTwoCycle": - CommandScheduler.getInstance().schedule(new WhitelistRoutineLeftSideAuto()); + CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); break; case "RightTwoCycle": - CommandScheduler.getInstance().schedule(new WhitelistRoutineRightSideAuto()); + CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); break; default: CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java index 0cbe7062..e00012de 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java @@ -9,6 +9,7 @@ import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeStow; import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -22,7 +23,8 @@ public class DepotAuton extends SequentialCommandGroup { public DepotAuton(PathPlannerPath... paths) { addCommands( - + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + // To Depot new IntakeDeploy().alongWith( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java index 541e3a30..6ece7d67 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java @@ -8,6 +8,7 @@ import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -20,7 +21,7 @@ public class EightFuel extends SequentialCommandGroup { public EightFuel(PathPlannerPath... paths) { addCommands( - + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), new SuperstructureKB().alongWith( new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()).andThen( new SpindexerRun().alongWith(new HandoffRun()) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 9b05bd07..2f81c2e2 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -14,6 +14,8 @@ import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -29,6 +31,7 @@ public class LeftTwoCycle extends SequentialCommandGroup { public LeftTwoCycle(PathPlannerPath... paths) { addCommands( + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index 92309c74..80690b0c 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -14,6 +14,7 @@ import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -29,7 +30,8 @@ public class RightTwoCycle extends SequentialCommandGroup { public RightTwoCycle(PathPlannerPath... paths) { addCommands( - + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( new WaitCommand(0.5).andThen(new IntakeDeploy()) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java new file mode 100644 index 00000000..76ae8e4d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SwerveResetPose extends InstantCommand{ + private final CommandSwerveDrivetrain swerve; + private final Pose2d poseToReset; + + public SwerveResetPose(Pose2d poseToReset) { + swerve = CommandSwerveDrivetrain.getInstance(); + this.poseToReset = poseToReset; + } + + @Override + public void execute() { + swerve.resetPose(poseToReset); + } +} From 4a8809ea24f5c84d62ef76e30921221c400aeeee Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 6 Apr 2026 18:27:19 -0400 Subject: [PATCH 05/58] fix: turret uses motor's closed loop error instead of calculated, + auton changes, + increased tolerances in XOTM --- src/main/deploy/pathplanner/paths/Box 1.path | 4 +- src/main/deploy/pathplanner/paths/Box 2.path | 4 +- src/main/deploy/pathplanner/paths/Box 3.path | 4 +- src/main/deploy/pathplanner/paths/Box 4.path | 4 +- .../paths/Depot To Tower Left.path | 4 +- .../pathplanner/paths/Left Bump To Depot.path | 4 +- .../paths/Left NZ To Score (B).path | 4 +- .../pathplanner/paths/Left NZ To Score.path | 4 +- .../paths/Left Score To NZ (F).path | 4 +- .../paths/Left Trench To NZ (B).path | 4 +- .../pathplanner/paths/Left Trench To NZ.path | 6 +-- .../paths/Right NZ To Score (B).path | 4 +- .../pathplanner/paths/Right NZ To Score.path | 4 +- .../paths/Right Score To NZ (F).path | 4 +- .../paths/Right Trench To NZ (B).path | 4 +- .../paths/Right Trench To NZ (F).path | 4 +- .../pathplanner/paths/Right Trench To NZ.path | 6 +-- .../pathplanner/paths/Straight Line.path | 4 +- src/main/deploy/pathplanner/settings.json | 4 +- .../com/stuypulse/robot/RobotContainer.java | 49 ++++++++++--------- .../stuypulse/robot/constants/Settings.java | 22 ++++----- .../superstructure/hood/HoodImpl.java | 3 +- .../superstructure/shooter/Shooter.java | 2 +- .../superstructure/shooter/ShooterImpl.java | 1 + .../superstructure/turret/TurretImpl.java | 12 +++++ 25 files changed, 92 insertions(+), 77 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Box 1.path b/src/main/deploy/pathplanner/paths/Box 1.path index 25f93397..383fc29e 100644 --- a/src/main/deploy/pathplanner/paths/Box 1.path +++ b/src/main/deploy/pathplanner/paths/Box 1.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Box 2.path b/src/main/deploy/pathplanner/paths/Box 2.path index 8fc9e120..0dfb8dd4 100644 --- a/src/main/deploy/pathplanner/paths/Box 2.path +++ b/src/main/deploy/pathplanner/paths/Box 2.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Box 3.path b/src/main/deploy/pathplanner/paths/Box 3.path index f82e9f22..0eb515ac 100644 --- a/src/main/deploy/pathplanner/paths/Box 3.path +++ b/src/main/deploy/pathplanner/paths/Box 3.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Box 4.path b/src/main/deploy/pathplanner/paths/Box 4.path index 7ea9100d..0a5359c2 100644 --- a/src/main/deploy/pathplanner/paths/Box 4.path +++ b/src/main/deploy/pathplanner/paths/Box 4.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path index 451bc3cc..e222e6f0 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path index b1438a51..c19c249e 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path index 9cd4a964..fe36b11d 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path @@ -52,8 +52,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 183615ca..694dc6b8 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index 8c5c7fa3..ac2add86 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ (B).path b/src/main/deploy/pathplanner/paths/Left Trench To NZ (B).path index 60cdf558..e19531ad 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ (B).path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ (B).path @@ -82,8 +82,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 2edab82d..937d997f 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -40,7 +40,7 @@ "minWaypointRelativePos": 0.629304523970286, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.75, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, @@ -52,8 +52,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path index ee55b03c..d4fcb968 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path @@ -52,8 +52,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 22c81e32..99222c8d 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 863c4e8b..8ddd5af6 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ (B).path b/src/main/deploy/pathplanner/paths/Right Trench To NZ (B).path index 329b195e..f8e2025e 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ (B).path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ (B).path @@ -72,8 +72,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Trench To NZ (F).path index 900466c6..7c9d49ff 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ (F).path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 89fc2c0b..20071656 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -40,7 +40,7 @@ "minWaypointRelativePos": 0.7346387575962167, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.75, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, @@ -52,8 +52,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path index 8b505063..c97fab84 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line.path +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, + "maxVelocity": 3.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index a5b88d8f..230e3d45 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -13,8 +13,8 @@ "autoFolders": [ "Tests" ], - "defaultMaxVel": 4.16, - "defaultMaxAccel": 10.0, + "defaultMaxVel": 3.75, + "defaultMaxAccel": 7.0, "defaultMaxAngVel": 300.0, "defaultMaxAngAccel": 900.0, "defaultNominalVoltage": 12.5, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 35395d93..23827062 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -159,31 +159,32 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Scoring Routine (TR) - driver.getTopButton() - .whileTrue(new LEDApplyPattern(Settings.LED.SHOOT_IN_PLACE)) - .whileTrue(new WaitUntilCommand(() -> spindexer.getState() == SpindexerState.FORWARD) - .andThen(new WaitCommand(0.75).andThen(new IntakeDeploy()))) - .whileTrue(new SwerveXMode()) - .whileTrue(new BuzzController(driver).onlyWhile(() -> !vision.hasData()).repeatedly()) - .whileTrue( - new SuperstructureInterpolation() - .andThen(new WaitUntilCommand(superstructure::isReadyToShoot)) - .andThen( - Commands.parallel( - new RunCommand( - () -> handoff.setState(HandoffState.FORWARD), - handoff), - new RunCommand( - () -> spindexer.setState(SpindexerState.FORWARD), - spindexer) - ) - .repeatedly() - ) - ); - // driver.getTopButton() - // .whileTrue(new IntakeAutoDigest().repeatedly()) - // .onFalse(new IntakeDeploy()); + // .whileTrue(new LEDApplyPattern(Settings.LED.SHOOT_IN_PLACE)) + // .whileTrue(new WaitUntilCommand(() -> spindexer.getState() == SpindexerState.FORWARD) + // .andThen(new WaitCommand(0.75).andThen(new IntakeDeploy()))) + // .whileTrue(new SwerveXMode()) + // .whileTrue(new BuzzController(driver).onlyWhile(() -> !vision.hasData()).repeatedly()) + // .whileTrue( + // new SuperstructureInterpolation() + // .andThen(new WaitUntilCommand(superstructure::isReadyToShoot)) + // .andThen( + // Commands.parallel( + // new RunCommand( + // () -> handoff.setState(HandoffState.FORWARD), + // handoff), + // new RunCommand( + // () -> spindexer.setState(SpindexerState.FORWARD), + // spindexer) + // ) + // .repeatedly() + // ) + // ); + + // Digest (TR) + driver.getTopButton() + .whileTrue(new IntakeAutoDigest().repeatedly()) + .onFalse(new IntakeDeploy()); // Intake Stow driver.getLeftTriggerButton() diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index eb220212..16ebece3 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -72,7 +72,7 @@ public interface Intake { Rotation2d ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE = Rotation2d.fromDegrees(15.0); double HOMING_VOLTAGE = 3.0; - double PUSHDOWN_VOLTAGE = 2.5; + double PUSHDOWN_VOLTAGE = 3.0; double GEAR_RATIO = 37.93; @@ -144,13 +144,13 @@ public interface FerryRPMInterpolation { {6.94, 3600.0}, {7.87, 3800.0}, {9.77, 4300.0}, - {10.694, 4595.0}, //STARTING FROM HERE THE DATA IS UNRELIABLE!!! - {11.516, 4750.0}, - {12.416, 4900.0}, - {13.316, 5050.0}, - {14.216, 5175.0}, - {15.148, 5200.0}, - {16.54, 5300} //FIELD LENGTH + {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS UNRELIABLE!!! + {11.516, 4900.0}, + {12.416, 5200.0}, + {13.316, 5500.0}, + {14.216, 5600.0} + // {15.148, 5200.0}, + // {16.54, 5300} //FIELD LENGTH }; } @@ -231,8 +231,8 @@ public interface Turret { public final Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0)); public final Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); public final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); - public final SmartNumber SOTM_TOLERANCE = new SmartNumber("Superstructure/Turret/SOTM Tolerance", 5);//Rotation2d.fromDegrees(10.0); - public final Rotation2d FOTM_TOLERANCE = Rotation2d.fromDegrees(5.0); + public final SmartNumber SOTM_TOLERANCE = new SmartNumber("Superstructure/Turret/SOTM Tolerance", 7.5);//Rotation2d.fromDegrees(10.0); + public final Rotation2d FOTM_TOLERANCE = Rotation2d.fromDegrees(10.0); public final Rotation2d KB = Rotation2d.fromDegrees(0.0); public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(-233.0); @@ -292,7 +292,7 @@ public interface Swerve { public interface Constraints { public final double MAX_VELOCITY_M_PER_S = 4.16; - public final double MAX_VELOCITY_SOTM_M_PER_S = 2.0; + public final double MAX_VELOCITY_SOTM_M_PER_S = 1.75; public final double MAX_VELOCITY_FOTM_M_PER_S = 4.16; public final double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(300.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index a07d159d..25deb31f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -180,11 +180,12 @@ public void periodicAfterScheduler() { hoodMotor.stopMotor(); } - SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); + // SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); SmartDashboard.putBoolean("Prematch Checks/Hood at Top?", getAngle().getDegrees() > 39.0); SmartDashboard.putNumber("Superstructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); SmartDashboard.putNumber("Superstructure/Hood/Closed Loop Error (deg)", hoodMotorClosedLoopError.getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Superstructure/Hood/Implemented Error (Degrees)", getTargetAngle().getDegrees() - getAngle().getDegrees()); if (Settings.DEBUG_MODE.get()) { SmartDashboard.putNumber("Superstructure/Hood/Applied Voltage (amps)", hoodMotorVoltage.getValueAsDouble()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index b67ceb7d..fedb0915 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -80,7 +80,7 @@ public double getShootRPM() { } public boolean atTolerance() { - double error = getTargetRPM() - getRPM(); + double error = getRPM() - getTargetRPM(); double toleranceHigh = switch (state) { case SOTM -> Settings.Superstructure.SHOOTER_SOTM_TOLERANCE_RPM_HIGH; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index f5dad04d..7b58c180 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -196,6 +196,7 @@ public void periodicAfterScheduler() { SmartDashboard.putNumber("InterpolationTesting/Shooter Closed Loop Error (RPM)", shooterLeaderClosedLoopError.getValueAsDouble() * 60.0); + SmartDashboard.putNumber("Superstructure/Shooter/Implemented Error (RPM)", getTargetRPM() - getLeaderRPM()); } private void setVoltageOverride(Optional voltageOverride) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index c9ecd531..fc90122c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -205,6 +205,18 @@ public double getWrappedTargetAngle() { return currentAngle + getDelta(getTargetAngle().getDegrees(), currentAngle); } + @Override + public boolean atTolerance() { + double error = turretMotor.getClosedLoopError().getValueAsDouble(); + + double tolerance = switch (getState()) { + case SOTM -> Settings.Superstructure.Turret.SOTM_TOLERANCE.get() / 360.0; + case FOTM -> Settings.Superstructure.Turret.FOTM_TOLERANCE.getRotations(); + default -> Settings.Superstructure.Turret.TOLERANCE.getRotations(); + }; + + return Math.abs(error) < tolerance; + } @Override public void periodicAfterScheduler() { From f77566e5ac0bdfdb0dde15eb72b61593f90cdba4 Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Mon, 6 Apr 2026 18:51:29 -0400 Subject: [PATCH 06/58] debug: fix whitelist instead of blacklisk and add blacklistalltagsforallcameras --- src/main/java/com/stuypulse/robot/Robot.java | 16 +++++----------- .../robot/commands/swerve/SwerveResetPose.java | 2 +- .../vision/BlackListAllTagsForAllCameras.java | 13 +++++++++++++ 3 files changed, 19 insertions(+), 12 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/vision/BlackListAllTagsForAllCameras.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 36a59bc2..e2c34893 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -48,6 +48,8 @@ import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; +import com.stuypulse.robot.commands.vision.BlackListAllTagsForAllCameras; +import com.stuypulse.robot.commands.vision.BlacklistAllTags; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; public class Robot extends TimedRobot { @@ -171,23 +173,15 @@ public void disabledInit() { mode = RobotMode.DISABLED; CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG1)); + + CommandScheduler.getInstance().schedule(new BlackListAllTagsForAllCameras()); + } @Override public void disabledPeriodic() { if (periodicCounter % Settings.LOGGING_FREQUENCY == 0) { auto = robot.getAutonomousCommand(); - switch (auto.getName()) { - case "LeftTwoCycle": - CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); - break; - case "RightTwoCycle": - CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); - break; - default: - CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); - break; - } } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java index 76ae8e4d..93d72bf8 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java @@ -15,7 +15,7 @@ public SwerveResetPose(Pose2d poseToReset) { } @Override - public void execute() { + public void initialize() { swerve.resetPose(poseToReset); } } diff --git a/src/main/java/com/stuypulse/robot/commands/vision/BlackListAllTagsForAllCameras.java b/src/main/java/com/stuypulse/robot/commands/vision/BlackListAllTagsForAllCameras.java new file mode 100644 index 00000000..31a93e71 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/vision/BlackListAllTagsForAllCameras.java @@ -0,0 +1,13 @@ +package com.stuypulse.robot.commands.vision; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; + +public class BlackListAllTagsForAllCameras extends ParallelCommandGroup{ + public BlackListAllTagsForAllCameras() { + addCommands( + new BlacklistAllTags("limelight-left"), + new BlacklistAllTags("limelight-right"), + new BlacklistAllTags("limelight-back") + ); + } +} From cfb70161542082a51215773d215d720060d42ef4 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 6 Apr 2026 20:00:41 -0400 Subject: [PATCH 07/58] fix: auton changes + tolerance change --- .../deploy/pathplanner/paths/Left NZ To Score.path | 4 ++-- .../deploy/pathplanner/paths/Right NZ To Score.path | 8 ++++---- .../java/com/stuypulse/robot/constants/Settings.java | 8 ++++---- .../subsystems/superstructure/turret/TurretImpl.java | 12 ------------ 4 files changed, 10 insertions(+), 22 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 694dc6b8..81788efb 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.325477888730385, - "y": 7.5996005706134095 + "x": 7.791269614835947, + "y": 8.026576319543508 }, "isLocked": false, "linkedName": "NZ Left Center" diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 99222c8d..f44438d8 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.351355206847359, - "y": 0.43158345221112837 + "x": 7.480741797432238, + "y": 0.3410128388017126 }, "isLocked": false, "linkedName": "Right Center NZ" @@ -20,8 +20,8 @@ "y": 0.5350927246790309 }, "prevControl": { - "x": 6.885563480741796, - "y": 0.4962767475035673 + "x": 8.03710413694722, + "y": 0.5739087018544944 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 16ebece3..8a5b7eb3 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -100,13 +100,13 @@ public interface Spindexer { public interface Superstructure { public final double SHOOTER_TOLERANCE_RPM_HIGH = 50.0; public final double SHOOTER_TOLERANCE_RPM_LOW = 80.0; - public final double SHOOTER_SOTM_TOLERANCE_RPM_HIGH = 50.0; - public final double SHOOTER_SOTM_TOLERANCE_RPM_LOW = 80.0; + public final double SHOOTER_SOTM_TOLERANCE_RPM_HIGH = 100.0; + public final double SHOOTER_SOTM_TOLERANCE_RPM_LOW = 100.0; public final double SHOOTER_FOTM_TOLERANCE_RPM_HIGH = 150.0; public final double SHOOTER_FOTM_TOLERANCE_RPM_LOW = 250.0; public final Rotation2d HOOD_TOLERANCE = Rotation2d.fromDegrees(0.5); - public final Rotation2d HOOD_SOTM_TOLERANCE = Rotation2d.fromDegrees(0.5); + public final Rotation2d HOOD_SOTM_TOLERANCE = Rotation2d.fromDegrees(2); public interface AngleInterpolation { double[][] distanceAngleInterpolationValues = { @@ -231,7 +231,7 @@ public interface Turret { public final Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0)); public final Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); public final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); - public final SmartNumber SOTM_TOLERANCE = new SmartNumber("Superstructure/Turret/SOTM Tolerance", 7.5);//Rotation2d.fromDegrees(10.0); + public final SmartNumber SOTM_TOLERANCE = new SmartNumber("Superstructure/Turret/SOTM Tolerance", 6);//Rotation2d.fromDegrees(10.0); public final Rotation2d FOTM_TOLERANCE = Rotation2d.fromDegrees(10.0); public final Rotation2d KB = Rotation2d.fromDegrees(0.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index fc90122c..c9ecd531 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -205,18 +205,6 @@ public double getWrappedTargetAngle() { return currentAngle + getDelta(getTargetAngle().getDegrees(), currentAngle); } - @Override - public boolean atTolerance() { - double error = turretMotor.getClosedLoopError().getValueAsDouble(); - - double tolerance = switch (getState()) { - case SOTM -> Settings.Superstructure.Turret.SOTM_TOLERANCE.get() / 360.0; - case FOTM -> Settings.Superstructure.Turret.FOTM_TOLERANCE.getRotations(); - default -> Settings.Superstructure.Turret.TOLERANCE.getRotations(); - }; - - return Math.abs(error) < tolerance; - } @Override public void periodicAfterScheduler() { From 1d5fa9135caff03df4cb94430f97bbc57b99de6b Mon Sep 17 00:00:00 2001 From: Daniel M Date: Mon, 6 Apr 2026 23:54:39 -0400 Subject: [PATCH 08/58] fix: MT2 all da tiem! --- src/main/java/com/stuypulse/robot/Robot.java | 12 ++++-------- .../java/com/stuypulse/robot/RobotContainer.java | 8 ++++---- .../robot/util/superstructure/SOTMCalculator.java | 2 -- 3 files changed, 8 insertions(+), 14 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index e2c34893..558ab41c 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -11,7 +11,6 @@ import com.stuypulse.robot.commands.vision.WhitelistAllTagsForAllCameras; import com.stuypulse.robot.commands.vision.WhitelistRoutineLeftSideAuto; import com.stuypulse.robot.commands.vision.WhitelistRoutineRightSideAuto; -import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Cameras.Camera; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -26,11 +25,9 @@ import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.IterativeRobotBase; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Watchdog; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -38,7 +35,6 @@ import java.lang.management.GarbageCollectorMXBean; import java.lang.management.ManagementFactory; -import java.lang.reflect.Field; import java.util.List; import java.util.Timer; @@ -106,8 +102,8 @@ public void robotInit() { DataLogManager.start(); SignalLogger.start(); CommandScheduler.getInstance().schedule(new SwerveAutonInit()); - FollowPathCommand.warmupCommand().schedule(); - PathfindingCommand.warmupCommand().schedule(); + CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); + CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand()); energyUtil = new EnergyUtil(); CommandScheduler.getInstance().schedule(new SwerveAutonInit()); @@ -172,7 +168,7 @@ public void robotPeriodic() { public void disabledInit() { mode = RobotMode.DISABLED; - CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG1)); + CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new BlackListAllTagsForAllCameras()); @@ -198,7 +194,7 @@ public void autonomousInit() { auto = robot.getAutonomousCommand(); if (auto != null) { - auto.schedule(); + CommandScheduler.getInstance().schedule(auto); } } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 23827062..ba80c884 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -104,7 +104,7 @@ public interface EnabledSubsystems { SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", true); SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", true); SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); - SmartBoolean LEDS = new SmartBoolean("Enabled Subsystems/LEDs Is Enabled", true); + SmartBoolean LEDS = new SmartBoolean("Enabled Subsystems/LEDs Is Enabled", false); SmartBoolean BACK_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Back Limelight Is Enabled", true); SmartBoolean LEFT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Left Limelight Is Enabled", true); @@ -127,7 +127,7 @@ public interface EnabledSubsystems { private final Shooter shooter = Shooter.getInstance(); private final Hood hood = Hood.getInstance(); - private final LEDController leds = LEDController.getInstance(); + // private final LEDController leds = LEDController.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); @@ -150,7 +150,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - leds.setDefaultCommand(new LEDDefaultCommand()); + // leds.setDefaultCommand(new LEDDefaultCommand()); } /***************/ @@ -447,7 +447,7 @@ public void periodicAfterScheduler() { handoff.periodicAfterScheduler(); intake.periodicAfterScheduler(); - leds.periodicAfterScheduler(); + // leds.periodicAfterScheduler(); TODO: ADD THESE BACK TY spindexer.periodicAfterScheduler(); hood.periodicAfterScheduler(); shooter.periodicAfterScheduler(); diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index 0a2cc784..eae5f66a 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -25,8 +25,6 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - public class SOTMCalculator { public static SmartBoolean accountForAccel = new SmartBoolean("Superstructure/SOTM/account for accel", false); From 54a4722b87e787de28f2565e2891286b1965f3fb Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Tue, 7 Apr 2026 00:17:43 -0400 Subject: [PATCH 09/58] FEAT: auto starting position changes --- .../deploy/pathplanner/paths/Left Trench To NZ.path | 10 +++++----- .../deploy/pathplanner/paths/Right Trench To NZ.path | 12 ++++++------ .../robot/commands/auton/regular/LeftTwoCycle.java | 3 ++- .../robot/commands/auton/regular/RightTwoCycle.java | 3 ++- 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 937d997f..1cc656e3 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.998600237247925, - "y": 7.526358244365362 + "x": 4.439727164887308, + "y": 7.655468564650059 }, "prevControl": null, "nextControl": { - "x": 8.99456490727532, - "y": 7.509029957203994 + "x": 9.033902728351128, + "y": 7.687746144721235 }, "isLocked": false, "linkedName": "Left Trench Start" @@ -67,7 +67,7 @@ "folder": "To NZ", "idealStartingState": { "velocity": 0, - "rotation": -76.49029381374567 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 20071656..5f732856 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.039058487874465, - "y": 0.5221540656205423 + "x": 4.439727164887308, + "y": 0.43604982206405685 }, "prevControl": null, "nextControl": { - "x": 9.408601997146931, - "y": 0.44452211126961516 + "x": 9.07693950177936, + "y": 0.4037722419928821 }, "isLocked": false, "linkedName": "Right Trench Start" @@ -20,8 +20,8 @@ "y": 3.5498002853067057 }, "prevControl": { - "x": 8.748730385164048, - "y": -0.5258773181169758 + "x": 8.50670225385528, + "y": -0.04811387900355957 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 2f81c2e2..21334790 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -31,6 +31,7 @@ public class LeftTwoCycle extends SequentialCommandGroup { public LeftTwoCycle(PathPlannerPath... paths) { addCommands( + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), // NZ Trip 1 @@ -62,7 +63,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(5.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index 80690b0c..ab3a445f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -30,6 +30,7 @@ public class RightTwoCycle extends SequentialCommandGroup { public RightTwoCycle(PathPlannerPath... paths) { addCommands( + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), // NZ Trip 1 @@ -61,7 +62,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(5.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), new ParallelCommandGroup( From 46f76996ae6da05101d7cc7388f033cf060d2859 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 7 Apr 2026 17:29:31 -0400 Subject: [PATCH 10/58] FEAT: auto changes, gains changes, sotm in neutral zone changes --- elastic-layout.json | 107 ++++++++++++------ simgui-ds.json | 1 + .../paths/Left NZ To Score (B).path | 8 +- .../pathplanner/paths/Left NZ To Score.path | 36 ++++-- .../paths/Left Score To NZ (F).path | 16 +-- .../paths/Left Score To Score.path | 28 +++-- .../pathplanner/paths/Left Trench To NZ.path | 22 ++-- .../paths/Right NZ To Score (B).path | 8 +- .../pathplanner/paths/Right NZ To Score.path | 36 ++++-- .../paths/Right Score To NZ (F).path | 16 +-- .../paths/Right Score To Score.path | 28 +++-- .../pathplanner/paths/Right Trench To NZ.path | 20 ++-- src/main/java/com/stuypulse/robot/Robot.java | 25 ++-- .../commands/auton/regular/LeftTwoCycle.java | 4 +- .../commands/auton/regular/RightTwoCycle.java | 4 +- .../com/stuypulse/robot/constants/Gains.java | 1 + .../robot/subsystems/intake/IntakeImpl.java | 4 +- .../superstructure/turret/TurretImpl.java | 6 +- 18 files changed, 227 insertions(+), 143 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index f475b1d9..1c569174 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -378,7 +378,7 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/FMSUtil/Time Left In Shift", + "topic": "/SmartDashboard/FMSUtil/time left in shift", "period": 0.06, "data_type": "double", "time_display_mode": "Minutes and Seconds", @@ -860,9 +860,8 @@ "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/FMSUtil/Is Active Shift?", + "topic": "/SmartDashboard/is active shift", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -877,7 +876,7 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/FMSUtil/Time Left In Shift", + "topic": "/SmartDashboard/FMSUtil/time left in shift", "period": 0.06, "data_type": "double", "time_display_mode": "Minutes and Seconds", @@ -976,7 +975,7 @@ } }, { - "title": "Current Intake Angle (deg)", + "title": "Current Intake Angle", "x": 910.0, "y": 700.0, "width": 210.0, @@ -1434,20 +1433,6 @@ "show_submit_button": false } }, - { - "title": "Current Intake Angle (deg)", - "x": 420.0, - "y": 560.0, - "width": 210.0, - "height": 140.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Intake/Current Angle (deg)", - "period": 0.5, - "data_type": "double", - "show_submit_button": false - } - }, { "title": "Seed Turret", "x": 980.0, @@ -1476,20 +1461,6 @@ "show_submit_button": false } }, - { - "title": "Current Hood Angle", - "x": 0.0, - "y": 560.0, - "width": 210.0, - "height": 140.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", - "period": 0.5, - "data_type": "double", - "show_submit_button": false - } - }, { "title": "Seed Intake At Lower Limit", "x": 700.0, @@ -1518,6 +1489,48 @@ "maximize_button_space": true } }, + { + "title": "Set Back LL PF", + "x": 0.0, + "y": 700.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Set Back LL PF", + "period": 0.5, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "Set Left LL PF", + "x": 280.0, + "y": 700.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Set Left LL PF", + "period": 0.5, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "Set Right LL PF", + "x": 560.0, + "y": 700.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Set Right LL PF", + "period": 0.5, + "show_type": true, + "maximize_button_space": false + } + }, { "title": "Seed Hood Relative Encoder At Lower Hardstop", "x": 0.0, @@ -1531,6 +1544,34 @@ "show_type": true, "maximize_button_space": false } + }, + { + "title": "Current Intake Angle", + "x": 420.0, + "y": 560.0, + "width": 210.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/Current Angle (deg)", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Current Hood Angle", + "x": 0.0, + "y": 560.0, + "width": 210.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } } ] } diff --git a/simgui-ds.json b/simgui-ds.json index ff81e307..3cddee95 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -99,6 +99,7 @@ } ], "robotJoysticks": [ + {}, { "guid": "Keyboard0" } diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path index fe36b11d..b204f5d0 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.27, - "y": 4.61077032810271 + "x": 8.308815977175463, + "y": 4.74015691868759 }, "prevControl": null, "nextControl": { - "x": 5.515784635743212, - "y": 5.167086622567657 + "x": 5.554600612918676, + "y": 5.296473213152537 }, "isLocked": false, "linkedName": "NZ Left Center" diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 81788efb..9503c764 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.27, - "y": 4.61077032810271 + "x": 8.308815977175463, + "y": 4.74015691868759 }, "prevControl": null, "nextControl": { - "x": 7.791269614835947, - "y": 8.026576319543508 + "x": 4.713277777777778, + "y": 6.188522222222222 }, "isLocked": false, "linkedName": "NZ Left Center" }, { "anchor": { - "x": 3.6120827389443653, - "y": 7.509029957203994 + "x": 3.0815977175463622, + "y": 7.250256776034237 }, "prevControl": { - "x": 7.428987161198288, - "y": 7.5996005706134095 + "x": 10.300853051193904, + "y": 7.3675474243210735 }, "nextControl": null, "isLocked": false, @@ -31,10 +31,24 @@ "rotationTargets": [ { "waypointRelativePos": 0.8063943161634181, - "rotationDegrees": -90.0 + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8372721134368668, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.5, + "unlimited": false + } } ], - "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -47,7 +61,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -90.0 + "rotation": 0.0 }, "reversed": false, "folder": "To Score", diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index ac2add86..cdaccb2e 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.6250213980028523, - "y": 7.483152639087019 + "x": 3.4075222222222226, + "y": 7.572233333333333 }, "prevControl": null, "nextControl": { - "x": 6.833808844507845, - "y": 7.483152639087019 + "x": 6.616309668727215, + "y": 7.572233333333333 }, "isLocked": false, "linkedName": "Left Score (F)" }, { "anchor": { - "x": 8.27, - "y": 4.61077032810271 + "x": 8.308815977175463, + "y": 4.74015691868759 }, "prevControl": { - "x": 7.881840228245363, - "y": 7.185563480741797 + "x": 7.920656205420826, + "y": 7.314950071326677 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index 38f52769..e975816c 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, - "y": 7.509029957203994 + "x": 3.0815977175463622, + "y": 7.250256776034237 }, "prevControl": null, "nextControl": { - "x": 6.92437945791726, - "y": 7.651355206847361 + "x": 6.852697717549599, + "y": 7.376934553817899 }, "isLocked": false, "linkedName": "Left Trench Score" @@ -64,12 +64,12 @@ }, { "anchor": { - "x": 3.6250213980028523, - "y": 7.483152639087019 + "x": 3.4075222222222226, + "y": 7.572233333333333 }, "prevControl": { - "x": 5.281169757489301, - "y": 7.4418502292099244 + "x": 5.06367058170867, + "y": 7.530930923456238 }, "nextControl": null, "isLocked": false, @@ -78,7 +78,15 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.8911495422177071, + "waypointRelativePos": 0.23880597014925345, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9593360995850618, "rotationDegrees": -90.0 }, { @@ -113,7 +121,7 @@ "folder": "To Score", "idealStartingState": { "velocity": 0.0, - "rotation": -90.0 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 1cc656e3..f6b5d239 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.439727164887308, - "y": 7.655468564650059 + "x": 3.998600237247925, + "y": 7.526358244365362 }, "prevControl": null, "nextControl": { - "x": 9.033902728351128, - "y": 7.687746144721235 + "x": 8.99456490727532, + "y": 7.509029957203994 }, "isLocked": false, "linkedName": "Left Trench Start" }, { "anchor": { - "x": 8.27, - "y": 4.61077032810271 + "x": 8.308815977175463, + "y": 4.74015691868759 }, "prevControl": { - "x": 8.218245363766048, - "y": 7.431398002853067 + "x": 8.257061340941512, + "y": 7.560784593437947 }, "nextControl": null, "isLocked": false, @@ -40,8 +40,8 @@ "minWaypointRelativePos": 0.629304523970286, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.75, - "maxAcceleration": 10.0, + "maxVelocity": 1.5, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, @@ -67,7 +67,7 @@ "folder": "To NZ", "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": -76.49029381374567 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path index d4fcb968..4f1fc3e5 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.56758915834522, - "y": 3.5498002853067057 + "x": 8.593466476462195, + "y": 3.3557203994293876 }, "prevControl": null, "nextControl": { - "x": 5.639047234674071, - "y": 2.8277371590830254 + "x": 5.664924552791047, + "y": 2.6336572732057073 }, "isLocked": false, "linkedName": "Right Center NZ" diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index f44438d8..69554862 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.56758915834522, - "y": 3.5498002853067057 + "x": 8.593466476462195, + "y": 3.3557203994293876 }, "prevControl": null, "nextControl": { - "x": 7.480741797432238, - "y": 0.3410128388017126 + "x": 5.643452211126961, + "y": 2.8381740370898734 }, "isLocked": false, "linkedName": "Right Center NZ" }, { "anchor": { - "x": 3.5473894436519258, - "y": 0.5350927246790309 + "x": 2.771069900142653, + "y": 0.71623395149786 }, "prevControl": { - "x": 8.03710413694722, - "y": 0.5739087018544944 + "x": 9.926148359486447, + "y": 0.3539514978602004 }, "nextControl": null, "isLocked": false, @@ -31,10 +31,24 @@ "rotationTargets": [ { "waypointRelativePos": 0.7921847246891675, - "rotationDegrees": 90.0 + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8507765023632616, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.5, + "unlimited": false + } } ], - "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -47,7 +61,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 90.0 + "rotation": 0.0 }, "reversed": false, "folder": "To Score", diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 8ddd5af6..226c9169 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.6250213980028523, - "y": 0.5868473609129818 + "x": 3.3880333333333335, + "y": 0.5075111111111108 }, "prevControl": null, "nextControl": { - "x": 7.196091298145506, - "y": 0.5739087018544944 + "x": 6.959103233475987, + "y": 0.49457245205262335 }, "isLocked": false, "linkedName": "Right Score (F)" }, { "anchor": { - "x": 8.56758915834522, - "y": 3.5498002853067057 + "x": 8.593466476462195, + "y": 3.3557203994293876 }, "prevControl": { - "x": 8.515834522111266, - "y": 0.819743223965764 + "x": 8.541711840228242, + "y": 0.625663338088446 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 8adafc11..7c4274b4 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.5473894436519258, - "y": 0.5350927246790309 + "x": 2.771069900142653, + "y": 0.71623395149786 }, "prevControl": null, "nextControl": { - "x": 6.833808844507845, - "y": 0.37982881597717666 + "x": 6.503192122364876, + "y": 0.6967450626089725 }, "isLocked": false, "linkedName": "Right Trench Score" @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 3.6250213980028523, - "y": 0.5868473609129818 + "x": 3.3880333333333335, + "y": 0.5075111111111108 }, "prevControl": { - "x": 6.989072753209699, - "y": 0.4962767475035662 + "x": 6.7520846885401795, + "y": 0.41694049770169517 }, "nextControl": null, "isLocked": false, @@ -62,7 +62,15 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.9722814498933927, + "waypointRelativePos": 0.23027718550106568, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.7344398340248979, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9933609958506272, "rotationDegrees": 90.0 }, { @@ -97,7 +105,7 @@ "folder": "To Score", "idealStartingState": { "velocity": 0, - "rotation": 90.0 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 5f732856..196f1b31 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.439727164887308, - "y": 0.43604982206405685 + "x": 4.039058487874465, + "y": 0.5221540656205423 }, "prevControl": null, "nextControl": { - "x": 9.07693950177936, - "y": 0.4037722419928821 + "x": 9.408601997146931, + "y": 0.44452211126961516 }, "isLocked": false, "linkedName": "Right Trench Start" }, { "anchor": { - "x": 8.56758915834522, - "y": 3.5498002853067057 + "x": 8.593466476462195, + "y": 3.3557203994293876 }, "prevControl": { - "x": 8.50670225385528, - "y": -0.04811387900355957 + "x": 8.774607703281024, + "y": -0.7199572039942939 }, "nextControl": null, "isLocked": false, @@ -40,8 +40,8 @@ "minWaypointRelativePos": 0.7346387575962167, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.75, - "maxAcceleration": 10.0, + "maxVelocity": 1.5, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 558ab41c..49367613 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -22,6 +22,7 @@ import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.stuylib.network.SmartBoolean; +import edu.wpi.first.hal.HAL; import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -36,7 +37,6 @@ import java.lang.management.GarbageCollectorMXBean; import java.lang.management.ManagementFactory; import java.util.List; -import java.util.Timer; import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.commands.FollowPathCommand; @@ -45,7 +45,6 @@ import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.vision.BlackListAllTagsForAllCameras; -import com.stuypulse.robot.commands.vision.BlacklistAllTags; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; public class Robot extends TimedRobot { @@ -56,8 +55,6 @@ public enum RobotMode { TEST } - private Timer threadTimer; - private RobotContainer robot; private Command auto; private static Alliance alliance; @@ -67,7 +64,6 @@ public enum RobotMode { private SendableChooser cameras = new SendableChooser(); private Camera selected; private GcStatsCollector gcStatsCollector; - private SmartBoolean shouldRunSecondThread; private static int periodicCounter = 0; @@ -143,19 +139,12 @@ public void robotPeriodic() { alliance = DriverStation.getAlliance().get(); } - if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && Superstructure.getInstance().getState() == SuperstructureState.SOTM) { - CommandScheduler.getInstance().schedule( - new SuperstructureFOTM(), - new SpindexerStop(), - new HandoffStop() - ); - } - SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime()); SmartDashboard.putData("Robot/Scheduled Commands", CommandScheduler.getInstance()); SmartDashboard.putNumber("Robot/Battery Voltage", batteryVoltage); SmartDashboard.putNumber("Robot/CPU Temperature (C)", RobotController.getCPUTemp()); - + SmartDashboard.putNumber("Robot/Times Disconnected", HAL.getCommsDisableCount()); + robot.periodicAfterScheduler(); energyUtil.periodic(); } @@ -229,6 +218,14 @@ public void teleopPeriodic() { SmartDashboard.putNumber("FMSUtil/time left in shift", fmsUtil.getTimeLeftInShift()); SmartDashboard.putBoolean("FMSUtil/is active shift", fmsUtil.isActiveShift()); SmartDashboard.putBoolean("FMSUtil/won auto?", fmsUtil.didWinAuto()); + + if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && Superstructure.getInstance().getState() == SuperstructureState.SOTM) { + CommandScheduler.getInstance().schedule( + new SuperstructureFOTM(), + new SpindexerStop(), + new HandoffStop() + ); + } } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 21334790..d2ca9149 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -48,7 +48,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(3.5), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 @@ -63,7 +63,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(5.0), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index ab3a445f..2224300c 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -47,7 +47,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(3.5), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 @@ -62,7 +62,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(5.0), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index b0104961..0eb92fb6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -55,6 +55,7 @@ public interface slot1 { } SmartNumber kOmega = new SmartNumber("Superstructure/Turret/Gains/kOmega", 3.43); + SmartNumber kTranslation = new SmartNumber("Superstructure/Turret/Gains/kTranslation", 0.0); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index dfcdfb61..dc91570c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -88,8 +88,8 @@ public IntakeImpl() { rollerConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake) - .withSupplyCurrentLimitAmps(30.0) + .withNeutralMode(NeutralModeValue.Coast) + .withSupplyCurrentLimitAmps(40.0) .withStatorCurrentLimitEnabled(false) .withRampRate(0.50); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index c9ecd531..34812073 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -265,13 +265,13 @@ public void periodicAfterScheduler() { double setpointVelocityRPS = delta / (360 * Settings.DT); // the component of the turret's setpoint velocity that comes from robot translation - double translationalVelocityRPS = setpointVelocityRPS - omega / (2 * Math.PI); - double translationFF = Gains.Superstructure.Turret.slot0.kV * translationalVelocityRPS; + double translationalComponentVelocityRPS = setpointVelocityRPS - omega / (2 * Math.PI); + double translationFF = Gains.Superstructure.Turret.kTranslation.get() * translationalComponentVelocityRPS; turretMotor.setControl(controller .withPosition(prevActualTargetAngle / 360.0) .withSlot(slot) - .withFeedForward(omegaFF /* + translationFF */) + .withFeedForward(omegaFF + translationFF) ); } } else { From 37f00122ed2f84bd0b283fa0698a771284fc8d50 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Tue, 7 Apr 2026 17:58:54 -0400 Subject: [PATCH 11/58] feat: add automatic pipeline switching when no data --- .../subsystems/vision/LimelightVision.java | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 40f684dc..24e3a458 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -54,11 +54,42 @@ public static LimelightVision getInstance() { private boolean hasData; private BStream debouncedHasData; + private Pipeline currentPipeline; + public enum MegaTagMode { MEGATAG1, MEGATAG2 } + public enum Pipeline { + NO_SUN, + LOW_SUN, + MED_SUN, + HIGH_SUN + } + + private int getCurrentPipelineID() { + return switch(this.currentPipeline) { + case NO_SUN -> 0; + case LOW_SUN -> 1; + case MED_SUN -> 2; + case HIGH_SUN -> 3; + }; + } + + private void iteratePipelineValue() { + this.currentPipeline = switch(this.currentPipeline) { + case NO_SUN -> Pipeline.LOW_SUN; + case LOW_SUN -> Pipeline.MED_SUN; + case MED_SUN -> Pipeline.HIGH_SUN; + case HIGH_SUN -> Pipeline.NO_SUN; + }; + } + + public void setPipeline(String cameraName, Pipeline pipeline) { + this.currentPipeline = pipeline; + } + public LimelightVision() { limelightPoseArray = new Pose2d[Cameras.LimelightCameras.length]; leftLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Left", Pose2d.struct).publish(); @@ -301,6 +332,11 @@ public void periodicAfterScheduler() { } SmartDashboard.putBoolean("Vision/Has Data", hasData); + + for(String cameraName: names) { + iteratePipelineValue(); + LimelightHelpers.setPipelineIndex(cameraName, getCurrentPipelineID()); + } } } } From 8aa8eab8241ef8e6dfd80e10ca30b0cc3709f8f4 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 7 Apr 2026 18:12:02 -0400 Subject: [PATCH 12/58] Revert "FEAT: auto changes, gains changes, sotm in neutral zone changes" This reverts commit 46f76996ae6da05101d7cc7388f033cf060d2859. --- elastic-layout.json | 107 ++++++------------ simgui-ds.json | 1 - .../paths/Left NZ To Score (B).path | 8 +- .../pathplanner/paths/Left NZ To Score.path | 36 ++---- .../paths/Left Score To NZ (F).path | 16 +-- .../paths/Left Score To Score.path | 28 ++--- .../pathplanner/paths/Left Trench To NZ.path | 22 ++-- .../paths/Right NZ To Score (B).path | 8 +- .../pathplanner/paths/Right NZ To Score.path | 36 ++---- .../paths/Right Score To NZ (F).path | 16 +-- .../paths/Right Score To Score.path | 28 ++--- .../pathplanner/paths/Right Trench To NZ.path | 20 ++-- src/main/java/com/stuypulse/robot/Robot.java | 25 ++-- .../commands/auton/regular/LeftTwoCycle.java | 4 +- .../commands/auton/regular/RightTwoCycle.java | 4 +- .../com/stuypulse/robot/constants/Gains.java | 1 - .../robot/subsystems/intake/IntakeImpl.java | 4 +- .../superstructure/turret/TurretImpl.java | 6 +- 18 files changed, 143 insertions(+), 227 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 1c569174..f475b1d9 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -378,7 +378,7 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/FMSUtil/time left in shift", + "topic": "/SmartDashboard/FMSUtil/Time Left In Shift", "period": 0.06, "data_type": "double", "time_display_mode": "Minutes and Seconds", @@ -860,8 +860,9 @@ "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/is active shift", + "topic": "/SmartDashboard/FMSUtil/Is Active Shift?", "period": 0.06, + "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -876,7 +877,7 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/FMSUtil/time left in shift", + "topic": "/SmartDashboard/FMSUtil/Time Left In Shift", "period": 0.06, "data_type": "double", "time_display_mode": "Minutes and Seconds", @@ -975,7 +976,7 @@ } }, { - "title": "Current Intake Angle", + "title": "Current Intake Angle (deg)", "x": 910.0, "y": 700.0, "width": 210.0, @@ -1433,6 +1434,20 @@ "show_submit_button": false } }, + { + "title": "Current Intake Angle (deg)", + "x": 420.0, + "y": 560.0, + "width": 210.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/Current Angle (deg)", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, { "title": "Seed Turret", "x": 980.0, @@ -1461,6 +1476,20 @@ "show_submit_button": false } }, + { + "title": "Current Hood Angle", + "x": 0.0, + "y": 560.0, + "width": 210.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, { "title": "Seed Intake At Lower Limit", "x": 700.0, @@ -1489,48 +1518,6 @@ "maximize_button_space": true } }, - { - "title": "Set Back LL PF", - "x": 0.0, - "y": 700.0, - "width": 280.0, - "height": 140.0, - "type": "Command", - "properties": { - "topic": "/SmartDashboard/Robot/Set Back LL PF", - "period": 0.5, - "show_type": true, - "maximize_button_space": false - } - }, - { - "title": "Set Left LL PF", - "x": 280.0, - "y": 700.0, - "width": 280.0, - "height": 140.0, - "type": "Command", - "properties": { - "topic": "/SmartDashboard/Robot/Set Left LL PF", - "period": 0.5, - "show_type": true, - "maximize_button_space": false - } - }, - { - "title": "Set Right LL PF", - "x": 560.0, - "y": 700.0, - "width": 280.0, - "height": 140.0, - "type": "Command", - "properties": { - "topic": "/SmartDashboard/Robot/Set Right LL PF", - "period": 0.5, - "show_type": true, - "maximize_button_space": false - } - }, { "title": "Seed Hood Relative Encoder At Lower Hardstop", "x": 0.0, @@ -1544,34 +1531,6 @@ "show_type": true, "maximize_button_space": false } - }, - { - "title": "Current Intake Angle", - "x": 420.0, - "y": 560.0, - "width": 210.0, - "height": 140.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Intake/Current Angle (deg)", - "period": 0.5, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Current Hood Angle", - "x": 0.0, - "y": 560.0, - "width": 210.0, - "height": 140.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", - "period": 0.5, - "data_type": "double", - "show_submit_button": false - } } ] } diff --git a/simgui-ds.json b/simgui-ds.json index 3cddee95..ff81e307 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -99,7 +99,6 @@ } ], "robotJoysticks": [ - {}, { "guid": "Keyboard0" } diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path index b204f5d0..fe36b11d 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.308815977175463, - "y": 4.74015691868759 + "x": 8.27, + "y": 4.61077032810271 }, "prevControl": null, "nextControl": { - "x": 5.554600612918676, - "y": 5.296473213152537 + "x": 5.515784635743212, + "y": 5.167086622567657 }, "isLocked": false, "linkedName": "NZ Left Center" diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 9503c764..81788efb 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.308815977175463, - "y": 4.74015691868759 + "x": 8.27, + "y": 4.61077032810271 }, "prevControl": null, "nextControl": { - "x": 4.713277777777778, - "y": 6.188522222222222 + "x": 7.791269614835947, + "y": 8.026576319543508 }, "isLocked": false, "linkedName": "NZ Left Center" }, { "anchor": { - "x": 3.0815977175463622, - "y": 7.250256776034237 + "x": 3.6120827389443653, + "y": 7.509029957203994 }, "prevControl": { - "x": 10.300853051193904, - "y": 7.3675474243210735 + "x": 7.428987161198288, + "y": 7.5996005706134095 }, "nextControl": null, "isLocked": false, @@ -31,24 +31,10 @@ "rotationTargets": [ { "waypointRelativePos": 0.8063943161634181, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.8372721134368668, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - } + "rotationDegrees": -90.0 } ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -61,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": -90.0 }, "reversed": false, "folder": "To Score", diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index cdaccb2e..ac2add86 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.4075222222222226, - "y": 7.572233333333333 + "x": 3.6250213980028523, + "y": 7.483152639087019 }, "prevControl": null, "nextControl": { - "x": 6.616309668727215, - "y": 7.572233333333333 + "x": 6.833808844507845, + "y": 7.483152639087019 }, "isLocked": false, "linkedName": "Left Score (F)" }, { "anchor": { - "x": 8.308815977175463, - "y": 4.74015691868759 + "x": 8.27, + "y": 4.61077032810271 }, "prevControl": { - "x": 7.920656205420826, - "y": 7.314950071326677 + "x": 7.881840228245363, + "y": 7.185563480741797 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index e975816c..38f52769 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.0815977175463622, - "y": 7.250256776034237 + "x": 3.6120827389443653, + "y": 7.509029957203994 }, "prevControl": null, "nextControl": { - "x": 6.852697717549599, - "y": 7.376934553817899 + "x": 6.92437945791726, + "y": 7.651355206847361 }, "isLocked": false, "linkedName": "Left Trench Score" @@ -64,12 +64,12 @@ }, { "anchor": { - "x": 3.4075222222222226, - "y": 7.572233333333333 + "x": 3.6250213980028523, + "y": 7.483152639087019 }, "prevControl": { - "x": 5.06367058170867, - "y": 7.530930923456238 + "x": 5.281169757489301, + "y": 7.4418502292099244 }, "nextControl": null, "isLocked": false, @@ -78,15 +78,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.23880597014925345, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.6340248962655519, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 0.9593360995850618, + "waypointRelativePos": 0.8911495422177071, "rotationDegrees": -90.0 }, { @@ -121,7 +113,7 @@ "folder": "To Score", "idealStartingState": { "velocity": 0.0, - "rotation": 0.0 + "rotation": -90.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index f6b5d239..1cc656e3 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.998600237247925, - "y": 7.526358244365362 + "x": 4.439727164887308, + "y": 7.655468564650059 }, "prevControl": null, "nextControl": { - "x": 8.99456490727532, - "y": 7.509029957203994 + "x": 9.033902728351128, + "y": 7.687746144721235 }, "isLocked": false, "linkedName": "Left Trench Start" }, { "anchor": { - "x": 8.308815977175463, - "y": 4.74015691868759 + "x": 8.27, + "y": 4.61077032810271 }, "prevControl": { - "x": 8.257061340941512, - "y": 7.560784593437947 + "x": 8.218245363766048, + "y": 7.431398002853067 }, "nextControl": null, "isLocked": false, @@ -40,8 +40,8 @@ "minWaypointRelativePos": 0.629304523970286, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 7.0, + "maxVelocity": 1.75, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, @@ -67,7 +67,7 @@ "folder": "To NZ", "idealStartingState": { "velocity": 0, - "rotation": -76.49029381374567 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path index 4f1fc3e5..d4fcb968 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.593466476462195, - "y": 3.3557203994293876 + "x": 8.56758915834522, + "y": 3.5498002853067057 }, "prevControl": null, "nextControl": { - "x": 5.664924552791047, - "y": 2.6336572732057073 + "x": 5.639047234674071, + "y": 2.8277371590830254 }, "isLocked": false, "linkedName": "Right Center NZ" diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 69554862..f44438d8 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.593466476462195, - "y": 3.3557203994293876 + "x": 8.56758915834522, + "y": 3.5498002853067057 }, "prevControl": null, "nextControl": { - "x": 5.643452211126961, - "y": 2.8381740370898734 + "x": 7.480741797432238, + "y": 0.3410128388017126 }, "isLocked": false, "linkedName": "Right Center NZ" }, { "anchor": { - "x": 2.771069900142653, - "y": 0.71623395149786 + "x": 3.5473894436519258, + "y": 0.5350927246790309 }, "prevControl": { - "x": 9.926148359486447, - "y": 0.3539514978602004 + "x": 8.03710413694722, + "y": 0.5739087018544944 }, "nextControl": null, "isLocked": false, @@ -31,24 +31,10 @@ "rotationTargets": [ { "waypointRelativePos": 0.7921847246891675, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.8507765023632616, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - } + "rotationDegrees": 90.0 } ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -61,7 +47,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 0.0 + "rotation": 90.0 }, "reversed": false, "folder": "To Score", diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 226c9169..8ddd5af6 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.3880333333333335, - "y": 0.5075111111111108 + "x": 3.6250213980028523, + "y": 0.5868473609129818 }, "prevControl": null, "nextControl": { - "x": 6.959103233475987, - "y": 0.49457245205262335 + "x": 7.196091298145506, + "y": 0.5739087018544944 }, "isLocked": false, "linkedName": "Right Score (F)" }, { "anchor": { - "x": 8.593466476462195, - "y": 3.3557203994293876 + "x": 8.56758915834522, + "y": 3.5498002853067057 }, "prevControl": { - "x": 8.541711840228242, - "y": 0.625663338088446 + "x": 8.515834522111266, + "y": 0.819743223965764 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 7c4274b4..8adafc11 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.771069900142653, - "y": 0.71623395149786 + "x": 3.5473894436519258, + "y": 0.5350927246790309 }, "prevControl": null, "nextControl": { - "x": 6.503192122364876, - "y": 0.6967450626089725 + "x": 6.833808844507845, + "y": 0.37982881597717666 }, "isLocked": false, "linkedName": "Right Trench Score" @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 3.3880333333333335, - "y": 0.5075111111111108 + "x": 3.6250213980028523, + "y": 0.5868473609129818 }, "prevControl": { - "x": 6.7520846885401795, - "y": 0.41694049770169517 + "x": 6.989072753209699, + "y": 0.4962767475035662 }, "nextControl": null, "isLocked": false, @@ -62,15 +62,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.23027718550106568, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.7344398340248979, - "rotationDegrees": 90.0 - }, - { - "waypointRelativePos": 0.9933609958506272, + "waypointRelativePos": 0.9722814498933927, "rotationDegrees": 90.0 }, { @@ -105,7 +97,7 @@ "folder": "To Score", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 90.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 196f1b31..5f732856 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.039058487874465, - "y": 0.5221540656205423 + "x": 4.439727164887308, + "y": 0.43604982206405685 }, "prevControl": null, "nextControl": { - "x": 9.408601997146931, - "y": 0.44452211126961516 + "x": 9.07693950177936, + "y": 0.4037722419928821 }, "isLocked": false, "linkedName": "Right Trench Start" }, { "anchor": { - "x": 8.593466476462195, - "y": 3.3557203994293876 + "x": 8.56758915834522, + "y": 3.5498002853067057 }, "prevControl": { - "x": 8.774607703281024, - "y": -0.7199572039942939 + "x": 8.50670225385528, + "y": -0.04811387900355957 }, "nextControl": null, "isLocked": false, @@ -40,8 +40,8 @@ "minWaypointRelativePos": 0.7346387575962167, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 7.0, + "maxVelocity": 1.75, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 49367613..558ab41c 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -22,7 +22,6 @@ import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.stuylib.network.SmartBoolean; -import edu.wpi.first.hal.HAL; import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -37,6 +36,7 @@ import java.lang.management.GarbageCollectorMXBean; import java.lang.management.ManagementFactory; import java.util.List; +import java.util.Timer; import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.commands.FollowPathCommand; @@ -45,6 +45,7 @@ import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.vision.BlackListAllTagsForAllCameras; +import com.stuypulse.robot.commands.vision.BlacklistAllTags; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; public class Robot extends TimedRobot { @@ -55,6 +56,8 @@ public enum RobotMode { TEST } + private Timer threadTimer; + private RobotContainer robot; private Command auto; private static Alliance alliance; @@ -64,6 +67,7 @@ public enum RobotMode { private SendableChooser cameras = new SendableChooser(); private Camera selected; private GcStatsCollector gcStatsCollector; + private SmartBoolean shouldRunSecondThread; private static int periodicCounter = 0; @@ -139,12 +143,19 @@ public void robotPeriodic() { alliance = DriverStation.getAlliance().get(); } + if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && Superstructure.getInstance().getState() == SuperstructureState.SOTM) { + CommandScheduler.getInstance().schedule( + new SuperstructureFOTM(), + new SpindexerStop(), + new HandoffStop() + ); + } + SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime()); SmartDashboard.putData("Robot/Scheduled Commands", CommandScheduler.getInstance()); SmartDashboard.putNumber("Robot/Battery Voltage", batteryVoltage); SmartDashboard.putNumber("Robot/CPU Temperature (C)", RobotController.getCPUTemp()); - SmartDashboard.putNumber("Robot/Times Disconnected", HAL.getCommsDisableCount()); - + robot.periodicAfterScheduler(); energyUtil.periodic(); } @@ -218,14 +229,6 @@ public void teleopPeriodic() { SmartDashboard.putNumber("FMSUtil/time left in shift", fmsUtil.getTimeLeftInShift()); SmartDashboard.putBoolean("FMSUtil/is active shift", fmsUtil.isActiveShift()); SmartDashboard.putBoolean("FMSUtil/won auto?", fmsUtil.didWinAuto()); - - if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && Superstructure.getInstance().getState() == SuperstructureState.SOTM) { - CommandScheduler.getInstance().schedule( - new SuperstructureFOTM(), - new SpindexerStop(), - new HandoffStop() - ); - } } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index d2ca9149..21334790 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -48,7 +48,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(3.5), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 @@ -63,7 +63,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(5.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index 2224300c..ab3a445f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -47,7 +47,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(3.5), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 @@ -62,7 +62,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(5.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 0eb92fb6..b0104961 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -55,7 +55,6 @@ public interface slot1 { } SmartNumber kOmega = new SmartNumber("Superstructure/Turret/Gains/kOmega", 3.43); - SmartNumber kTranslation = new SmartNumber("Superstructure/Turret/Gains/kTranslation", 0.0); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index dc91570c..dfcdfb61 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -88,8 +88,8 @@ public IntakeImpl() { rollerConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast) - .withSupplyCurrentLimitAmps(40.0) + .withNeutralMode(NeutralModeValue.Brake) + .withSupplyCurrentLimitAmps(30.0) .withStatorCurrentLimitEnabled(false) .withRampRate(0.50); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 34812073..c9ecd531 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -265,13 +265,13 @@ public void periodicAfterScheduler() { double setpointVelocityRPS = delta / (360 * Settings.DT); // the component of the turret's setpoint velocity that comes from robot translation - double translationalComponentVelocityRPS = setpointVelocityRPS - omega / (2 * Math.PI); - double translationFF = Gains.Superstructure.Turret.kTranslation.get() * translationalComponentVelocityRPS; + double translationalVelocityRPS = setpointVelocityRPS - omega / (2 * Math.PI); + double translationFF = Gains.Superstructure.Turret.slot0.kV * translationalVelocityRPS; turretMotor.setControl(controller .withPosition(prevActualTargetAngle / 360.0) .withSlot(slot) - .withFeedForward(omegaFF + translationFF) + .withFeedForward(omegaFF /* + translationFF */) ); } } else { From be6640feecbd49af91f1ee2aae99836609fa725e Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 7 Apr 2026 18:13:07 -0400 Subject: [PATCH 13/58] Reapply "FEAT: auto changes, gains changes, sotm in neutral zone changes" This reverts commit 8aa8eab8241ef8e6dfd80e10ca30b0cc3709f8f4. --- elastic-layout.json | 107 ++++++++++++------ simgui-ds.json | 1 + .../paths/Left NZ To Score (B).path | 8 +- .../pathplanner/paths/Left NZ To Score.path | 36 ++++-- .../paths/Left Score To NZ (F).path | 16 +-- .../paths/Left Score To Score.path | 28 +++-- .../pathplanner/paths/Left Trench To NZ.path | 22 ++-- .../paths/Right NZ To Score (B).path | 8 +- .../pathplanner/paths/Right NZ To Score.path | 36 ++++-- .../paths/Right Score To NZ (F).path | 16 +-- .../paths/Right Score To Score.path | 28 +++-- .../pathplanner/paths/Right Trench To NZ.path | 20 ++-- src/main/java/com/stuypulse/robot/Robot.java | 25 ++-- .../commands/auton/regular/LeftTwoCycle.java | 4 +- .../commands/auton/regular/RightTwoCycle.java | 4 +- .../com/stuypulse/robot/constants/Gains.java | 1 + .../robot/subsystems/intake/IntakeImpl.java | 4 +- .../superstructure/turret/TurretImpl.java | 6 +- 18 files changed, 227 insertions(+), 143 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index f475b1d9..1c569174 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -378,7 +378,7 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/FMSUtil/Time Left In Shift", + "topic": "/SmartDashboard/FMSUtil/time left in shift", "period": 0.06, "data_type": "double", "time_display_mode": "Minutes and Seconds", @@ -860,9 +860,8 @@ "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/FMSUtil/Is Active Shift?", + "topic": "/SmartDashboard/is active shift", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -877,7 +876,7 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/FMSUtil/Time Left In Shift", + "topic": "/SmartDashboard/FMSUtil/time left in shift", "period": 0.06, "data_type": "double", "time_display_mode": "Minutes and Seconds", @@ -976,7 +975,7 @@ } }, { - "title": "Current Intake Angle (deg)", + "title": "Current Intake Angle", "x": 910.0, "y": 700.0, "width": 210.0, @@ -1434,20 +1433,6 @@ "show_submit_button": false } }, - { - "title": "Current Intake Angle (deg)", - "x": 420.0, - "y": 560.0, - "width": 210.0, - "height": 140.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Intake/Current Angle (deg)", - "period": 0.5, - "data_type": "double", - "show_submit_button": false - } - }, { "title": "Seed Turret", "x": 980.0, @@ -1476,20 +1461,6 @@ "show_submit_button": false } }, - { - "title": "Current Hood Angle", - "x": 0.0, - "y": 560.0, - "width": 210.0, - "height": 140.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", - "period": 0.5, - "data_type": "double", - "show_submit_button": false - } - }, { "title": "Seed Intake At Lower Limit", "x": 700.0, @@ -1518,6 +1489,48 @@ "maximize_button_space": true } }, + { + "title": "Set Back LL PF", + "x": 0.0, + "y": 700.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Set Back LL PF", + "period": 0.5, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "Set Left LL PF", + "x": 280.0, + "y": 700.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Set Left LL PF", + "period": 0.5, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "Set Right LL PF", + "x": 560.0, + "y": 700.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Set Right LL PF", + "period": 0.5, + "show_type": true, + "maximize_button_space": false + } + }, { "title": "Seed Hood Relative Encoder At Lower Hardstop", "x": 0.0, @@ -1531,6 +1544,34 @@ "show_type": true, "maximize_button_space": false } + }, + { + "title": "Current Intake Angle", + "x": 420.0, + "y": 560.0, + "width": 210.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/Current Angle (deg)", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Current Hood Angle", + "x": 0.0, + "y": 560.0, + "width": 210.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } } ] } diff --git a/simgui-ds.json b/simgui-ds.json index ff81e307..3cddee95 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -99,6 +99,7 @@ } ], "robotJoysticks": [ + {}, { "guid": "Keyboard0" } diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path index fe36b11d..b204f5d0 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.27, - "y": 4.61077032810271 + "x": 8.308815977175463, + "y": 4.74015691868759 }, "prevControl": null, "nextControl": { - "x": 5.515784635743212, - "y": 5.167086622567657 + "x": 5.554600612918676, + "y": 5.296473213152537 }, "isLocked": false, "linkedName": "NZ Left Center" diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 81788efb..9503c764 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.27, - "y": 4.61077032810271 + "x": 8.308815977175463, + "y": 4.74015691868759 }, "prevControl": null, "nextControl": { - "x": 7.791269614835947, - "y": 8.026576319543508 + "x": 4.713277777777778, + "y": 6.188522222222222 }, "isLocked": false, "linkedName": "NZ Left Center" }, { "anchor": { - "x": 3.6120827389443653, - "y": 7.509029957203994 + "x": 3.0815977175463622, + "y": 7.250256776034237 }, "prevControl": { - "x": 7.428987161198288, - "y": 7.5996005706134095 + "x": 10.300853051193904, + "y": 7.3675474243210735 }, "nextControl": null, "isLocked": false, @@ -31,10 +31,24 @@ "rotationTargets": [ { "waypointRelativePos": 0.8063943161634181, - "rotationDegrees": -90.0 + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8372721134368668, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.5, + "unlimited": false + } } ], - "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -47,7 +61,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -90.0 + "rotation": 0.0 }, "reversed": false, "folder": "To Score", diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index ac2add86..cdaccb2e 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.6250213980028523, - "y": 7.483152639087019 + "x": 3.4075222222222226, + "y": 7.572233333333333 }, "prevControl": null, "nextControl": { - "x": 6.833808844507845, - "y": 7.483152639087019 + "x": 6.616309668727215, + "y": 7.572233333333333 }, "isLocked": false, "linkedName": "Left Score (F)" }, { "anchor": { - "x": 8.27, - "y": 4.61077032810271 + "x": 8.308815977175463, + "y": 4.74015691868759 }, "prevControl": { - "x": 7.881840228245363, - "y": 7.185563480741797 + "x": 7.920656205420826, + "y": 7.314950071326677 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index 38f52769..e975816c 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, - "y": 7.509029957203994 + "x": 3.0815977175463622, + "y": 7.250256776034237 }, "prevControl": null, "nextControl": { - "x": 6.92437945791726, - "y": 7.651355206847361 + "x": 6.852697717549599, + "y": 7.376934553817899 }, "isLocked": false, "linkedName": "Left Trench Score" @@ -64,12 +64,12 @@ }, { "anchor": { - "x": 3.6250213980028523, - "y": 7.483152639087019 + "x": 3.4075222222222226, + "y": 7.572233333333333 }, "prevControl": { - "x": 5.281169757489301, - "y": 7.4418502292099244 + "x": 5.06367058170867, + "y": 7.530930923456238 }, "nextControl": null, "isLocked": false, @@ -78,7 +78,15 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.8911495422177071, + "waypointRelativePos": 0.23880597014925345, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9593360995850618, "rotationDegrees": -90.0 }, { @@ -113,7 +121,7 @@ "folder": "To Score", "idealStartingState": { "velocity": 0.0, - "rotation": -90.0 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 1cc656e3..f6b5d239 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.439727164887308, - "y": 7.655468564650059 + "x": 3.998600237247925, + "y": 7.526358244365362 }, "prevControl": null, "nextControl": { - "x": 9.033902728351128, - "y": 7.687746144721235 + "x": 8.99456490727532, + "y": 7.509029957203994 }, "isLocked": false, "linkedName": "Left Trench Start" }, { "anchor": { - "x": 8.27, - "y": 4.61077032810271 + "x": 8.308815977175463, + "y": 4.74015691868759 }, "prevControl": { - "x": 8.218245363766048, - "y": 7.431398002853067 + "x": 8.257061340941512, + "y": 7.560784593437947 }, "nextControl": null, "isLocked": false, @@ -40,8 +40,8 @@ "minWaypointRelativePos": 0.629304523970286, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.75, - "maxAcceleration": 10.0, + "maxVelocity": 1.5, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.5, @@ -67,7 +67,7 @@ "folder": "To NZ", "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": -76.49029381374567 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path index d4fcb968..4f1fc3e5 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.56758915834522, - "y": 3.5498002853067057 + "x": 8.593466476462195, + "y": 3.3557203994293876 }, "prevControl": null, "nextControl": { - "x": 5.639047234674071, - "y": 2.8277371590830254 + "x": 5.664924552791047, + "y": 2.6336572732057073 }, "isLocked": false, "linkedName": "Right Center NZ" diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index f44438d8..69554862 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.56758915834522, - "y": 3.5498002853067057 + "x": 8.593466476462195, + "y": 3.3557203994293876 }, "prevControl": null, "nextControl": { - "x": 7.480741797432238, - "y": 0.3410128388017126 + "x": 5.643452211126961, + "y": 2.8381740370898734 }, "isLocked": false, "linkedName": "Right Center NZ" }, { "anchor": { - "x": 3.5473894436519258, - "y": 0.5350927246790309 + "x": 2.771069900142653, + "y": 0.71623395149786 }, "prevControl": { - "x": 8.03710413694722, - "y": 0.5739087018544944 + "x": 9.926148359486447, + "y": 0.3539514978602004 }, "nextControl": null, "isLocked": false, @@ -31,10 +31,24 @@ "rotationTargets": [ { "waypointRelativePos": 0.7921847246891675, - "rotationDegrees": 90.0 + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8507765023632616, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.5, + "unlimited": false + } } ], - "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -47,7 +61,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 90.0 + "rotation": 0.0 }, "reversed": false, "folder": "To Score", diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 8ddd5af6..226c9169 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.6250213980028523, - "y": 0.5868473609129818 + "x": 3.3880333333333335, + "y": 0.5075111111111108 }, "prevControl": null, "nextControl": { - "x": 7.196091298145506, - "y": 0.5739087018544944 + "x": 6.959103233475987, + "y": 0.49457245205262335 }, "isLocked": false, "linkedName": "Right Score (F)" }, { "anchor": { - "x": 8.56758915834522, - "y": 3.5498002853067057 + "x": 8.593466476462195, + "y": 3.3557203994293876 }, "prevControl": { - "x": 8.515834522111266, - "y": 0.819743223965764 + "x": 8.541711840228242, + "y": 0.625663338088446 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 8adafc11..7c4274b4 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.5473894436519258, - "y": 0.5350927246790309 + "x": 2.771069900142653, + "y": 0.71623395149786 }, "prevControl": null, "nextControl": { - "x": 6.833808844507845, - "y": 0.37982881597717666 + "x": 6.503192122364876, + "y": 0.6967450626089725 }, "isLocked": false, "linkedName": "Right Trench Score" @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 3.6250213980028523, - "y": 0.5868473609129818 + "x": 3.3880333333333335, + "y": 0.5075111111111108 }, "prevControl": { - "x": 6.989072753209699, - "y": 0.4962767475035662 + "x": 6.7520846885401795, + "y": 0.41694049770169517 }, "nextControl": null, "isLocked": false, @@ -62,7 +62,15 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.9722814498933927, + "waypointRelativePos": 0.23027718550106568, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.7344398340248979, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9933609958506272, "rotationDegrees": 90.0 }, { @@ -97,7 +105,7 @@ "folder": "To Score", "idealStartingState": { "velocity": 0, - "rotation": 90.0 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 5f732856..196f1b31 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.439727164887308, - "y": 0.43604982206405685 + "x": 4.039058487874465, + "y": 0.5221540656205423 }, "prevControl": null, "nextControl": { - "x": 9.07693950177936, - "y": 0.4037722419928821 + "x": 9.408601997146931, + "y": 0.44452211126961516 }, "isLocked": false, "linkedName": "Right Trench Start" }, { "anchor": { - "x": 8.56758915834522, - "y": 3.5498002853067057 + "x": 8.593466476462195, + "y": 3.3557203994293876 }, "prevControl": { - "x": 8.50670225385528, - "y": -0.04811387900355957 + "x": 8.774607703281024, + "y": -0.7199572039942939 }, "nextControl": null, "isLocked": false, @@ -40,8 +40,8 @@ "minWaypointRelativePos": 0.7346387575962167, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.75, - "maxAcceleration": 10.0, + "maxVelocity": 1.5, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 558ab41c..49367613 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -22,6 +22,7 @@ import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.stuylib.network.SmartBoolean; +import edu.wpi.first.hal.HAL; import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -36,7 +37,6 @@ import java.lang.management.GarbageCollectorMXBean; import java.lang.management.ManagementFactory; import java.util.List; -import java.util.Timer; import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.commands.FollowPathCommand; @@ -45,7 +45,6 @@ import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.vision.BlackListAllTagsForAllCameras; -import com.stuypulse.robot.commands.vision.BlacklistAllTags; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; public class Robot extends TimedRobot { @@ -56,8 +55,6 @@ public enum RobotMode { TEST } - private Timer threadTimer; - private RobotContainer robot; private Command auto; private static Alliance alliance; @@ -67,7 +64,6 @@ public enum RobotMode { private SendableChooser cameras = new SendableChooser(); private Camera selected; private GcStatsCollector gcStatsCollector; - private SmartBoolean shouldRunSecondThread; private static int periodicCounter = 0; @@ -143,19 +139,12 @@ public void robotPeriodic() { alliance = DriverStation.getAlliance().get(); } - if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && Superstructure.getInstance().getState() == SuperstructureState.SOTM) { - CommandScheduler.getInstance().schedule( - new SuperstructureFOTM(), - new SpindexerStop(), - new HandoffStop() - ); - } - SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime()); SmartDashboard.putData("Robot/Scheduled Commands", CommandScheduler.getInstance()); SmartDashboard.putNumber("Robot/Battery Voltage", batteryVoltage); SmartDashboard.putNumber("Robot/CPU Temperature (C)", RobotController.getCPUTemp()); - + SmartDashboard.putNumber("Robot/Times Disconnected", HAL.getCommsDisableCount()); + robot.periodicAfterScheduler(); energyUtil.periodic(); } @@ -229,6 +218,14 @@ public void teleopPeriodic() { SmartDashboard.putNumber("FMSUtil/time left in shift", fmsUtil.getTimeLeftInShift()); SmartDashboard.putBoolean("FMSUtil/is active shift", fmsUtil.isActiveShift()); SmartDashboard.putBoolean("FMSUtil/won auto?", fmsUtil.didWinAuto()); + + if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && Superstructure.getInstance().getState() == SuperstructureState.SOTM) { + CommandScheduler.getInstance().schedule( + new SuperstructureFOTM(), + new SpindexerStop(), + new HandoffStop() + ); + } } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 21334790..d2ca9149 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -48,7 +48,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(3.5), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 @@ -63,7 +63,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(5.0), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index ab3a445f..2224300c 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -47,7 +47,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(3.5), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 @@ -62,7 +62,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(5.0), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index b0104961..0eb92fb6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -55,6 +55,7 @@ public interface slot1 { } SmartNumber kOmega = new SmartNumber("Superstructure/Turret/Gains/kOmega", 3.43); + SmartNumber kTranslation = new SmartNumber("Superstructure/Turret/Gains/kTranslation", 0.0); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index dfcdfb61..dc91570c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -88,8 +88,8 @@ public IntakeImpl() { rollerConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake) - .withSupplyCurrentLimitAmps(30.0) + .withNeutralMode(NeutralModeValue.Coast) + .withSupplyCurrentLimitAmps(40.0) .withStatorCurrentLimitEnabled(false) .withRampRate(0.50); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index c9ecd531..34812073 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -265,13 +265,13 @@ public void periodicAfterScheduler() { double setpointVelocityRPS = delta / (360 * Settings.DT); // the component of the turret's setpoint velocity that comes from robot translation - double translationalVelocityRPS = setpointVelocityRPS - omega / (2 * Math.PI); - double translationFF = Gains.Superstructure.Turret.slot0.kV * translationalVelocityRPS; + double translationalComponentVelocityRPS = setpointVelocityRPS - omega / (2 * Math.PI); + double translationFF = Gains.Superstructure.Turret.kTranslation.get() * translationalComponentVelocityRPS; turretMotor.setControl(controller .withPosition(prevActualTargetAngle / 360.0) .withSlot(slot) - .withFeedForward(omegaFF /* + translationFF */) + .withFeedForward(omegaFF + translationFF) ); } } else { From c2306b840918541ba68e705becbf0a3ff1a381e0 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 7 Apr 2026 18:13:22 -0400 Subject: [PATCH 14/58] Revert "feat: add automatic pipeline switching when no data" This reverts commit 37f00122ed2f84bd0b283fa0698a771284fc8d50. --- .../subsystems/vision/LimelightVision.java | 36 ------------------- 1 file changed, 36 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 24e3a458..40f684dc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -54,42 +54,11 @@ public static LimelightVision getInstance() { private boolean hasData; private BStream debouncedHasData; - private Pipeline currentPipeline; - public enum MegaTagMode { MEGATAG1, MEGATAG2 } - public enum Pipeline { - NO_SUN, - LOW_SUN, - MED_SUN, - HIGH_SUN - } - - private int getCurrentPipelineID() { - return switch(this.currentPipeline) { - case NO_SUN -> 0; - case LOW_SUN -> 1; - case MED_SUN -> 2; - case HIGH_SUN -> 3; - }; - } - - private void iteratePipelineValue() { - this.currentPipeline = switch(this.currentPipeline) { - case NO_SUN -> Pipeline.LOW_SUN; - case LOW_SUN -> Pipeline.MED_SUN; - case MED_SUN -> Pipeline.HIGH_SUN; - case HIGH_SUN -> Pipeline.NO_SUN; - }; - } - - public void setPipeline(String cameraName, Pipeline pipeline) { - this.currentPipeline = pipeline; - } - public LimelightVision() { limelightPoseArray = new Pose2d[Cameras.LimelightCameras.length]; leftLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Left", Pose2d.struct).publish(); @@ -332,11 +301,6 @@ public void periodicAfterScheduler() { } SmartDashboard.putBoolean("Vision/Has Data", hasData); - - for(String cameraName: names) { - iteratePipelineValue(); - LimelightHelpers.setPipelineIndex(cameraName, getCurrentPipelineID()); - } } } } From 990d03c426fb0da5e7fde6c9e52610f2cf386285 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 7 Apr 2026 19:27:08 -0400 Subject: [PATCH 15/58] feat: auton changes + intake roller limits down to 37A each --- .../pathplanner/paths/Left NZ To Score.path | 12 ++++++------ .../pathplanner/paths/Left Score To NZ (F).path | 8 ++++---- .../pathplanner/paths/Left Score To Score.path | 16 ++++++++-------- .../pathplanner/paths/Left Trench To NZ.path | 2 +- .../pathplanner/paths/Right NZ To Score (B).path | 8 ++++---- .../pathplanner/paths/Right NZ To Score.path | 16 ++++++++-------- .../pathplanner/paths/Right Score To NZ (F).path | 16 ++++++++-------- .../pathplanner/paths/Right Score To Score.path | 16 ++++++++-------- .../pathplanner/paths/Right Trench To NZ.path | 8 ++++---- .../robot/subsystems/intake/IntakeImpl.java | 2 +- 10 files changed, 52 insertions(+), 52 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 9503c764..1ab2ab67 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.713277777777778, - "y": 6.188522222222222 + "x": 4.582482168330956, + "y": 5.89169757489301 }, "isLocked": false, "linkedName": "NZ Left Center" }, { "anchor": { - "x": 3.0815977175463622, - "y": 7.250256776034237 + "x": 3.09453637660485, + "y": 7.366704707560627 }, "prevControl": { - "x": 10.300853051193904, - "y": 7.3675474243210735 + "x": 9.964964336661911, + "y": 7.819557774607704 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index cdaccb2e..adefa99d 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.4075222222222226, - "y": 7.572233333333333 + "x": 3.09453637660485, + "y": 7.366704707560627 }, "prevControl": null, "nextControl": { - "x": 6.616309668727215, - "y": 7.572233333333333 + "x": 6.303323823109842, + "y": 7.366704707560627 }, "isLocked": false, "linkedName": "Left Score (F)" diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index e975816c..9607da36 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.0815977175463622, - "y": 7.250256776034237 + "x": 3.09453637660485, + "y": 7.366704707560627 }, "prevControl": null, "nextControl": { - "x": 6.852697717549599, - "y": 7.376934553817899 + "x": 6.865636376608087, + "y": 7.49338248534429 }, "isLocked": false, "linkedName": "Left Trench Score" @@ -64,12 +64,12 @@ }, { "anchor": { - "x": 3.4075222222222226, - "y": 7.572233333333333 + "x": 3.09453637660485, + "y": 7.366704707560627 }, "prevControl": { - "x": 5.06367058170867, - "y": 7.530930923456238 + "x": 4.750684736091299, + "y": 7.3254022976835325 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index f6b5d239..80fa6da2 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -67,7 +67,7 @@ "folder": "To NZ", "idealStartingState": { "velocity": 0, - "rotation": -76.49029381374567 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path index 4f1fc3e5..2a6727b0 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.593466476462195, - "y": 3.3557203994293876 + "x": 8.231184022824536, + "y": 3.3427817403708993 }, "prevControl": null, "nextControl": { - "x": 5.664924552791047, - "y": 2.6336572732057073 + "x": 5.302642099153387, + "y": 2.620718614147219 }, "isLocked": false, "linkedName": "Right Center NZ" diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 69554862..af348df6 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.593466476462195, - "y": 3.3557203994293876 + "x": 8.231184022824536, + "y": 3.3427817403708993 }, "prevControl": null, "nextControl": { - "x": 5.643452211126961, - "y": 2.8381740370898734 + "x": 4.996519258202568, + "y": 2.4111982881597713 }, "isLocked": false, "linkedName": "Right Center NZ" }, { "anchor": { - "x": 2.771069900142653, - "y": 0.71623395149786 + "x": 2.978088445078458, + "y": 0.6386019971469341 }, "prevControl": { - "x": 9.926148359486447, - "y": 0.3539514978602004 + "x": 9.162767475035661, + "y": 0.06930099857346694 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 226c9169..c0b3de28 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.3880333333333335, - "y": 0.5075111111111108 + "x": 2.978088445078458, + "y": 0.6386019971469341 }, "prevControl": null, "nextControl": { - "x": 6.959103233475987, - "y": 0.49457245205262335 + "x": 6.549158345221111, + "y": 0.6256633380884467 }, "isLocked": false, "linkedName": "Right Score (F)" }, { "anchor": { - "x": 8.593466476462195, - "y": 3.3557203994293876 + "x": 8.231184022824536, + "y": 3.3427817403708993 }, "prevControl": { - "x": 8.541711840228242, - "y": 0.625663338088446 + "x": 8.179429386590582, + "y": 0.6127246790299576 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 7c4274b4..ce062cad 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.771069900142653, - "y": 0.71623395149786 + "x": 2.978088445078458, + "y": 0.6386019971469341 }, "prevControl": null, "nextControl": { - "x": 6.503192122364876, - "y": 0.6967450626089725 + "x": 7.2090299572039935, + "y": 0.6127246790299581 }, "isLocked": false, "linkedName": "Right Trench Score" @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 3.3880333333333335, - "y": 0.5075111111111108 + "x": 2.978088445078458, + "y": 0.6386019971469341 }, "prevControl": { - "x": 6.7520846885401795, - "y": 0.41694049770169517 + "x": 6.794992867332382, + "y": 0.5480313837375186 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 196f1b31..73b3ceba 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.593466476462195, - "y": 3.3557203994293876 + "x": 8.231184022824536, + "y": 3.3427817403708993 }, "prevControl": { - "x": 8.774607703281024, - "y": -0.7199572039942939 + "x": 8.412325249643365, + "y": -0.7328958630527822 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index dc91570c..a3758f94 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -89,7 +89,7 @@ public IntakeImpl() { rollerConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast) - .withSupplyCurrentLimitAmps(40.0) + .withSupplyCurrentLimitAmps(37.0) .withStatorCurrentLimitEnabled(false) .withRampRate(0.50); From 9714ef9f30083157ec718c1be9ac0ac128cbd519 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Tue, 7 Apr 2026 17:58:54 -0400 Subject: [PATCH 16/58] feat: add automatic pipeline switching when no data --- .../subsystems/vision/LimelightVision.java | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 40f684dc..24e3a458 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -54,11 +54,42 @@ public static LimelightVision getInstance() { private boolean hasData; private BStream debouncedHasData; + private Pipeline currentPipeline; + public enum MegaTagMode { MEGATAG1, MEGATAG2 } + public enum Pipeline { + NO_SUN, + LOW_SUN, + MED_SUN, + HIGH_SUN + } + + private int getCurrentPipelineID() { + return switch(this.currentPipeline) { + case NO_SUN -> 0; + case LOW_SUN -> 1; + case MED_SUN -> 2; + case HIGH_SUN -> 3; + }; + } + + private void iteratePipelineValue() { + this.currentPipeline = switch(this.currentPipeline) { + case NO_SUN -> Pipeline.LOW_SUN; + case LOW_SUN -> Pipeline.MED_SUN; + case MED_SUN -> Pipeline.HIGH_SUN; + case HIGH_SUN -> Pipeline.NO_SUN; + }; + } + + public void setPipeline(String cameraName, Pipeline pipeline) { + this.currentPipeline = pipeline; + } + public LimelightVision() { limelightPoseArray = new Pose2d[Cameras.LimelightCameras.length]; leftLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Left", Pose2d.struct).publish(); @@ -301,6 +332,11 @@ public void periodicAfterScheduler() { } SmartDashboard.putBoolean("Vision/Has Data", hasData); + + for(String cameraName: names) { + iteratePipelineValue(); + LimelightHelpers.setPipelineIndex(cameraName, getCurrentPipelineID()); + } } } } From 819d4e6b4064a1aadc6c975cad5b2e334a17e545 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 7 Apr 2026 19:46:51 -0400 Subject: [PATCH 17/58] feat: pipeline buttons + brownout voltage down to 6.3 --- src/main/java/com/stuypulse/robot/Robot.java | 1 + .../com/stuypulse/robot/RobotContainer.java | 7 ++++ .../robot/commands/vision/SetPipeline.java | 32 +++++++++++++++++++ .../subsystems/vision/LimelightVision.java | 21 +++--------- 4 files changed, 45 insertions(+), 16 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/vision/SetPipeline.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 49367613..f23828f5 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -103,6 +103,7 @@ public void robotInit() { energyUtil = new EnergyUtil(); CommandScheduler.getInstance().schedule(new SwerveAutonInit()); + RobotController.setBrownoutVoltage(6.3); } @Override diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index ba80c884..fdfb297f 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -53,6 +53,7 @@ import com.stuypulse.robot.commands.vision.ResetLimelightIMU; import com.stuypulse.robot.commands.vision.SetIMUMode; import com.stuypulse.robot.commands.vision.SetMegaTagMode; +import com.stuypulse.robot.commands.vision.SetPipeline; import com.stuypulse.robot.commands.vision.WhitelistAllTagsForAllCameras; import com.stuypulse.robot.commands.vision.WhitelistOutpostTags; import com.stuypulse.robot.commands.vision.WhitelistTowerTags; @@ -74,6 +75,7 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; +import com.stuypulse.robot.subsystems.vision.LimelightVision.Pipeline; import com.stuypulse.robot.util.PathUtil.AutonConfig; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; @@ -327,6 +329,11 @@ private void configureElasticButtons() { SmartDashboard.putData("Robot/WL Tower Tags Right-Camera", new WhitelistTowerTags("limelight-right")); SmartDashboard.putData("Robot/Whitelist All Cameras", new WhitelistAllTagsForAllCameras()); + SmartDashboard.putData("Robot/Set Exposure 25", new SetPipeline(Pipeline.NO_SUN)); + SmartDashboard.putData("Robot/Set Exposure 80", new SetPipeline(Pipeline.LOW_SUN)); + SmartDashboard.putData("Robot/Set Exposure 100", new SetPipeline(Pipeline.MED_SUN)); + SmartDashboard.putData("Robot/Set Exposure 130", new SetPipeline(Pipeline.HIGH_SUN)); + // Unjamming SmartDashboard.putData("Robot/Handoff Reverse", new ConditionalCommand( diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetPipeline.java b/src/main/java/com/stuypulse/robot/commands/vision/SetPipeline.java new file mode 100644 index 00000000..cf82ff98 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetPipeline.java @@ -0,0 +1,32 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.vision; + +import com.stuypulse.robot.subsystems.vision.LimelightVision; +import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; +import com.stuypulse.robot.subsystems.vision.LimelightVision.Pipeline; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SetPipeline extends InstantCommand { + private LimelightVision vision; + private Pipeline pipeline; + + public SetPipeline(Pipeline pipeline) { + this.vision = LimelightVision.getInstance(); + this.pipeline = pipeline; + } + + @Override + public void initialize() { + vision.setPipeline(pipeline); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 24e3a458..71a179fe 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -77,17 +77,11 @@ private int getCurrentPipelineID() { }; } - private void iteratePipelineValue() { - this.currentPipeline = switch(this.currentPipeline) { - case NO_SUN -> Pipeline.LOW_SUN; - case LOW_SUN -> Pipeline.MED_SUN; - case MED_SUN -> Pipeline.HIGH_SUN; - case HIGH_SUN -> Pipeline.NO_SUN; - }; - } - - public void setPipeline(String cameraName, Pipeline pipeline) { - this.currentPipeline = pipeline; + public void setPipeline(Pipeline pipeline) { + for(String camName: names) { + this.currentPipeline = pipeline; + LimelightHelpers.setPipelineIndex(camName, getCurrentPipelineID()); + } } public LimelightVision() { @@ -332,11 +326,6 @@ public void periodicAfterScheduler() { } SmartDashboard.putBoolean("Vision/Has Data", hasData); - - for(String cameraName: names) { - iteratePipelineValue(); - LimelightHelpers.setPipelineIndex(cameraName, getCurrentPipelineID()); - } } } } From 711dd270c6e914156e58e69c9b19805665c7d506 Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Wed, 8 Apr 2026 01:32:49 -0400 Subject: [PATCH 18/58] debug: (perf) decrease logging frequency --- src/main/java/com/stuypulse/robot/constants/Settings.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 8a5b7eb3..fb2bea18 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,9 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; - import com.ctre.phoenix6.CANBus; import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.stuylib.network.SmartBoolean; @@ -21,6 +18,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.util.Color; @@ -33,7 +32,7 @@ public interface Settings { public final double DT = 0.020; - public final int LOGGING_FREQUENCY = 2; + public final int LOGGING_FREQUENCY = 5; public final double SECONDS_IN_A_MINUTE = 60.0; public final SmartBoolean DEBUG_MODE = new SmartBoolean("Robot/DebugMode", true); public final CANBus CANIVORE = new CANBus("canivore", "./logs/example.hoot"); From 8c6656d934ff81fa81570248c74574b5d4970f4b Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Wed, 8 Apr 2026 01:38:44 -0400 Subject: [PATCH 19/58] URGENT DEBUG: wasting instructions on meaningless checks --- .../superstructure/shooter/ShooterImpl.java | 39 +++++-------------- 1 file changed, 9 insertions(+), 30 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 7b58c180..65f96fe5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -5,6 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.shooter; +import java.util.Optional; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; @@ -22,16 +31,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.MotorAlignmentValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import java.util.Optional; - public class ShooterImpl extends Shooter { private final Motors.TalonFXConfig shooterConfig; @@ -129,26 +128,6 @@ public double getBangBangOutput(double mesurement, double setpoint) { public void periodicAfterScheduler() { super.periodicAfterScheduler(); - shooterConfig.updateGainsConfig( - shooterLeader, - 0, - Gains.Superstructure.Shooter.kP, - Gains.Superstructure.Shooter.kI, - Gains.Superstructure.Shooter.kD, - Gains.Superstructure.Shooter.kS, - Gains.Superstructure.Shooter.kV, - Gains.Superstructure.Shooter.kA); - - shooterConfig.updateGainsConfig( - shooterFollower, - 0, - Gains.Superstructure.Shooter.kP, - Gains.Superstructure.Shooter.kI, - Gains.Superstructure.Shooter.kD, - Gains.Superstructure.Shooter.kS, - Gains.Superstructure.Shooter.kV, - Gains.Superstructure.Shooter.kA); - if (EnabledSubsystems.SHOOTER.get() || getState() == ShooterState.STOP) { if (voltageOverride.isPresent()) { shooterLeader.setVoltage(voltageOverride.get()); From 7eec10f6a4e6309ab62dbabf0ef67a145b90a4a2 Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Wed, 8 Apr 2026 01:39:20 -0400 Subject: [PATCH 20/58] URGENT DEBUG: kill all the port forwarding that is likely causing issues --- .../com/stuypulse/robot/RobotContainer.java | 20 ++-------- .../commands/vision/EnableBackLimelight.java | 12 +++--- .../commands/vision/EnableLeftLimelight.java | 12 +++--- .../commands/vision/EnablePortForwarding.java | 40 +++++++++---------- .../commands/vision/EnableRightLimelight.java | 12 +++--- 5 files changed, 41 insertions(+), 55 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index fdfb297f..67bd8f11 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -27,29 +27,22 @@ import com.stuypulse.robot.commands.intake.SeedPivotDeployed; import com.stuypulse.robot.commands.intake.SeedPivotStowed; import com.stuypulse.robot.commands.leds.LEDApplyPattern; -import com.stuypulse.robot.commands.leds.LEDDefaultCommand; import com.stuypulse.robot.commands.spindexer.SpindexerReverse; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; -import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; import com.stuypulse.robot.commands.superstructure.SuperstructureLeftCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureRightCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.superstructure.SuperstructureManualOverride; import com.stuypulse.robot.commands.superstructure.SuperstructureStow; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveFOTM; import com.stuypulse.robot.commands.swerve.SwerveDriveSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.commands.swerve.pidToPose.SwerveDrivePIDToPose; import com.stuypulse.robot.commands.turret.SeedTurret; import com.stuypulse.robot.commands.turret.ZeroTurret; -import com.stuypulse.robot.commands.vision.EnableBackLimelight; -import com.stuypulse.robot.commands.vision.EnableLeftLimelight; -import com.stuypulse.robot.commands.vision.EnableRightLimelight; import com.stuypulse.robot.commands.vision.ResetLimelightIMU; import com.stuypulse.robot.commands.vision.SetIMUMode; import com.stuypulse.robot.commands.vision.SetMegaTagMode; @@ -64,7 +57,6 @@ import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.intake.Intake.RollerState; -import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -81,20 +73,14 @@ import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import com.stuypulse.stuylib.network.SmartBoolean; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RepeatCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; public class RobotContainer { @@ -321,9 +307,9 @@ private void configureElasticButtons() { SmartDashboard.putData("Robot/Set Megatag 1", new SetMegaTagMode(MegaTagMode.MEGATAG1)); SmartDashboard.putData("Robot/Set Megatag 2", new SetMegaTagMode(MegaTagMode.MEGATAG2)); - SmartDashboard.putData("Robot/Set Left LL PF", new EnableLeftLimelight()); - SmartDashboard.putData("Robot/Set Right LL PF", new EnableRightLimelight()); - SmartDashboard.putData("Robot/Set Back LL PF", new EnableBackLimelight()); + // SmartDashboard.putData("Robot/Set Left LL PF", new EnableLeftLimelight()); + // SmartDashboard.putData("Robot/Set Right LL PF", new EnableRightLimelight()); + // SmartDashboard.putData("Robot/Set Back LL PF", new EnableBackLimelight()); SmartDashboard.putData("Robot/WL Outpost Tags Left-Camera", new WhitelistOutpostTags("limelight-left")); SmartDashboard.putData("Robot/WL Tower Tags Right-Camera", new WhitelistTowerTags("limelight-right")); diff --git a/src/main/java/com/stuypulse/robot/commands/vision/EnableBackLimelight.java b/src/main/java/com/stuypulse/robot/commands/vision/EnableBackLimelight.java index 02a8a163..fd33ab7d 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/EnableBackLimelight.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/EnableBackLimelight.java @@ -1,9 +1,9 @@ -package com.stuypulse.robot.commands.vision; +// package com.stuypulse.robot.commands.vision; -public class EnableBackLimelight extends EnablePortForwarding { - public EnableBackLimelight() { - super("limelight-back.local"); - } -} +// public class EnableBackLimelight extends EnablePortForwarding { +// public EnableBackLimelight() { +// super("limelight-back.local"); +// } +// } diff --git a/src/main/java/com/stuypulse/robot/commands/vision/EnableLeftLimelight.java b/src/main/java/com/stuypulse/robot/commands/vision/EnableLeftLimelight.java index 0a0567d9..5aa48164 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/EnableLeftLimelight.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/EnableLeftLimelight.java @@ -1,7 +1,7 @@ -package com.stuypulse.robot.commands.vision; +// package com.stuypulse.robot.commands.vision; -public class EnableLeftLimelight extends EnablePortForwarding { - public EnableLeftLimelight() { - super("limelight-left.local"); - } -} +// public class EnableLeftLimelight extends EnablePortForwarding { +// public EnableLeftLimelight() { +// super("limelight-left.local"); +// } +// } diff --git a/src/main/java/com/stuypulse/robot/commands/vision/EnablePortForwarding.java b/src/main/java/com/stuypulse/robot/commands/vision/EnablePortForwarding.java index ca4bc3d0..78dac7ac 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/EnablePortForwarding.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/EnablePortForwarding.java @@ -1,25 +1,25 @@ -package com.stuypulse.robot.commands.vision; +// package com.stuypulse.robot.commands.vision; -import edu.wpi.first.net.PortForwarder; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.net.PortForwarder; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.InstantCommand; -public class EnablePortForwarding extends InstantCommand { - private String hostname; +// public class EnablePortForwarding extends InstantCommand { +// private String hostname; - public EnablePortForwarding(String hostname) { - this.hostname = hostname; - } +// public EnablePortForwarding(String hostname) { +// this.hostname = hostname; +// } - @Override - public void initialize() { - PortForwarder.remove(10000); - PortForwarder.add(10000, hostname, 5801); - SmartDashboard.putString("Limelight/Limelight Being Port Forwarded", hostname); - } +// @Override +// public void initialize() { +// PortForwarder.remove(10000); +// PortForwarder.add(10000, hostname, 5801); +// SmartDashboard.putString("Limelight/Limelight Being Port Forwarded", hostname); +// } - @Override - public boolean runsWhenDisabled() { - return true; - } -} +// @Override +// public boolean runsWhenDisabled() { +// return true; +// } +// } diff --git a/src/main/java/com/stuypulse/robot/commands/vision/EnableRightLimelight.java b/src/main/java/com/stuypulse/robot/commands/vision/EnableRightLimelight.java index f14bd871..418be5df 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/EnableRightLimelight.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/EnableRightLimelight.java @@ -1,7 +1,7 @@ -package com.stuypulse.robot.commands.vision; +// package com.stuypulse.robot.commands.vision; -public class EnableRightLimelight extends EnablePortForwarding { - public EnableRightLimelight() { - super("limelight-right.local"); - } -} +// public class EnableRightLimelight extends EnablePortForwarding { +// public EnableRightLimelight() { +// super("limelight-right.local"); +// } +// } From cdbbf7ae2069f519454446da0f51c17759b82def Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Wed, 8 Apr 2026 01:42:12 -0400 Subject: [PATCH 21/58] debug: reduce canbus utilization on rio --- .../subsystems/superstructure/shooter/ShooterImpl.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 65f96fe5..9bb84fcc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -71,15 +71,16 @@ public ShooterImpl() { .withSupplyCurrentLimitEnabled(true) .withLowerLimitSupplyCurrent(60, 1); + // RIO CANBUS UTILIZATION AT 100 PERCENT, DROPPED UPDATE HZ FOR VELOCITY AND TORQUECURRENT TO 500 FROM 1000 shooterLeader = new TalonFX(Ports.Superstructure.Shooter.MOTOR_LEAD, Ports.RIO); - shooterLeader.getVelocity().setUpdateFrequency(1000.0); - shooterLeader.getTorqueCurrent().setUpdateFrequency(1000.0); + shooterLeader.getVelocity().setUpdateFrequency(500.0); + shooterLeader.getTorqueCurrent().setUpdateFrequency(500.0); shooterLeader.getStatorCurrent().setUpdateFrequency(50.0); shooterLeader.getSupplyCurrent().setUpdateFrequency(50.0); shooterFollower = new TalonFX(Ports.Superstructure.Shooter.MOTOR_FOLLOW, Ports.RIO); - shooterFollower.getVelocity().setUpdateFrequency(1000.0); - shooterFollower.getTorqueCurrent().setUpdateFrequency(1000.0); + shooterFollower.getVelocity().setUpdateFrequency(500.0); + shooterFollower.getTorqueCurrent().setUpdateFrequency(500.0); shooterFollower.getStatorCurrent().setUpdateFrequency(50.0); shooterFollower.getSupplyCurrent().setUpdateFrequency(50.0); From b6778bff518a3377368fd5d43aaa6c72b53c7389 Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Wed, 8 Apr 2026 01:51:11 -0400 Subject: [PATCH 22/58] debug: (swerve) add logging of failed data aqusitions --- .../subsystems/swerve/CommandSwerveDrivetrain.java | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 8e97bd69..3527340b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -5,11 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.swerve; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; - -import java.sql.Struct; - import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -48,9 +43,10 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; @@ -669,6 +665,9 @@ public void periodicAfterScheduler() { Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { + SmartDashboard.putNumber("Swerve/Failed DAQ Count", this.getState().FailedDaqs); + // will confirm whether we are even getting data + SmartDashboard.putBoolean("FieldPositions/isBehindTower", isBehindTower()); SmartDashboard.putBoolean("FieldPositions/isUnderTrench", isUnderTrench()); SmartDashboard.putBoolean("FieldPositions/isBehindHub", isBehindHub()); From 8eb3996484f7451dfc3dff73f631bf5decd864da Mon Sep 17 00:00:00 2001 From: dfn-slxxp Date: Wed, 8 Apr 2026 03:10:10 -0400 Subject: [PATCH 23/58] debug: further kill port forwarding, lower turret closed loop update freq --- src/main/java/com/stuypulse/robot/Robot.java | 10 +++++----- .../subsystems/superstructure/shooter/ShooterImpl.java | 4 ++-- .../subsystems/superstructure/turret/TurretImpl.java | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index f23828f5..8c0e1f16 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -114,11 +114,11 @@ public void robotPeriodic() { if (periodicCounter % 50 == 0) { DataLogManager.getLog().resume(); } - if (cameras.getSelected() != selected) { - PortForwarder.remove(5801); - selected = cameras.getSelected(); - PortForwarder.add(5801, selected + ".local:5801", 5801); - } + // if (cameras.getSelected() != selected) { + // PortForwarder.remove(5801); + // selected = cameras.getSelected(); + // PortForwarder.add(5801, selected + ".local:5801", 5801); + // } periodicCounter++; double batteryVoltage = RobotController.getBatteryVoltage(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 9bb84fcc..c2b94f8e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -73,13 +73,13 @@ public ShooterImpl() { // RIO CANBUS UTILIZATION AT 100 PERCENT, DROPPED UPDATE HZ FOR VELOCITY AND TORQUECURRENT TO 500 FROM 1000 shooterLeader = new TalonFX(Ports.Superstructure.Shooter.MOTOR_LEAD, Ports.RIO); - shooterLeader.getVelocity().setUpdateFrequency(500.0); + shooterLeader.getVelocity().setUpdateFrequency(500.0); //TODO: Go lower shooterLeader.getTorqueCurrent().setUpdateFrequency(500.0); shooterLeader.getStatorCurrent().setUpdateFrequency(50.0); shooterLeader.getSupplyCurrent().setUpdateFrequency(50.0); shooterFollower = new TalonFX(Ports.Superstructure.Shooter.MOTOR_FOLLOW, Ports.RIO); - shooterFollower.getVelocity().setUpdateFrequency(500.0); + shooterFollower.getVelocity().setUpdateFrequency(500.0); //TODO: Go lower shooterFollower.getTorqueCurrent().setUpdateFrequency(500.0); shooterFollower.getStatorCurrent().setUpdateFrequency(50.0); shooterFollower.getSupplyCurrent().setUpdateFrequency(50.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 34812073..895b6702 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -123,7 +123,7 @@ public TurretImpl() { controller = new PositionVoltage(getTargetAngle().getRotations()).withEnableFOC(true); - turretMotor.getClosedLoopError().setUpdateFrequency(1000.0); + turretMotor.getClosedLoopError().setUpdateFrequency(500.0); //TODO: Go lower encoder18tPos = encoder18t.getAbsolutePosition(); encoder17tPos = encoder17t.getAbsolutePosition(); From 0ce72d9d3269899c7d20ca9e46bb3a205e98cba7 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Wed, 8 Apr 2026 05:42:45 -0400 Subject: [PATCH 24/58] fix: put hood is connected on the right chain --- .../robot/subsystems/superstructure/hood/HoodImpl.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 25deb31f..270cbb67 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -182,7 +182,7 @@ public void periodicAfterScheduler() { // SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); - SmartDashboard.putBoolean("Prematch Checks/Hood at Top?", getAngle().getDegrees() > 39.0); + SmartDashboard.putBoolean("Prematch Checks/Hood at Bottom?", getAngle().getDegrees() < Settings.Superstructure.Hood.REVERSE_SOFT_LIMIT.getDegrees()); SmartDashboard.putNumber("Superstructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); SmartDashboard.putNumber("Superstructure/Hood/Closed Loop Error (deg)", hoodMotorClosedLoopError.getValueAsDouble() * 360.0); SmartDashboard.putNumber("Superstructure/Hood/Implemented Error (Degrees)", getTargetAngle().getDegrees() - getAngle().getDegrees()); @@ -195,10 +195,9 @@ public void periodicAfterScheduler() { SmartDashboard.putBoolean("Superstructure/Hood/is stalling", isStalling()); Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); - if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { - SmartDashboard.putBoolean("Robot/CAN/Canivore/Hood Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Hood.MOTOR) + ")", hoodMotor.isConnected()); - // SmartDashboard.putBoolean("Robot/CAN/Canivore/Hood Encoder Connected? (ID " + String.valueOf(hoodEncoder.getDeviceID()) + ")", hoodEncoder.isConnected()); + SmartDashboard.putBoolean("Robot/CAN/Rio/Hood Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Hood.MOTOR) + ")", hoodMotor.isConnected()); + // SmartDashboard.putBoolean("Robot/CAN/Rio/Hood Encoder Connected? (ID " + String.valueOf(hoodEncoder.getDeviceID()) + ")", hoodEncoder.isConnected()); } } } From 04b3a0233ce2dfeae332a1ca3a4725d528160994 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Wed, 8 Apr 2026 07:17:01 -0400 Subject: [PATCH 25/58] feat: pose buffer --- .../java/com/stuypulse/robot/constants/Settings.java | 1 + .../subsystems/swerve/CommandSwerveDrivetrain.java | 11 ++++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index fb2bea18..9666faa4 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -288,6 +288,7 @@ public interface SOTM { public interface Swerve { public final double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; public final double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; + double ACCEPTABLE_POSE_DELTA_METERS = Math.sqrt(Field.LENGTH * Field.LENGTH + Field.WIDTH * Field.WIDTH); //TODO: Might wanna make this smaller. public interface Constraints { public final double MAX_VELOCITY_M_PER_S = 4.16; diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 8e97bd69..31f70cc7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -88,6 +88,8 @@ public static CommandSwerveDrivetrain getInstance() { private Notifier m_simNotifier = null; private double m_lastSimTime; + private Pose2d lastGoodPose = Pose2d.kZero; + /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_moduleTranslationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); @@ -390,7 +392,14 @@ public void addVisionMeasurement( } public Pose2d getPose() { - return getState().Pose; + double proposedX = getState().Pose.getX(); + double proposedY = getState().Pose.getY(); + double poseDelta = lastGoodPose.getTranslation().getDistance(getState().Pose.getTranslation()); + if(!(proposedX > Field.LENGTH || proposedX < 0 || proposedY > Field.WIDTH || proposedY < 0) && + poseDelta < Settings.Swerve.ACCEPTABLE_POSE_DELTA_METERS) { + lastGoodPose = getState().Pose; + } + return lastGoodPose; } public void configureAutoBuilder() { From 51d5c899c4fc1b1147206593f288d3450ab47ab5 Mon Sep 17 00:00:00 2001 From: dfn-slxxp Date: Wed, 8 Apr 2026 08:05:45 -0400 Subject: [PATCH 26/58] debug: lower signals to 50hz except turret pos and shooter vel (those to 200) --- .../subsystems/superstructure/shooter/ShooterImpl.java | 8 ++++---- .../subsystems/superstructure/turret/TurretImpl.java | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index c2b94f8e..8ba3d8e0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -73,14 +73,14 @@ public ShooterImpl() { // RIO CANBUS UTILIZATION AT 100 PERCENT, DROPPED UPDATE HZ FOR VELOCITY AND TORQUECURRENT TO 500 FROM 1000 shooterLeader = new TalonFX(Ports.Superstructure.Shooter.MOTOR_LEAD, Ports.RIO); - shooterLeader.getVelocity().setUpdateFrequency(500.0); //TODO: Go lower - shooterLeader.getTorqueCurrent().setUpdateFrequency(500.0); + shooterLeader.getVelocity().setUpdateFrequency(200.0); //TODO: Go lower + shooterLeader.getTorqueCurrent().setUpdateFrequency(50.0); shooterLeader.getStatorCurrent().setUpdateFrequency(50.0); shooterLeader.getSupplyCurrent().setUpdateFrequency(50.0); shooterFollower = new TalonFX(Ports.Superstructure.Shooter.MOTOR_FOLLOW, Ports.RIO); - shooterFollower.getVelocity().setUpdateFrequency(500.0); //TODO: Go lower - shooterFollower.getTorqueCurrent().setUpdateFrequency(500.0); + shooterFollower.getVelocity().setUpdateFrequency(200.0); //TODO: Go lower + shooterFollower.getTorqueCurrent().setUpdateFrequency(50.0); shooterFollower.getStatorCurrent().setUpdateFrequency(50.0); shooterFollower.getSupplyCurrent().setUpdateFrequency(50.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 895b6702..9395c4d3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -123,7 +123,7 @@ public TurretImpl() { controller = new PositionVoltage(getTargetAngle().getRotations()).withEnableFOC(true); - turretMotor.getClosedLoopError().setUpdateFrequency(500.0); //TODO: Go lower + turretMotor.getClosedLoopError().setUpdateFrequency(200.0); //TODO: Go lower encoder18tPos = encoder18t.getAbsolutePosition(); encoder17tPos = encoder17t.getAbsolutePosition(); From 237d84dd3fbe71b265d1297f4a792c1be4b0ff33 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 8 Apr 2026 08:14:51 -0400 Subject: [PATCH 27/58] fix: TorqueCurrent signal up to 500 bc follower consumes the leader signal --- .../robot/subsystems/intake/IntakeImpl.java | 18 +++++++++--------- .../superstructure/shooter/ShooterImpl.java | 7 +++---- .../superstructure/turret/TurretImpl.java | 18 +++++++++--------- 3 files changed, 21 insertions(+), 22 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index a3758f94..6e84cf98 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -167,15 +167,15 @@ public void periodicAfterScheduler() { PivotState pivotState = getPivotState(); RollerState rollerState = getRollerState(); - pivotConfig.updateGainsConfig( - pivot, - 0, - Gains.Intake.Pivot.kP, - Gains.Intake.Pivot.kI, - Gains.Intake.Pivot.kD, - Gains.Intake.Pivot.kS, - Gains.Intake.Pivot.kV, - Gains.Intake.Pivot.kA); + // pivotConfig.updateGainsConfig( + // pivot, + // 0, + // Gains.Intake.Pivot.kP, + // Gains.Intake.Pivot.kI, + // Gains.Intake.Pivot.kD, + // Gains.Intake.Pivot.kS, + // Gains.Intake.Pivot.kV, + // Gains.Intake.Pivot.kA); boolean applyingPushdownVoltage = false; if (EnabledSubsystems.INTAKE.get()) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 8ba3d8e0..5cce22f6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -71,15 +71,14 @@ public ShooterImpl() { .withSupplyCurrentLimitEnabled(true) .withLowerLimitSupplyCurrent(60, 1); - // RIO CANBUS UTILIZATION AT 100 PERCENT, DROPPED UPDATE HZ FOR VELOCITY AND TORQUECURRENT TO 500 FROM 1000 shooterLeader = new TalonFX(Ports.Superstructure.Shooter.MOTOR_LEAD, Ports.RIO); - shooterLeader.getVelocity().setUpdateFrequency(200.0); //TODO: Go lower - shooterLeader.getTorqueCurrent().setUpdateFrequency(50.0); + shooterLeader.getVelocity().setUpdateFrequency(200.0); + shooterLeader.getTorqueCurrent().setUpdateFrequency(500.0); shooterLeader.getStatorCurrent().setUpdateFrequency(50.0); shooterLeader.getSupplyCurrent().setUpdateFrequency(50.0); shooterFollower = new TalonFX(Ports.Superstructure.Shooter.MOTOR_FOLLOW, Ports.RIO); - shooterFollower.getVelocity().setUpdateFrequency(200.0); //TODO: Go lower + shooterFollower.getVelocity().setUpdateFrequency(200.0); shooterFollower.getTorqueCurrent().setUpdateFrequency(50.0); shooterFollower.getStatorCurrent().setUpdateFrequency(50.0); shooterFollower.getSupplyCurrent().setUpdateFrequency(50.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 9395c4d3..8b85667d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -123,7 +123,7 @@ public TurretImpl() { controller = new PositionVoltage(getTargetAngle().getRotations()).withEnableFOC(true); - turretMotor.getClosedLoopError().setUpdateFrequency(200.0); //TODO: Go lower + turretMotor.getClosedLoopError().setUpdateFrequency(50.0); encoder18tPos = encoder18t.getAbsolutePosition(); encoder17tPos = encoder17t.getAbsolutePosition(); @@ -210,14 +210,14 @@ public double getWrappedTargetAngle() { public void periodicAfterScheduler() { super.periodicAfterScheduler(); - turretConfig.updateGainsConfig( - turretMotor, 1, - Gains.Superstructure.Turret.slot1.kP, - Gains.Superstructure.Turret.slot1.kI, - Gains.Superstructure.Turret.slot1.kD, - Gains.Superstructure.Turret.slot1.kS, - Gains.Superstructure.Turret.slot1.kV, - Gains.Superstructure.Turret.slot1.kA); + // turretConfig.updateGainsConfig( + // turretMotor, 1, + // Gains.Superstructure.Turret.slot1.kP, + // Gains.Superstructure.Turret.slot1.kI, + // Gains.Superstructure.Turret.slot1.kD, + // Gains.Superstructure.Turret.slot1.kS, + // Gains.Superstructure.Turret.slot1.kV, + // Gains.Superstructure.Turret.slot1.kA); if (!hasUsedAbsoluteEncoder) { seedTurret(); From a8ee6a89fdbdc8f08246c97947c094a89202b916 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Wed, 8 Apr 2026 11:33:49 -0400 Subject: [PATCH 28/58] feat: add pose buffering --- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 +- .../robot/subsystems/swerve/CommandSwerveDrivetrain.java | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 9666faa4..5ea5cef0 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -288,7 +288,7 @@ public interface SOTM { public interface Swerve { public final double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; public final double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; - double ACCEPTABLE_POSE_DELTA_METERS = Math.sqrt(Field.LENGTH * Field.LENGTH + Field.WIDTH * Field.WIDTH); //TODO: Might wanna make this smaller. + double MAX_ACCEPTABLE_POSE_DELTA_METERS = Math.sqrt(Field.LENGTH * Field.LENGTH + Field.WIDTH * Field.WIDTH); //TODO: Might wanna make this smaller. public interface Constraints { public final double MAX_VELOCITY_M_PER_S = 4.16; diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 023690a4..d6472f5a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -391,10 +391,12 @@ public Pose2d getPose() { double proposedX = getState().Pose.getX(); double proposedY = getState().Pose.getY(); double poseDelta = lastGoodPose.getTranslation().getDistance(getState().Pose.getTranslation()); + if(!(proposedX > Field.LENGTH || proposedX < 0 || proposedY > Field.WIDTH || proposedY < 0) && - poseDelta < Settings.Swerve.ACCEPTABLE_POSE_DELTA_METERS) { + poseDelta < Settings.Swerve.MAX_ACCEPTABLE_POSE_DELTA_METERS) { lastGoodPose = getState().Pose; } + return lastGoodPose; } From a92f39ef6336ce31025f8d9f0b1fa0565c6b9d74 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Wed, 8 Apr 2026 11:39:07 -0400 Subject: [PATCH 29/58] actually push the bufferedPose --- .../subsystems/swerve/CommandSwerveDrivetrain.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index d6472f5a..f2969381 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -388,13 +388,14 @@ public void addVisionMeasurement( } public Pose2d getPose() { - double proposedX = getState().Pose.getX(); - double proposedY = getState().Pose.getY(); - double poseDelta = lastGoodPose.getTranslation().getDistance(getState().Pose.getTranslation()); + Pose2d proposedPose = getState().Pose; + double proposedX = proposedPose.getX(); + double proposedY = proposedPose.getY(); + double poseDelta = lastGoodPose.getTranslation().getDistance(proposedPose.getTranslation()); if(!(proposedX > Field.LENGTH || proposedX < 0 || proposedY > Field.WIDTH || proposedY < 0) && - poseDelta < Settings.Swerve.MAX_ACCEPTABLE_POSE_DELTA_METERS) { - lastGoodPose = getState().Pose; + poseDelta <= Settings.Swerve.MAX_ACCEPTABLE_POSE_DELTA_METERS) { + lastGoodPose = proposedPose; } return lastGoodPose; From 32d83fccaaa37ee52d81f676d5dc0647c3358ba5 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Wed, 8 Apr 2026 11:43:24 -0400 Subject: [PATCH 30/58] feat: ad logging of CANIVORE bus util --- .../robot/subsystems/swerve/CommandSwerveDrivetrain.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index f2969381..0a7512c8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -26,6 +26,7 @@ import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains; +import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; @@ -678,6 +679,7 @@ public void periodicAfterScheduler() { if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { SmartDashboard.putNumber("Swerve/Failed DAQ Count", this.getState().FailedDaqs); + SmartDashboard.putNumber("Swerve/CANBus Utiliaztion", Ports.CANIVORE.getStatus().BusUtilization); // will confirm whether we are even getting data SmartDashboard.putBoolean("FieldPositions/isBehindTower", isBehindTower()); From c29c07e78b8b34577e9580e753716cc76246a872 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Wed, 8 Apr 2026 12:40:47 -0400 Subject: [PATCH 31/58] refactor: (swerve) more concise getModuleStates and getChassisSpeeds --- .../robot/subsystems/swerve/CommandSwerveDrivetrain.java | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 0a7512c8..955e4315 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -439,15 +439,11 @@ public Command followPathCommand(PathPlannerPath path) { } public SwerveModuleState[] getModuleStates() { - SwerveModuleState[] moduleStates = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - moduleStates[i] = getModule(i).getCurrentState(); - } - return moduleStates; + return getState().ModuleStates; } public ChassisSpeeds getChassisSpeeds() { - return getKinematics().toChassisSpeeds(getModuleStates()); + return getState().Speeds; } public Vector2D getFieldRelativeSpeeds() { From be2c30efe8763b2070a6c5bb2258d6b4ce589fc2 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 8 Apr 2026 13:00:02 -0400 Subject: [PATCH 32/58] debug: (logging) add logging of all swervemodule information and vision data --- .../swerve/CommandSwerveDrivetrain.java | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 955e4315..9866ddb3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -41,6 +41,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -75,6 +76,20 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su static { instance = TunerConstants.createDrivetrain(); + instance.registerTelemetry(instance::telemeterize); + } + + public void telemeterize(SwerveDriveState state) { + /* Write drive state to the log file */ + SignalLogger.writeStruct("DriveState/Pose", Pose2d.struct, state.Pose); + SignalLogger.writeStruct("DriveState/Speeds", ChassisSpeeds.struct, state.Speeds); + SignalLogger.writeStructArray("DriveState/ModuleStates", SwerveModuleState.struct, state.ModuleStates); + SignalLogger.writeStructArray("DriveState/ModuleTargets", SwerveModuleState.struct, state.ModuleTargets); + SignalLogger.writeStructArray("DriveState/ModulePositions", SwerveModulePosition.struct, state.ModulePositions); + SignalLogger.writeStruct("DriveState/RawHeading", Rotation2d.struct, state.RawHeading); + SignalLogger.writeDouble("DriveState/Timestamp", state.Timestamp, "seconds"); + SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); + SignalLogger.writeInteger("DriveState/FailedDaqs", state.FailedDaqs); } public static CommandSwerveDrivetrain getInstance() { @@ -358,6 +373,8 @@ private void startSimThread() { */ @Override public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); + SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); } @@ -380,10 +397,9 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS * meters and radians. */ @Override - public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs) { + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { + SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); + SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); } From 279083bde4ec5eeb330e16a72f79ea03136944a0 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Wed, 8 Apr 2026 15:41:36 -0400 Subject: [PATCH 33/58] feat: reset pose button for left and right corner --- src/main/java/com/stuypulse/robot/Robot.java | 1 + .../java/com/stuypulse/robot/RobotContainer.java | 7 +++++++ .../robot/commands/swerve/SwerveResetPose.java | 2 +- .../swerve/SwerveResetPoseLeftCorner.java | 12 ++++++++++++ .../swerve/SwerveResetPoseRightCorner.java | 12 ++++++++++++ .../com/stuypulse/robot/constants/Settings.java | 1 + .../swerve/CommandSwerveDrivetrain.java | 16 +++++++++++++--- 7 files changed, 47 insertions(+), 4 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseLeftCorner.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseRightCorner.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 8c0e1f16..1991fd9b 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -24,6 +24,7 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 67bd8f11..a66c817d 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -40,6 +40,9 @@ import com.stuypulse.robot.commands.swerve.SwerveDriveFOTM; import com.stuypulse.robot.commands.swerve.SwerveDriveSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveResetPoseLeftCorner; +import com.stuypulse.robot.commands.swerve.SwerveResetPoseRightCorner; import com.stuypulse.robot.commands.swerve.SwerveXMode; import com.stuypulse.robot.commands.turret.SeedTurret; import com.stuypulse.robot.commands.turret.ZeroTurret; @@ -73,6 +76,7 @@ import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import com.stuypulse.stuylib.network.SmartBoolean; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -307,6 +311,9 @@ private void configureElasticButtons() { SmartDashboard.putData("Robot/Set Megatag 1", new SetMegaTagMode(MegaTagMode.MEGATAG1)); SmartDashboard.putData("Robot/Set Megatag 2", new SetMegaTagMode(MegaTagMode.MEGATAG2)); + SmartDashboard.putData("Robot/Set Robot Pose Left Corner", new SwerveResetPoseLeftCorner()); + SmartDashboard.putData("Robot/Set Robot Pose Right Corner", new SwerveResetPoseRightCorner()); + // SmartDashboard.putData("Robot/Set Left LL PF", new EnableLeftLimelight()); // SmartDashboard.putData("Robot/Set Right LL PF", new EnableRightLimelight()); // SmartDashboard.putData("Robot/Set Back LL PF", new EnableBackLimelight()); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java index 93d72bf8..0bddbe82 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.InstantCommand; -public class SwerveResetPose extends InstantCommand{ +public class SwerveResetPose extends InstantCommand { private final CommandSwerveDrivetrain swerve; private final Pose2d poseToReset; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseLeftCorner.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseLeftCorner.java new file mode 100644 index 00000000..b752c219 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseLeftCorner.java @@ -0,0 +1,12 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; + +public class SwerveResetPoseLeftCorner extends SwerveResetPose { + public SwerveResetPoseLeftCorner() { + super(new Pose2d(0, Field.WIDTH, CommandSwerveDrivetrain.getInstance().getPose().getRotation())); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseRightCorner.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseRightCorner.java new file mode 100644 index 00000000..ebd7ffab --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseRightCorner.java @@ -0,0 +1,12 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; + +public class SwerveResetPoseRightCorner extends SwerveResetPose { + public SwerveResetPoseRightCorner() { + super(new Pose2d(0, 0, CommandSwerveDrivetrain.getInstance().getPose().getRotation())); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 5ea5cef0..b585438f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -289,6 +289,7 @@ public interface Swerve { public final double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; public final double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; double MAX_ACCEPTABLE_POSE_DELTA_METERS = Math.sqrt(Field.LENGTH * Field.LENGTH + Field.WIDTH * Field.WIDTH); //TODO: Might wanna make this smaller. + double MAX_ACCEPTABLE_VISION_DEVIATION_METERS = 1.0; public interface Constraints { public final double MAX_VELOCITY_M_PER_S = 4.16; diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 9866ddb3..b26265cd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -361,6 +361,10 @@ private void startSimThread() { m_simNotifier.startPeriodic(kSimLoopPeriod); } + private boolean checkIfVisionMeasurementValid(Pose2d visionPose) { + return visionPose.getTranslation().getDistance(getState().Pose.getTranslation()) < Settings.Swerve.MAX_ACCEPTABLE_VISION_DEVIATION_METERS; + } + /** * Adds a vision measurement to the Kalman Filter. This will correct the * odometry pose estimate @@ -375,7 +379,10 @@ private void startSimThread() { public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + + if(checkIfVisionMeasurementValid(visionRobotPoseMeters)) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + } } /** @@ -400,8 +407,11 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), - visionMeasurementStdDevs); + + if(checkIfVisionMeasurementValid(visionRobotPoseMeters)) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), + visionMeasurementStdDevs); + } } public Pose2d getPose() { From 01955382872f1c57a91c137be91d3d9e30755940 Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Thu, 9 Apr 2026 14:47:48 -0400 Subject: [PATCH 34/58] chore: update phoenix6 vendor dep, supposedly could be more performant --- vendordeps/Phoenix6-26.1.3.json | 449 ++++++++++++++++++++++++ vendordeps/Phoenix6-frc2026-latest.json | 447 ----------------------- 2 files changed, 449 insertions(+), 447 deletions(-) create mode 100644 vendordeps/Phoenix6-26.1.3.json delete mode 100644 vendordeps/Phoenix6-frc2026-latest.json diff --git a/vendordeps/Phoenix6-26.1.3.json b/vendordeps/Phoenix6-26.1.3.json new file mode 100644 index 00000000..d5bc4a24 --- /dev/null +++ b/vendordeps/Phoenix6-26.1.3.json @@ -0,0 +1,449 @@ +{ + "fileName": "Phoenix6-26.1.3.json", + "name": "CTRE-Phoenix (v6)", + "version": "26.1.3", + "frcYear": "2026", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2026-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "26.1.3" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "26.1.3", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.1.3", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "26.1.3", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.1.3", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.1.3", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.1.3", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.1.3", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.1.3", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.1.3", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.1.3", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.1.3", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.1.3", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.1.3", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.1.3", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6-frc2026-latest.json b/vendordeps/Phoenix6-frc2026-latest.json deleted file mode 100644 index 07275f30..00000000 --- a/vendordeps/Phoenix6-frc2026-latest.json +++ /dev/null @@ -1,447 +0,0 @@ -{ - "fileName": "Phoenix6-frc2026-latest.json", - "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", - "frcYear": "2026", - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": ["https://maven.ctr-electronics.com/release/"], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json", - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2026-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "26.1.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "api-cpp", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "api-cpp-sim", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdle", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "26.1.0", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "26.1.0", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "26.1.0", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "26.1.0", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "26.1.0", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "26.1.0", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "26.1.0", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "26.1.0", - "libName": "CTRE_SimProTalonFXS", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "26.1.0", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "26.1.0", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "26.1.0", - "libName": "CTRE_SimProCANrange", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "26.1.0", - "libName": "CTRE_SimProCANdi", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdle", - "version": "26.1.0", - "libName": "CTRE_SimProCANdle", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} From e22df1113bb1faffa7d6518d7718b997ffc03b28 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Thu, 9 Apr 2026 16:13:35 -0400 Subject: [PATCH 35/58] FEAT: added bump autos --- simgui.json | 6 + .../autos/{Box Test.auto => Left Middy.auto} | 14 +-- .../{Depot Auto.auto => Right Middy.auto} | 10 +- .../pathplanner/autos/Straight Line Test.auto | 19 ---- src/main/deploy/pathplanner/paths/Box 1.path | 54 --------- src/main/deploy/pathplanner/paths/Box 2.path | 54 --------- src/main/deploy/pathplanner/paths/Box 3.path | 54 --------- .../paths/Depot To Tower Left.path | 59 ---------- ...(B).path => Left Bump Score To Depot.path} | 49 ++++++--- .../pathplanner/paths/Left Bump To Depot.path | 54 --------- ...ch To NZ (B).path => Left Bump To NZ.path} | 57 ++++------ .../paths/Left Bump To Trench.path | 59 ---------- ...ine.path => Left Middy To Bump Score.path} | 22 ++-- .../pathplanner/paths/Left NZ To Score.path | 42 +++++-- .../paths/Left Score To NZ (F).path | 12 +- .../paths/Left Score To Score.path | 20 ++-- .../paths/Left Trench To NZ (B).path | 103 ------------------ .../pathplanner/paths/Left Trench To NZ.path | 16 +-- .../paths/Right Bump Score To Depot.path | 102 +++++++++++++++++ .../pathplanner/paths/Right Bump To NZ.path | 84 ++++++++++++++ .../paths/Right Bump To Trench.path | 59 ---------- ... 4.path => Right Middy To Bump Score.path} | 24 ++-- .../paths/Right NZ To Score (B).path | 73 ------------- .../pathplanner/paths/Right NZ To Score.path | 42 +++++-- .../paths/Right Score To NZ (F).path | 12 +- .../paths/Right Score To Score.path | 22 ++-- .../paths/Right Trench To NZ (F).path | 59 ---------- .../pathplanner/paths/Right Trench To NZ.path | 14 +-- src/main/deploy/pathplanner/settings.json | 9 +- .../com/stuypulse/robot/RobotContainer.java | 12 +- .../commands/auton/regular/DepotAuton.java | 53 --------- .../commands/auton/regular/EightFuel.java | 35 ------ .../commands/auton/regular/LeftMiddy.java | 50 +++++++++ .../commands/auton/regular/RightMiddy.java | 50 +++++++++ 34 files changed, 502 insertions(+), 902 deletions(-) rename src/main/deploy/pathplanner/autos/{Box Test.auto => Left Middy.auto} (63%) rename src/main/deploy/pathplanner/autos/{Depot Auto.auto => Right Middy.auto} (59%) delete mode 100644 src/main/deploy/pathplanner/autos/Straight Line Test.auto delete mode 100644 src/main/deploy/pathplanner/paths/Box 1.path delete mode 100644 src/main/deploy/pathplanner/paths/Box 2.path delete mode 100644 src/main/deploy/pathplanner/paths/Box 3.path delete mode 100644 src/main/deploy/pathplanner/paths/Depot To Tower Left.path rename src/main/deploy/pathplanner/paths/{Left NZ To Score (B).path => Left Bump Score To Depot.path} (53%) delete mode 100644 src/main/deploy/pathplanner/paths/Left Bump To Depot.path rename src/main/deploy/pathplanner/paths/{Right Trench To NZ (B).path => Left Bump To NZ.path} (52%) delete mode 100644 src/main/deploy/pathplanner/paths/Left Bump To Trench.path rename src/main/deploy/pathplanner/paths/{Straight Line.path => Left Middy To Bump Score.path} (69%) delete mode 100644 src/main/deploy/pathplanner/paths/Left Trench To NZ (B).path create mode 100644 src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path create mode 100644 src/main/deploy/pathplanner/paths/Right Bump To NZ.path delete mode 100644 src/main/deploy/pathplanner/paths/Right Bump To Trench.path rename src/main/deploy/pathplanner/paths/{Box 4.path => Right Middy To Bump Score.path} (67%) delete mode 100644 src/main/deploy/pathplanner/paths/Right NZ To Score (B).path delete mode 100644 src/main/deploy/pathplanner/paths/Right Trench To NZ (F).path delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java diff --git a/simgui.json b/simgui.json index 9aec20cb..a3bae0c6 100644 --- a/simgui.json +++ b/simgui.json @@ -31,10 +31,16 @@ "/SmartDashboard/Robot/Seed Pivot Encoder at Upper Limit (Stowed)": "Command", "/SmartDashboard/Robot/Seed Turret": "Command", "/SmartDashboard/Robot/Set Back LL PF": "Command", + "/SmartDashboard/Robot/Set Exposure 100": "Command", + "/SmartDashboard/Robot/Set Exposure 130": "Command", + "/SmartDashboard/Robot/Set Exposure 25": "Command", + "/SmartDashboard/Robot/Set Exposure 80": "Command", "/SmartDashboard/Robot/Set Left LL PF": "Command", "/SmartDashboard/Robot/Set Megatag 1": "Command", "/SmartDashboard/Robot/Set Megatag 2": "Command", "/SmartDashboard/Robot/Set Right LL PF": "Command", + "/SmartDashboard/Robot/Set Robot Pose Left Corner": "Command", + "/SmartDashboard/Robot/Set Robot Pose Right Corner": "Command", "/SmartDashboard/Robot/Spindexer Reverse": "Command", "/SmartDashboard/Robot/WL Outpost Tags Left-Camera": "Command", "/SmartDashboard/Robot/WL Tower Tags Right-Camera": "Command", diff --git a/src/main/deploy/pathplanner/autos/Box Test.auto b/src/main/deploy/pathplanner/autos/Left Middy.auto similarity index 63% rename from src/main/deploy/pathplanner/autos/Box Test.auto rename to src/main/deploy/pathplanner/autos/Left Middy.auto index 34e6aefd..069289b1 100644 --- a/src/main/deploy/pathplanner/autos/Box Test.auto +++ b/src/main/deploy/pathplanner/autos/Left Middy.auto @@ -7,31 +7,25 @@ { "type": "path", "data": { - "pathName": "Box 1" + "pathName": "Left Bump To NZ" } }, { "type": "path", "data": { - "pathName": "Box 2" + "pathName": "Left Middy To Bump Score" } }, { "type": "path", "data": { - "pathName": "Box 3" - } - }, - { - "type": "path", - "data": { - "pathName": "Box 4" + "pathName": "Left Bump Score To Depot" } } ] } }, "resetOdom": true, - "folder": "Tests", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Depot Auto.auto b/src/main/deploy/pathplanner/autos/Right Middy.auto similarity index 59% rename from src/main/deploy/pathplanner/autos/Depot Auto.auto rename to src/main/deploy/pathplanner/autos/Right Middy.auto index 03a0af76..457285aa 100644 --- a/src/main/deploy/pathplanner/autos/Depot Auto.auto +++ b/src/main/deploy/pathplanner/autos/Right Middy.auto @@ -7,13 +7,19 @@ { "type": "path", "data": { - "pathName": "Left Bump To Depot" + "pathName": "Right Bump To NZ" } }, { "type": "path", "data": { - "pathName": "Depot To Tower Left" + "pathName": "Right Middy To Bump Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Bump Score To Depot" } } ] diff --git a/src/main/deploy/pathplanner/autos/Straight Line Test.auto b/src/main/deploy/pathplanner/autos/Straight Line Test.auto deleted file mode 100644 index 92bbd21a..00000000 --- a/src/main/deploy/pathplanner/autos/Straight Line Test.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Straight Line" - } - } - ] - } - }, - "resetOdom": true, - "folder": "Tests", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 1.path b/src/main/deploy/pathplanner/paths/Box 1.path deleted file mode 100644 index 383fc29e..00000000 --- a/src/main/deploy/pathplanner/paths/Box 1.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 2.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0000000000000018, - "y": 2.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0, - "y": 2.0 - }, - "prevControl": { - "x": 2.0, - "y": 2.0 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.75, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Tests", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 2.path b/src/main/deploy/pathplanner/paths/Box 2.path deleted file mode 100644 index 0dfb8dd4..00000000 --- a/src/main/deploy/pathplanner/paths/Box 2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.0, - "y": 2.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.003409325748791, - "y": 1.720136381756153 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0, - "y": 1.0 - }, - "prevControl": { - "x": 3.0045204355743644, - "y": 1.6922222258449668 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.75, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "Tests", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 3.path b/src/main/deploy/pathplanner/paths/Box 3.path deleted file mode 100644 index 0eb515ac..00000000 --- a/src/main/deploy/pathplanner/paths/Box 3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.0, - "y": 1.0 - }, - "prevControl": null, - "nextControl": { - "x": 1.77619604200323, - "y": 0.9977414835927593 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.0, - "y": 1.0 - }, - "prevControl": { - "x": 3.0717077172960874, - "y": 0.9943971527224802 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.75, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "Tests", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path deleted file mode 100644 index e222e6f0..00000000 --- a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.7278054567022547, - "y": 5.9339976275207595 - }, - "prevControl": null, - "nextControl": { - "x": 3.869489916963227, - "y": 6.149181494661922 - }, - "isLocked": false, - "linkedName": "Depot Collect" - }, - { - "anchor": { - "x": 1.2227283511269282, - "y": 4.922633451957295 - }, - "prevControl": { - "x": 3.643546856465006, - "y": 4.83655990510083 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Tower Left" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.06336939721792927, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.75, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Misc", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path similarity index 53% rename from src/main/deploy/pathplanner/paths/Left NZ To Score (B).path rename to src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path index b204f5d0..1e3ccf68 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path +++ b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path @@ -3,48 +3,61 @@ "waypoints": [ { "anchor": { - "x": 8.308815977175463, - "y": 4.74015691868759 + "x": 3.4749522900763368, + "y": 5.507824427480916 }, "prevControl": null, "nextControl": { - "x": 5.554600612918676, - "y": 5.296473213152537 + "x": 1.6506583969465654, + "y": 5.959713740458016 }, "isLocked": false, - "linkedName": "NZ Left Center" + "linkedName": "Left Bump Score" }, { "anchor": { - "x": 2.8904033214709375, - "y": 5.643499406880189 + "x": 0.5795133587786261, + "y": 5.959713740458016 }, "prevControl": { - "x": 4.644151838671412, - "y": 5.600462633451957 + "x": 1.4665553435114504, + "y": 5.959713740458016 }, "nextControl": null, "isLocked": false, - "linkedName": "Left Bump Score" + "linkedName": "Depot" } ], "rotationTargets": [ { - "waypointRelativePos": 0.3516873889875677, + "waypointRelativePos": 0.6590257879656263, "rotationDegrees": 180.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.42902609802673963, - "maxWaypointRelativePos": 0.846594525779758, + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.7, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.75, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.5, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 5.0, + "maxVelocity": 1.75, + "maxAcceleration": 4.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "nominalVoltage": 12.5, "unlimited": false } } @@ -64,10 +77,10 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Left Bump", + "folder": "To Depot", "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path deleted file mode 100644 index c19c249e..00000000 --- a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.589750889679717, - "y": 5.9339976275207595 - }, - "prevControl": null, - "nextControl": { - "x": 3.8521567589767303, - "y": 5.92251805970181 - }, - "isLocked": false, - "linkedName": "Left Bump Start" - }, - { - "anchor": { - "x": 0.7278054567022547, - "y": 5.9339976275207595 - }, - "prevControl": { - "x": 1.6779675215686134, - "y": 5.934033465612401 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Depot Collect" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.75, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "Misc", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ (B).path b/src/main/deploy/pathplanner/paths/Left Bump To NZ.path similarity index 52% rename from src/main/deploy/pathplanner/paths/Right Trench To NZ (B).path rename to src/main/deploy/pathplanner/paths/Left Bump To NZ.path index f8e2025e..75261865 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ (B).path +++ b/src/main/deploy/pathplanner/paths/Left Bump To NZ.path @@ -3,68 +3,59 @@ "waypoints": [ { "anchor": { - "x": 4.051997146932953, - "y": 0.6256633380884444 + "x": 3.500057251908398, + "y": 5.122881679389313 }, "prevControl": null, "nextControl": { - "x": 6.784832259625718, - "y": 0.5611081779460949 + "x": 6.378759541984733, + "y": 5.315353053435115 }, "isLocked": false, - "linkedName": "Right Trench (B)" + "linkedName": "Left Bump Start" }, { "anchor": { - "x": 7.092582025677602, - "y": 1.531369472182596 + "x": 6.110973282442748, + "y": 4.562204198473283 }, "prevControl": { - "x": 7.054174783837401, - "y": 0.732498841906393 + "x": 5.914394099487028, + "y": 5.110601060796104 }, "nextControl": { - "x": 7.157275320970041, - "y": 2.8769900142653353 + "x": 6.354700620229008, + "y": 3.8822781488549625 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.911921708185054, - "y": 2.491055753262159 + "x": 8.144475190839694, + "y": 4.026631679389314 }, "prevControl": { - "x": 5.2897034400948995, - "y": 2.437259786476868 + "x": 7.03148854961832, + "y": 3.951316793893131 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Bump Score" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.12255772646536273, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1.6, - "rotationDegrees": 180.0 + "linkedName": "Middy" } ], + "rotationTargets": [], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 1.5964353914703855, - "maxWaypointRelativePos": 1.866327180140039, + "minWaypointRelativePos": 1.6600517687661842, + "maxWaypointRelativePos": 2.0, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 5.0, + "maxVelocity": 1.75, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "nominalVoltage": 12.5, "unlimited": false } } @@ -81,10 +72,10 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, - "folder": "Right Bump", + "folder": "To NZ", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Left Bump To Trench.path b/src/main/deploy/pathplanner/paths/Left Bump To Trench.path deleted file mode 100644 index aa0da23f..00000000 --- a/src/main/deploy/pathplanner/paths/Left Bump To Trench.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.8904033214709375, - "y": 5.643499406880189 - }, - "prevControl": null, - "nextControl": { - "x": 2.7290154211150655, - "y": 6.7732147093712936 - }, - "isLocked": false, - "linkedName": "Left Bump Score" - }, - { - "anchor": { - "x": 4.31061684460261, - "y": 7.408007117437722 - }, - "prevControl": { - "x": 2.57838671411625, - "y": 7.451043890865957 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Left Trench (B)" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.49378330373001517, - "rotationDegrees": 45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 75.0, - "maxAngularAcceleration": 300.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Left Bump", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path similarity index 69% rename from src/main/deploy/pathplanner/paths/Straight Line.path rename to src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path index c97fab84..8ccc86c4 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line.path +++ b/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 8.144475190839694, + "y": 4.026631679389314 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 4.997986641221376, + "y": 5.876030534351146 }, "isLocked": false, - "linkedName": null + "linkedName": "Middy" }, { "anchor": { - "x": 4.48276393831554, - "y": 7.0 + "x": 3.4749522900763368, + "y": 5.507824427480916 }, "prevControl": { - "x": 3.48276393831554, - "y": 7.0 + "x": 5.516822519083972, + "y": 5.616612595419848 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Left Bump Score" } ], "rotationTargets": [], @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Tests", + "folder": "To Score", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 1ab2ab67..94130117 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -8,20 +8,36 @@ }, "prevControl": null, "nextControl": { - "x": 4.582482168330956, - "y": 5.89169757489301 + "x": 5.82645038167939, + "y": 4.687729007633589 }, "isLocked": false, - "linkedName": "NZ Left Center" + "linkedName": "Left NZ" }, { "anchor": { - "x": 3.09453637660485, - "y": 7.366704707560627 + "x": 6.052395038167939, + "y": 6.378129770992366 }, "prevControl": { - "x": 9.964964336661911, - "y": 7.819557774607704 + "x": 6.061945145745694, + "y": 4.945613634328742 + }, + "nextControl": { + "x": 6.044026717557253, + "y": 7.63337786259542 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.583740458015267, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.504284351145039, + "y": 7.524589694656489 }, "nextControl": null, "isLocked": false, @@ -30,17 +46,21 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.8063943161634181, + "waypointRelativePos": 0.5, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.6, "rotationDegrees": 0.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.8372721134368668, - "maxWaypointRelativePos": 1.0, + "minWaypointRelativePos": 1.65, + "maxWaypointRelativePos": 2.0, "constraints": { - "maxVelocity": 2.0, + "maxVelocity": 2.5, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index adefa99d..e7ae2b7e 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.09453637660485, - "y": 7.366704707560627 + "x": 3.583740458015267, + "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 6.303323823109842, - "y": 7.366704707560627 + "x": 6.792527904520259, + "y": 7.440906488549619 }, "isLocked": false, - "linkedName": "Left Score (F)" + "linkedName": "Left Trench Score" }, { "anchor": { @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "NZ Left Center" + "linkedName": "Left NZ" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index 9607da36..32dcb9a8 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.09453637660485, - "y": 7.366704707560627 + "x": 3.583740458015267, + "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 6.865636376608087, - "y": 7.49338248534429 + "x": 7.354840458018504, + "y": 7.567584266333281 }, "isLocked": false, "linkedName": "Left Trench Score" @@ -64,21 +64,21 @@ }, { "anchor": { - "x": 3.09453637660485, - "y": 7.366704707560627 + "x": 3.583740458015267, + "y": 7.440906488549619 }, "prevControl": { - "x": 4.750684736091299, - "y": 7.3254022976835325 + "x": 5.239888817501716, + "y": 7.399604078672524 }, "nextControl": null, "isLocked": false, - "linkedName": "Left Score (F)" + "linkedName": "Left Trench Score" } ], "rotationTargets": [ { - "waypointRelativePos": 0.23880597014925345, + "waypointRelativePos": 0.1891117478510029, "rotationDegrees": 0.0 }, { diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ (B).path b/src/main/deploy/pathplanner/paths/Left Trench To NZ (B).path deleted file mode 100644 index e19531ad..00000000 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ (B).path +++ /dev/null @@ -1,103 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.31061684460261, - "y": 7.408007117437722 - }, - "prevControl": null, - "nextControl": { - "x": 6.462455516014234, - "y": 7.612431791221827 - }, - "isLocked": false, - "linkedName": "Left Trench (B)" - }, - { - "anchor": { - "x": 6.064365361803085, - "y": 5.944756820877817 - }, - "prevControl": { - "x": 6.064365361803085, - "y": 7.378315878190629 - }, - "nextControl": { - "x": 6.064365361803085, - "y": 3.9005100830367745 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.064365361803085, - "y": 4.793523131672598 - }, - "prevControl": { - "x": 6.456698309590022, - "y": 3.6435817329867413 - }, - "nextControl": { - "x": 5.7523487544484, - "y": 5.708054567022539 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.8904033214709375, - "y": 5.643499406880189 - }, - "prevControl": { - "x": 6.2903084223013055, - "y": 5.632740213523132 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Left Bump Score" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.9005328596802835, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 1.641207815275316, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 2.26998223801069, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.75, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "Left Bump", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 80fa6da2..84f1a734 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.998600237247925, - "y": 7.526358244365362 + "x": 4.445677480916031, + "y": 7.675219465648855 }, "prevControl": null, "nextControl": { - "x": 8.99456490727532, - "y": 7.509029957203994 + "x": 8.86415076335878, + "y": 7.700324427480917 }, "isLocked": false, "linkedName": "Left Trench Start" @@ -20,17 +20,17 @@ "y": 4.74015691868759 }, "prevControl": { - "x": 8.257061340941512, - "y": 7.560784593437947 + "x": 8.261631679389314, + "y": 7.265171755725191 }, "nextControl": null, "isLocked": false, - "linkedName": "NZ Left Center" + "linkedName": "Left NZ" } ], "rotationTargets": [ { - "waypointRelativePos": 0.05, + "waypointRelativePos": 0.140401146131807, "rotationDegrees": -90.0 } ], diff --git a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path new file mode 100644 index 00000000..a0393557 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path @@ -0,0 +1,102 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4749522900763368, + "y": 2.486860687022901 + }, + "prevControl": null, + "nextControl": { + "x": 1.8598664122137403, + "y": 4.838358778625954 + }, + "isLocked": false, + "linkedName": "Right Bump Score" + }, + { + "anchor": { + "x": 1.8933396946564884, + "y": 4.938778625954199 + }, + "prevControl": { + "x": 1.9041990007255656, + "y": 4.689014586365434 + }, + "nextControl": { + "x": 1.8369555219739746, + "y": 6.235614597651982 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5795133587786261, + "y": 5.959713740458016 + }, + "prevControl": { + "x": 1.684131679389313, + "y": 5.859293893129771 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.6, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.65, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.5, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.65, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.75, + "maxAcceleration": 4.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.5, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.75, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.5, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To Depot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump To NZ.path b/src/main/deploy/pathplanner/paths/Right Bump To NZ.path new file mode 100644 index 00000000..bb98e1a0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Bump To NZ.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4833206106870227, + "y": 2.9220133587786257 + }, + "prevControl": null, + "nextControl": { + "x": 5.4749809160305345, + "y": 2.813225190839695 + }, + "isLocked": false, + "linkedName": "Right Bump Start" + }, + { + "anchor": { + "x": 6.336917938931298, + "y": 3.4492175572519077 + }, + "prevControl": { + "x": 6.36674218315174, + "y": 2.6439629632999404 + }, + "nextControl": { + "x": 6.3034446564885505, + "y": 4.352996183206107 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.144475190839694, + "y": 4.026631679389314 + }, + "prevControl": { + "x": 7.144475190839694, + "y": 4.026631679389314 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Middy" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.6496980155306307, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.75, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.5, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.75, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.5, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump To Trench.path b/src/main/deploy/pathplanner/paths/Right Bump To Trench.path deleted file mode 100644 index 80d24be7..00000000 --- a/src/main/deploy/pathplanner/paths/Right Bump To Trench.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.911921708185054, - "y": 2.491055753262159 - }, - "prevControl": null, - "nextControl": { - "x": 2.567627520759194, - "y": 1.2752669039145905 - }, - "isLocked": false, - "linkedName": "Right Bump Score" - }, - { - "anchor": { - "x": 4.051997146932953, - "y": 0.6256633380884444 - }, - "prevControl": { - "x": 2.4087874465049923, - "y": 0.8973751783166917 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Right Trench (B)" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.49378330373001517, - "rotationDegrees": -45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 75.0, - "maxAngularAcceleration": 300.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Right Bump", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 4.path b/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path similarity index 67% rename from src/main/deploy/pathplanner/paths/Box 4.path rename to src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path index 0a5359c2..cf4dfedf 100644 --- a/src/main/deploy/pathplanner/paths/Box 4.path +++ b/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 1.0 + "x": 8.144475190839694, + "y": 4.026631679389314 }, "prevControl": null, "nextControl": { - "x": 2.0299091229524393, - "y": 1.2482044406617574 + "x": 5.533559160305344, + "y": 2.2023377862595424 }, "isLocked": false, - "linkedName": null + "linkedName": "Middy" }, { "anchor": { - "x": 2.0, - "y": 2.0 + "x": 3.4749522900763368, + "y": 2.486860687022901 }, "prevControl": { - "x": 2.0255951320775263, - "y": 1.751313673045875 + "x": 5.223931297709925, + "y": 2.5119656488549618 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Right Bump Score" } ], "rotationTargets": [], @@ -45,10 +45,10 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Tests", + "folder": "To Score", "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path deleted file mode 100644 index 2a6727b0..00000000 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path +++ /dev/null @@ -1,73 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.231184022824536, - "y": 3.3427817403708993 - }, - "prevControl": null, - "nextControl": { - "x": 5.302642099153387, - "y": 2.620718614147219 - }, - "isLocked": false, - "linkedName": "Right Center NZ" - }, - { - "anchor": { - "x": 2.911921708185054, - "y": 2.491055753262159 - }, - "prevControl": { - "x": 4.8270581257414005, - "y": 2.5771293001186257 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Right Bump Score" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3516873889875677, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.42138765117759275, - "maxWaypointRelativePos": 0.8338637810311925, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.75, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "Right Bump", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index af348df6..ebfdd5c6 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -8,20 +8,36 @@ }, "prevControl": null, "nextControl": { - "x": 4.996519258202568, - "y": 2.4111982881597713 + "x": 6.0775, + "y": 3.24837786259542 }, "isLocked": false, - "linkedName": "Right Center NZ" + "linkedName": "Right NZ" }, { "anchor": { - "x": 2.978088445078458, - "y": 0.6386019971469341 + "x": 6.362022900763359, + "y": 1.5496087786259545 }, "prevControl": { - "x": 9.162767475035661, - "y": 0.06930099857346694 + "x": 6.211393129770993, + "y": 3.0810114503816792 + }, + "nextControl": { + "x": 6.486333025849947, + "y": 0.28578917357895817 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.633950381679389, + "y": 0.5705152671755729 + }, + "prevControl": { + "x": 6.582732165608568, + "y": 0.5693751020897069 }, "nextControl": null, "isLocked": false, @@ -30,17 +46,21 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.7921847246891675, + "waypointRelativePos": 0.4183381088825223, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.6, "rotationDegrees": 0.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.8507765023632616, - "maxWaypointRelativePos": 1.0, + "minWaypointRelativePos": 1.7, + "maxWaypointRelativePos": 2.0, "constraints": { - "maxVelocity": 2.0, + "maxVelocity": 2.5, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index c0b3de28..9919ba30 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 2.978088445078458, - "y": 0.6386019971469341 + "x": 3.633950381679389, + "y": 0.5705152671755729 }, "prevControl": null, "nextControl": { - "x": 6.549158345221111, - "y": 0.6256633380884467 + "x": 7.2050202818220415, + "y": 0.5575766081170854 }, "isLocked": false, - "linkedName": "Right Score (F)" + "linkedName": "Right Trench Score" }, { "anchor": { @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Right Center NZ" + "linkedName": "Right NZ" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index ce062cad..cc4b998b 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.978088445078458, - "y": 0.6386019971469341 + "x": 3.633950381679389, + "y": 0.5705152671755729 }, "prevControl": null, "nextControl": { - "x": 7.2090299572039935, - "y": 0.6127246790299581 + "x": 6.847385496183207, + "y": 0.5119370229007632 }, "isLocked": false, "linkedName": "Right Trench Score" @@ -48,16 +48,16 @@ }, { "anchor": { - "x": 2.978088445078458, - "y": 0.6386019971469341 + "x": 3.633950381679389, + "y": 0.5705152671755729 }, "prevControl": { - "x": 6.794992867332382, - "y": 0.5480313837375186 + "x": 7.4508548039333125, + "y": 0.47994465376615736 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Score (F)" + "linkedName": "Right Trench Score" } ], "rotationTargets": [ @@ -66,11 +66,11 @@ "rotationDegrees": 0.0 }, { - "waypointRelativePos": 0.7344398340248979, + "waypointRelativePos": 0.5329512893982814, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 0.9933609958506272, + "waypointRelativePos": 1.031518624641834, "rotationDegrees": 90.0 }, { diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Trench To NZ (F).path deleted file mode 100644 index 7c9d49ff..00000000 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ (F).path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.051997146932953, - "y": 0.6256633380884444 - }, - "prevControl": null, - "nextControl": { - "x": 6.9354609666245315, - "y": 0.6364225314455019 - }, - "isLocked": false, - "linkedName": "Right Trench (B)" - }, - { - "anchor": { - "x": 8.218245363766048, - "y": 3.0840085592011417 - }, - "prevControl": { - "x": 7.301672597864768, - "y": 0.6619928825622776 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.21871820956256674, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.75, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 93.81407483429037 - }, - "reversed": false, - "folder": "Right Bump", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 73b3ceba..2812295c 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.039058487874465, - "y": 0.5221540656205423 + "x": 4.412204198473283, + "y": 0.3947805343511448 }, "prevControl": null, "nextControl": { - "x": 9.408601997146931, - "y": 0.44452211126961516 + "x": 9.056622137404581, + "y": 0.44499045801526793 }, "isLocked": false, "linkedName": "Right Trench Start" @@ -20,12 +20,12 @@ "y": 3.3427817403708993 }, "prevControl": { - "x": 8.412325249643365, - "y": -0.7328958630527822 + "x": 8.286736641221374, + "y": 0.7713549618320608 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Center NZ" + "linkedName": "Right NZ" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 230e3d45..beb42229 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,16 +3,11 @@ "robotLength": 0.762, "holonomicMode": true, "pathFolders": [ - "Left Bump", - "Misc", - "Right Bump", - "Tests", + "To Depot", "To NZ", "To Score" ], - "autoFolders": [ - "Tests" - ], + "autoFolders": [], "defaultMaxVel": 3.75, "defaultMaxAccel": 7.0, "defaultMaxAngVel": 300.0, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index a66c817d..c8eee889 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,8 +7,9 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.auton.regular.DepotAuton; +import com.stuypulse.robot.commands.auton.regular.LeftMiddy; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; +import com.stuypulse.robot.commands.auton.regular.RightMiddy; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; @@ -354,9 +355,12 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); // DEPOT - AutonConfig DEPOT_AUTON = new AutonConfig("Depot Auton", DepotAuton::new, - "Left Bump To Depot", "Depot To Tower Left"); - DEPOT_AUTON.register(autonChooser); + AutonConfig LEFT_MIDDY = new AutonConfig("Left Middy", LeftMiddy::new, + "Left Bump To NZ", "Left Middy To Bump Score", "Left Bump Score To Depot"); + LEFT_MIDDY.register(autonChooser); + AutonConfig RIGHT_MIDDY = new AutonConfig("Right Middy", RightMiddy::new, + "Right Bump To NZ", "Right Middy To Bump Score", "Right Bump Score To Depot"); + RIGHT_MIDDY.register(autonChooser); // TWO CYCLES (TRENCH) AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java deleted file mode 100644 index e00012de..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java +++ /dev/null @@ -1,53 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; - -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.intake.IntakeStow; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -import com.pathplanner.lib.path.PathPlannerPath; - -public class DepotAuton extends SequentialCommandGroup { - - public DepotAuton(PathPlannerPath... paths) { - - addCommands( - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - - // To Depot - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) - ), - - new IntakeStow().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]) - ), - - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SpindexerRun().alongWith( - new HandoffRun() - ) - // .until(() -> DriverStation.getMatchTime() < 2).andThen( - // new ParallelCommandGroup( - // new HandoffStop(), - // new SpindexerStop(), - // new ClimberDown() - // ) - // ) - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java deleted file mode 100644 index 6ece7d67..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java +++ /dev/null @@ -1,35 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; - -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.superstructure.SuperstructureKB; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -import com.pathplanner.lib.path.PathPlannerPath; - -public class EightFuel extends SequentialCommandGroup { - - public EightFuel(PathPlannerPath... paths) { - - addCommands( - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - new SuperstructureKB().alongWith( - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()).andThen( - new SpindexerRun().alongWith(new HandoffRun()) - ) - ) - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java new file mode 100644 index 00000000..cd9cadc9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java @@ -0,0 +1,50 @@ +package com.stuypulse.robot.commands.auton.regular; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class LeftMiddy extends SequentialCommandGroup { + + public LeftMiddy(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(1.0).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + + // SOTM To Depot + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffRun().andThen( + new SpindexerRun() + ) + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java new file mode 100644 index 00000000..73477f4b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java @@ -0,0 +1,50 @@ +package com.stuypulse.robot.commands.auton.regular; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightMiddy extends SequentialCommandGroup { + + public RightMiddy(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(1.0).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + + // SOTM To Depot + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffRun().andThen( + new SpindexerRun() + ) + ) + + ); + + } + +} From a34e88fabcb9574880d7a99202ffe86b9721fab4 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Thu, 9 Apr 2026 18:14:33 -0400 Subject: [PATCH 36/58] FEAT: brought robot out of depot in auto --- .../deploy/pathplanner/autos/Left Middy.auto | 6 ++ .../deploy/pathplanner/autos/Right Middy.auto | 6 ++ .../pathplanner/paths/Depot To Score.path | 68 +++++++++++++++++++ .../paths/Left Bump Score To Depot.path | 13 ---- .../paths/Right Bump Score To Depot.path | 15 +--- .../com/stuypulse/robot/RobotContainer.java | 4 +- .../commands/auton/regular/LeftMiddy.java | 20 +++++- .../commands/auton/regular/RightMiddy.java | 20 +++++- 8 files changed, 117 insertions(+), 35 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Depot To Score.path diff --git a/src/main/deploy/pathplanner/autos/Left Middy.auto b/src/main/deploy/pathplanner/autos/Left Middy.auto index 069289b1..427c8f10 100644 --- a/src/main/deploy/pathplanner/autos/Left Middy.auto +++ b/src/main/deploy/pathplanner/autos/Left Middy.auto @@ -21,6 +21,12 @@ "data": { "pathName": "Left Bump Score To Depot" } + }, + { + "type": "path", + "data": { + "pathName": "Depot To Score" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Right Middy.auto b/src/main/deploy/pathplanner/autos/Right Middy.auto index 457285aa..b1b5803c 100644 --- a/src/main/deploy/pathplanner/autos/Right Middy.auto +++ b/src/main/deploy/pathplanner/autos/Right Middy.auto @@ -21,6 +21,12 @@ "data": { "pathName": "Right Bump Score To Depot" } + }, + { + "type": "path", + "data": { + "pathName": "Depot To Score" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/Depot To Score.path b/src/main/deploy/pathplanner/paths/Depot To Score.path new file mode 100644 index 00000000..21e6fd17 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot To Score.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.5795133587786261, + "y": 5.959713740458016 + }, + "prevControl": null, + "nextControl": { + "x": 1.579513358778626, + "y": 5.959713740458016 + }, + "isLocked": false, + "linkedName": "Depot" + }, + { + "anchor": { + "x": 2.7074970344009497, + "y": 5.959713740458016 + }, + "prevControl": { + "x": 1.7074970344009497, + "y": 5.959713740458016 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot Out" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.14848143982002254, + "maxWaypointRelativePos": 0.9448818897637792, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.5, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.75, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.5, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path index 1e3ccf68..71699467 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path @@ -47,19 +47,6 @@ "nominalVoltage": 12.5, "unlimited": false } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.75, - "maxAcceleration": 4.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - } } ], "pointTowardsZones": [], diff --git a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path index a0393557..0ef89ef7 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path @@ -56,26 +56,13 @@ "minWaypointRelativePos": 0, "maxWaypointRelativePos": 1.65, "constraints": { - "maxVelocity": 0.5, + "maxVelocity": 0.75, "maxAcceleration": 1.0, "maxAngularVelocity": 50.0, "maxAngularAcceleration": 100.0, "nominalVoltage": 12.5, "unlimited": false } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.65, - "maxWaypointRelativePos": 2.0, - "constraints": { - "maxVelocity": 1.75, - "maxAcceleration": 4.0, - "maxAngularVelocity": 50.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.5, - "unlimited": false - } } ], "pointTowardsZones": [], diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c8eee889..ea15fada 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -356,10 +356,10 @@ public void configureAutons() { // DEPOT AutonConfig LEFT_MIDDY = new AutonConfig("Left Middy", LeftMiddy::new, - "Left Bump To NZ", "Left Middy To Bump Score", "Left Bump Score To Depot"); + "Left Bump To NZ", "Left Middy To Bump Score", "Left Bump Score To Depot", "Depot To Score"); LEFT_MIDDY.register(autonChooser); AutonConfig RIGHT_MIDDY = new AutonConfig("Right Middy", RightMiddy::new, - "Right Bump To NZ", "Right Middy To Bump Score", "Right Bump Score To Depot"); + "Right Bump To NZ", "Right Middy To Bump Score", "Right Bump Score To Depot", "Depot To Score"); RIGHT_MIDDY.register(autonChooser); // TWO CYCLES (TRENCH) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java index cd9cadc9..b6fe1713 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java @@ -2,8 +2,11 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; @@ -36,11 +39,22 @@ public LeftMiddy(PathPlannerPath... paths) { // SOTM To Depot new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new HandoffRun().alongWith(new SpindexerRun()), new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffRun().andThen( - new SpindexerRun() - ) + new WaitCommand(3.0).andThen( + new IntakeAutoDigest().repeatedly().withTimeout(2.0).andThen(new IntakeDeploy()) + ), + new WaitCommand(5.0).andThen( + new HandoffStop().alongWith(new SpindexerStop()) + ) + ), + + // Off Depot + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new WaitCommand(1.0).andThen(new HandoffRun().alongWith(new SpindexerRun())), + new WaitCommand(2.5).andThen(new IntakeAutoDigest().repeatedly()) ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java index 73477f4b..f61c6f3d 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java @@ -2,8 +2,11 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; @@ -36,11 +39,22 @@ public RightMiddy(PathPlannerPath... paths) { // SOTM To Depot new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new HandoffRun().alongWith(new SpindexerRun()), new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffRun().andThen( - new SpindexerRun() - ) + new WaitCommand(3.0).andThen( + new IntakeAutoDigest().repeatedly().withTimeout(3.0).andThen(new IntakeDeploy()) + ), + new WaitCommand(6.0).andThen( + new HandoffStop().alongWith(new SpindexerStop()) + ) + ), + + // Off Depot + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new WaitCommand(1.0).andThen(new HandoffRun().alongWith(new SpindexerRun())), + new WaitCommand(2.5).andThen(new IntakeAutoDigest().repeatedly()) ) ); From 6fbfc33a600c278d3ae919f19376416cc83d8c6a Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 9 Apr 2026 19:38:43 -0400 Subject: [PATCH 37/58] SOMEBODY forgot to push at the end of Gotham --- elastic-layout.json | 203 +++++++++++++----- .../pathplanner/paths/Depot To Score.path | 8 +- .../paths/Left Bump Score To Depot.path | 8 +- .../pathplanner/paths/Left NZ To Score.path | 8 +- .../paths/Left Score To NZ (F).path | 8 +- .../paths/Left Score To Score.path | 24 +-- .../pathplanner/paths/Left Trench To NZ.path | 8 +- .../pathplanner/paths/Right NZ To Score.path | 28 +-- .../paths/Right Score To NZ (F).path | 8 +- .../paths/Right Score To Score.path | 14 +- .../pathplanner/paths/Right Trench To NZ.path | 12 +- .../com/stuypulse/robot/RobotContainer.java | 8 +- .../stuypulse/robot/constants/Settings.java | 2 + .../swerve/CommandSwerveDrivetrain.java | 10 +- 14 files changed, 216 insertions(+), 133 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 1c569174..36122ead 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -70,93 +70,93 @@ } }, { - "title": "Swerve", + "title": "Shooter", "x": 0.0, "y": 0.0, "width": 166.0, "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Swerve Is Enabled", + "topic": "SmartDashboard/Enabled Subsystems/Shooter Is Enabled", "period": 0.06, "data_type": "boolean" } }, { - "title": "Shooter", + "title": "Turret", "x": 0.0, "y": 0.0, "width": 166.0, "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Shooter Is Enabled", + "topic": "SmartDashboard/Enabled Subsystems/Turret Is Enabled", "period": 0.06, "data_type": "boolean" } }, { - "title": "Turret", + "title": "Back Limelight", "x": 0.0, "y": 0.0, "width": 166.0, "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Turret Is Enabled", + "topic": "SmartDashboard/Enabled Subsystems/Back Limelight Is Enabled", "period": 0.06, "data_type": "boolean" } }, { - "title": "Back Limelight", + "title": "Right Limelight", "x": 0.0, "y": 0.0, "width": 166.0, "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Back Limelight Is Enabled", + "topic": "SmartDashboard/Enabled Subsystems/Right Limelight Is Enabled", "period": 0.06, "data_type": "boolean" } }, { - "title": "Left Limelight", + "title": "LEDs Is Enabled", "x": 0.0, "y": 0.0, - "width": 166.0, - "height": 166.0, + "width": 140.0, + "height": 140.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Left Limelight Is Enabled", - "period": 0.06, + "topic": "SmartDashboard/Enabled Subsystems/LEDs Is Enabled", + "period": 0.5, "data_type": "boolean" } }, { - "title": "Right Limelight", + "title": "Swerve", "x": 0.0, "y": 0.0, "width": 166.0, "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Right Limelight Is Enabled", + "topic": "SmartDashboard/Enabled Subsystems/Swerve Is Enabled", "period": 0.06, "data_type": "boolean" } }, { - "title": "LEDs Is Enabled", + "title": "Left Limelight", "x": 0.0, "y": 0.0, - "width": 140.0, - "height": 140.0, + "width": 166.0, + "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/LEDs Is Enabled", - "period": 0.5, + "topic": "SmartDashboard/Enabled Subsystems/Left Limelight Is Enabled", + "period": 0.06, "data_type": "boolean" } } @@ -903,10 +903,10 @@ }, { "title": "Handoff Reverse", - "x": 1120.0, + "x": 560.0, "y": 420.0, "width": 280.0, - "height": 280.0, + "height": 140.0, "type": "Command", "properties": { "topic": "/SmartDashboard/Robot/Handoff Reverse", @@ -917,10 +917,10 @@ }, { "title": "Intake Reverse", - "x": 560.0, + "x": 840.0, "y": 420.0, "width": 280.0, - "height": 280.0, + "height": 140.0, "type": "Command", "properties": { "topic": "/SmartDashboard/Robot/Intake Reverse", @@ -931,10 +931,10 @@ }, { "title": "Spindexer Reverse", - "x": 840.0, + "x": 1120.0, "y": 420.0, "width": 280.0, - "height": 280.0, + "height": 140.0, "type": "Command", "properties": { "topic": "/SmartDashboard/Robot/Spindexer Reverse", @@ -976,7 +976,7 @@ }, { "title": "Current Intake Angle", - "x": 910.0, + "x": 560.0, "y": 700.0, "width": 210.0, "height": 140.0, @@ -1006,7 +1006,7 @@ }, { "title": "Current Hood Angle", - "x": 280.0, + "x": 1050.0, "y": 700.0, "width": 210.0, "height": 140.0, @@ -1020,7 +1020,7 @@ }, { "title": "Hood Homing Routine Lower", - "x": 0.0, + "x": 770.0, "y": 700.0, "width": 280.0, "height": 140.0, @@ -1034,7 +1034,7 @@ }, { "title": "Seed Intake At Upper Limit", - "x": 630.0, + "x": 280.0, "y": 700.0, "width": 280.0, "height": 140.0, @@ -1048,7 +1048,7 @@ }, { "title": "Seed Intake At Lower Limit", - "x": 1120.0, + "x": 0.0, "y": 700.0, "width": 280.0, "height": 140.0, @@ -1073,6 +1073,47 @@ "data_type": "string", "show_submit_button": false } + }, + { + "title": "Enable Distance Check?", + "x": 560.0, + "y": 560.0, + "width": 140.0, + "height": 140.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Robot/Enable Distance Check?", + "period": 0.5, + "data_type": "boolean" + } + }, + { + "title": "Set Robot Pose Right Corner", + "x": 1120.0, + "y": 560.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Set Robot Pose Right Corner", + "period": 0.5, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "Set Robot Pose Left Corner", + "x": 840.0, + "y": 560.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Set Robot Pose Left Corner", + "period": 0.5, + "show_type": true, + "maximize_button_space": false + } } ] } @@ -1451,7 +1492,7 @@ "title": "Current Turret Angle", "x": 980.0, "y": 560.0, - "width": 210.0, + "width": 280.0, "height": 140.0, "type": "Text Display", "properties": { @@ -1490,84 +1531,140 @@ } }, { - "title": "Set Back LL PF", + "title": "Current Intake Angle", + "x": 770.0, + "y": 560.0, + "width": 210.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/Current Angle (deg)", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Current Hood Angle", + "x": 0.0, + "y": 560.0, + "width": 210.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Seed Hood Relative Encoder At Lower Hardstop", + "x": 0.0, + "y": 420.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Seed Hood Relative Encoder At Lower Hardstop", + "period": 0.5, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "Set Exposure 25", "x": 0.0, "y": 700.0, "width": 280.0, "height": 140.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Robot/Set Back LL PF", + "topic": "/SmartDashboard/Robot/Set Exposure 25", "period": 0.5, "show_type": true, "maximize_button_space": false } }, { - "title": "Set Left LL PF", + "title": "Set Exposure 80", "x": 280.0, "y": 700.0, "width": 280.0, "height": 140.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Robot/Set Left LL PF", + "topic": "/SmartDashboard/Robot/Set Exposure 80", "period": 0.5, "show_type": true, "maximize_button_space": false } }, { - "title": "Set Right LL PF", + "title": "Set Exposure 100", "x": 560.0, "y": 700.0, "width": 280.0, "height": 140.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Robot/Set Right LL PF", + "topic": "/SmartDashboard/Robot/Set Exposure 100", "period": 0.5, "show_type": true, "maximize_button_space": false } }, { - "title": "Seed Hood Relative Encoder At Lower Hardstop", - "x": 0.0, - "y": 420.0, + "title": "Set Exposure 130", + "x": 840.0, + "y": 700.0, "width": 280.0, "height": 140.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Robot/Seed Hood Relative Encoder At Lower Hardstop", + "topic": "/SmartDashboard/Robot/Set Exposure 130", "period": 0.5, "show_type": true, "maximize_button_space": false } }, { - "title": "Current Intake Angle", - "x": 420.0, + "title": "Pipeline back", + "x": 1260.0, "y": 560.0, - "width": 210.0, - "height": 140.0, + "width": 140.0, + "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Current Angle (deg)", + "topic": "/limelight-back/getpipe", "period": 0.5, "data_type": "double", "show_submit_button": false } }, { - "title": "Current Hood Angle", - "x": 0.0, - "y": 560.0, - "width": 210.0, - "height": 140.0, + "title": "Pipeline right", + "x": 1260.0, + "y": 490.0, + "width": 140.0, + "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", + "topic": "/limelight-right/getpipe", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pipeline Left", + "x": 1260.0, + "y": 420.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/limelight-left/getpipe", "period": 0.5, "data_type": "double", "show_submit_button": false diff --git a/src/main/deploy/pathplanner/paths/Depot To Score.path b/src/main/deploy/pathplanner/paths/Depot To Score.path index 21e6fd17..195d09f8 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Score.path +++ b/src/main/deploy/pathplanner/paths/Depot To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.5795133587786261, - "y": 5.959713740458016 + "x": 7.864855907780978, + "y": 3.3946109510086457 }, "prevControl": null, "nextControl": { - "x": 1.579513358778626, - "y": 5.959713740458016 + "x": 4.936313984109829, + "y": 2.6725478247849654 }, "isLocked": false, "linkedName": "Depot" diff --git a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path index 71699467..e2937f3d 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.4749522900763368, - "y": 5.507824427480916 + "x": 7.868901569186875, + "y": 4.675463623395149 }, "prevControl": null, "nextControl": { - "x": 1.6506583969465654, - "y": 5.959713740458016 + "x": 5.114686204930087, + "y": 5.231779917860097 }, "isLocked": false, "linkedName": "Left Bump Score" diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 94130117..692049f8 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.308815977175463, - "y": 4.74015691868759 + "x": 7.868901569186875, + "y": 4.675463623395149 }, "prevControl": null, "nextControl": { - "x": 5.82645038167939, - "y": 4.687729007633589 + "x": 4.142567760342367, + "y": 5.82700427960057 }, "isLocked": false, "linkedName": "Left NZ" diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index e7ae2b7e..e29f19c7 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.308815977175463, - "y": 4.74015691868759 + "x": 7.868901569186875, + "y": 4.675463623395149 }, "prevControl": { - "x": 7.920656205420826, - "y": 7.314950071326677 + "x": 7.480741797432238, + "y": 7.250256776034236 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index 32dcb9a8..d1d904d4 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -32,32 +32,32 @@ }, { "anchor": { - "x": 6.769115549215405, - "y": 4.481383737517832 + "x": 7.185259365994238, + "y": 4.518559077809798 }, "prevControl": { - "x": 6.398810472352867, - "y": 4.205129596104213 + "x": 6.8149542891317, + "y": 4.24230493639618 }, "nextControl": { - "x": 7.410830063131713, - "y": 4.960114122681885 + "x": 7.826973879910546, + "y": 4.997289462973852 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.769115549215405, - "y": 7.392582025677603 + "x": 7.185259365994238, + "y": 7.366704707560627 }, "prevControl": { - "x": 7.066884105261373, - "y": 7.384797226826728 + "x": 7.483027922040206, + "y": 7.358919908709752 }, "nextControl": { - "x": 4.789500713266761, - "y": 7.444336661911555 + "x": 5.205644530045594, + "y": 7.418459343794579 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 84f1a734..aad4a35c 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.308815977175463, - "y": 4.74015691868759 + "x": 7.868901569186875, + "y": 4.675463623395149 }, "prevControl": { - "x": 8.261631679389314, - "y": 7.265171755725191 + "x": 7.817146932952923, + "y": 7.496091298145506 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index ebfdd5c6..72db843d 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.231184022824536, - "y": 3.3427817403708993 + "x": 7.864855907780978, + "y": 3.3946109510086457 }, "prevControl": null, "nextControl": { - "x": 6.0775, - "y": 3.24837786259542 + "x": 4.63019114315901, + "y": 2.463027498797518 }, "isLocked": false, "linkedName": "Right NZ" @@ -20,24 +20,8 @@ "y": 1.5496087786259545 }, "prevControl": { - "x": 6.211393129770993, - "y": 3.0810114503816792 - }, - "nextControl": { - "x": 6.486333025849947, - "y": 0.28578917357895817 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.633950381679389, - "y": 0.5705152671755729 - }, - "prevControl": { - "x": 6.582732165608568, - "y": 0.5693751020897069 + "x": 9.162767475035661, + "y": 0.06930099857346683 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 9919ba30..c451216b 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.231184022824536, - "y": 3.3427817403708993 + "x": 7.864855907780978, + "y": 3.3946109510086457 }, "prevControl": { - "x": 8.179429386590582, - "y": 0.6127246790299576 + "x": 7.8131012715470245, + "y": 0.6645538896677041 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index cc4b998b..836f4b93 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 6.9502567760342355, + "x": 7.302881844380403, "y": 2.851112696148359 }, "prevControl": { - "x": 6.923667038863391, - "y": 3.8349329714696054 + "x": 7.215816731986725, + "y": 3.83143356936277 }, "nextControl": { - "x": 7.014950071326675, - "y": 0.45746077032810284 + "x": 7.525057636887608, + "y": 0.34949567723342856 }, "isLocked": false, "linkedName": null @@ -52,8 +52,8 @@ "y": 0.5705152671755729 }, "prevControl": { - "x": 7.4508548039333125, - "y": 0.47994465376615736 + "x": 7.694956772334294, + "y": 0.5324639769452448 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 2812295c..99f552e5 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 9.056622137404581, - "y": 0.44499045801526793 + "x": 9.06721902017291, + "y": 0.41484149855907726 }, "isLocked": false, "linkedName": "Right Trench Start" }, { "anchor": { - "x": 8.231184022824536, - "y": 3.3427817403708993 + "x": 7.864855907780978, + "y": 3.3946109510086457 }, "prevControl": { - "x": 8.286736641221374, - "y": 0.7713549618320608 + "x": 7.773371757925073, + "y": 0.4017723342939483 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index ea15fada..5f5b7861 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -323,10 +323,10 @@ private void configureElasticButtons() { SmartDashboard.putData("Robot/WL Tower Tags Right-Camera", new WhitelistTowerTags("limelight-right")); SmartDashboard.putData("Robot/Whitelist All Cameras", new WhitelistAllTagsForAllCameras()); - SmartDashboard.putData("Robot/Set Exposure 25", new SetPipeline(Pipeline.NO_SUN)); - SmartDashboard.putData("Robot/Set Exposure 80", new SetPipeline(Pipeline.LOW_SUN)); - SmartDashboard.putData("Robot/Set Exposure 100", new SetPipeline(Pipeline.MED_SUN)); - SmartDashboard.putData("Robot/Set Exposure 130", new SetPipeline(Pipeline.HIGH_SUN)); + SmartDashboard.putData("Robot/Set Pipeline No Sun", new SetPipeline(Pipeline.NO_SUN)); + SmartDashboard.putData("Robot/Set Pipeline Low Sun", new SetPipeline(Pipeline.LOW_SUN)); + SmartDashboard.putData("Robot/Set Pipeline Med Sun", new SetPipeline(Pipeline.MED_SUN)); + SmartDashboard.putData("Robot/Set Pipeline High Sun", new SetPipeline(Pipeline.HIGH_SUN)); // Unjamming SmartDashboard.putData("Robot/Handoff Reverse", diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b585438f..8b1c9d5c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -37,6 +37,8 @@ public interface Settings { public final SmartBoolean DEBUG_MODE = new SmartBoolean("Robot/DebugMode", true); public final CANBus CANIVORE = new CANBus("canivore", "./logs/example.hoot"); public final double LOOP_OVERRUN_WARNING_TIME_SEC = 1; + SmartBoolean ENABLE_DISTANCE_CHECK = new SmartBoolean("Robot/Enable Distance Check?", false); + SmartBoolean ENABLE_OUT_OF_FIELD_CHECK = new SmartBoolean("Robot/Enable out of field check", true); public interface Handoff { public final double GEAR_RATIO = 3.0 / 1.0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index b26265cd..14fcd227 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -362,7 +362,7 @@ private void startSimThread() { } private boolean checkIfVisionMeasurementValid(Pose2d visionPose) { - return visionPose.getTranslation().getDistance(getState().Pose.getTranslation()) < Settings.Swerve.MAX_ACCEPTABLE_VISION_DEVIATION_METERS; + return !Settings.ENABLE_DISTANCE_CHECK.get() || visionPose.getTranslation().getDistance(getState().Pose.getTranslation()) < Settings.Swerve.MAX_ACCEPTABLE_VISION_DEVIATION_METERS; } /** @@ -419,11 +419,11 @@ public Pose2d getPose() { double proposedX = proposedPose.getX(); double proposedY = proposedPose.getY(); double poseDelta = lastGoodPose.getTranslation().getDistance(proposedPose.getTranslation()); - - if(!(proposedX > Field.LENGTH || proposedX < 0 || proposedY > Field.WIDTH || proposedY < 0) && - poseDelta <= Settings.Swerve.MAX_ACCEPTABLE_POSE_DELTA_METERS) { + + if (!(proposedX > Field.LENGTH || proposedX < 0 || proposedY > Field.WIDTH || proposedY < 0) && + poseDelta <= Settings.Swerve.MAX_ACCEPTABLE_POSE_DELTA_METERS) { lastGoodPose = proposedPose; - } + } return lastGoodPose; } From eab5591885cf0ccbc1cfea51ea4354d8d5634a68 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Thu, 9 Apr 2026 19:50:38 -0400 Subject: [PATCH 38/58] FIX: auto pathing conflicts post COMPuter push --- .../pathplanner/paths/Depot To Score.path | 12 +++--- .../paths/Left Bump Score To Depot.path | 12 +++--- .../pathplanner/paths/Left NZ To Score.path | 22 +++++----- .../paths/Left Score To NZ (F).path | 8 ++-- .../paths/Left Score To Score.path | 4 +- .../pathplanner/paths/Left Trench To NZ.path | 12 +++--- .../pathplanner/paths/Right NZ To Score.path | 42 +++++++++++++------ .../paths/Right Score To NZ (F).path | 8 ++-- .../paths/Right Score To Score.path | 10 ++--- .../pathplanner/paths/Right Trench To NZ.path | 8 ++-- 10 files changed, 77 insertions(+), 61 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Depot To Score.path b/src/main/deploy/pathplanner/paths/Depot To Score.path index 195d09f8..44d6a43f 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Score.path +++ b/src/main/deploy/pathplanner/paths/Depot To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.864855907780978, - "y": 3.3946109510086457 + "x": 0.5795133587786261, + "y": 5.959713740458016 }, "prevControl": null, "nextControl": { - "x": 4.936313984109829, - "y": 2.6725478247849654 + "x": 1.617398935069807, + "y": 5.933846094852308 }, "isLocked": false, "linkedName": "Depot" @@ -20,8 +20,8 @@ "y": 5.959713740458016 }, "prevControl": { - "x": 1.7074970344009497, - "y": 5.959713740458016 + "x": 1.856554697887364, + "y": 5.954351642472394 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path index e2937f3d..7bb6954b 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.868901569186875, - "y": 4.675463623395149 + "x": 3.4749522900763368, + "y": 5.507824427480916 }, "prevControl": null, "nextControl": { - "x": 5.114686204930087, - "y": 5.231779917860097 + "x": 0.7207369258195486, + "y": 6.064140721945863 }, "isLocked": false, "linkedName": "Left Bump Score" @@ -30,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.6590257879656263, + "waypointRelativePos": 0.42984014209591426, "rotationDegrees": 180.0 } ], @@ -38,7 +38,7 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 0.5016872890888694, "constraints": { "maxVelocity": 0.5, "maxAcceleration": 0.75, diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 692049f8..800f2a1d 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.868901569186875, - "y": 4.675463623395149 + "x": 8.248481613285884, + "y": 4.621376037959667 }, "prevControl": null, "nextControl": { - "x": 4.142567760342367, - "y": 5.82700427960057 + "x": 5.7738671411625155, + "y": 4.589098457888493 }, "isLocked": false, "linkedName": "Left NZ" @@ -20,12 +20,12 @@ "y": 6.378129770992366 }, "prevControl": { - "x": 6.061945145745694, - "y": 4.945613634328742 + "x": 6.075124555160143, + "y": 5.761850533807828 }, "nextControl": { - "x": 6.044026717557253, - "y": 7.63337786259542 + "x": 6.006129596458399, + "y": 7.632552869140023 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 7.440906488549619 }, "prevControl": { - "x": 6.504284351145039, - "y": 7.524589694656489 + "x": 5.8276631079478065, + "y": 7.504839857651245 }, "nextControl": null, "isLocked": false, @@ -60,7 +60,7 @@ "minWaypointRelativePos": 1.65, "maxWaypointRelativePos": 2.0, "constraints": { - "maxVelocity": 2.5, + "maxVelocity": 3.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index e29f19c7..c525192a 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.868901569186875, - "y": 4.675463623395149 + "x": 8.248481613285884, + "y": 4.621376037959667 }, "prevControl": { - "x": 7.480741797432238, - "y": 7.250256776034236 + "x": 7.860321841531247, + "y": 7.196169190598754 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index d1d904d4..71756025 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.354840458018504, - "y": 7.567584266333281 + "x": 6.839027283511269, + "y": 7.472562277580071 }, "isLocked": false, "linkedName": "Left Trench Score" diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index aad4a35c..333b8916 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 8.86415076335878, - "y": 7.700324427480917 + "x": 8.517461447212337, + "y": 7.70926453143535 }, "isLocked": false, "linkedName": "Left Trench Start" }, { "anchor": { - "x": 7.868901569186875, - "y": 4.675463623395149 + "x": 8.248481613285884, + "y": 4.621376037959667 }, "prevControl": { - "x": 7.817146932952923, - "y": 7.496091298145506 + "x": 8.196726977051933, + "y": 7.442003712710024 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 72db843d..34242f5c 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 7.864855907780978, - "y": 3.3946109510086457 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": null, "nextControl": { - "x": 4.63019114315901, - "y": 2.463027498797518 + "x": 5.763107947805457, + "y": 3.5131791221826814 }, "isLocked": false, "linkedName": "Right NZ" }, { "anchor": { - "x": 6.362022900763359, - "y": 1.5496087786259545 + "x": 6.053851272542522, + "y": 1.6151808747904899 }, "prevControl": { - "x": 9.162767475035661, - "y": 0.06930099857346683 + "x": 6.053606168446026, + "y": 2.684721233689205 + }, + "nextControl": { + "x": 6.054109349362734, + "y": 0.48903252043804946 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.633950381679389, + "y": 0.5705152671755729 + }, + "prevControl": { + "x": 6.225753262158957, + "y": 0.5974377224199281 }, "nextControl": null, "isLocked": false, @@ -30,21 +46,21 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.4183381088825223, + "waypointRelativePos": 0.23445825932504563, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 1.6, + "waypointRelativePos": 1.6376554174067646, "rotationDegrees": 0.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 1.7, - "maxWaypointRelativePos": 2.0, + "minWaypointRelativePos": 2.7, + "maxWaypointRelativePos": 3.0, "constraints": { - "maxVelocity": 2.5, + "maxVelocity": 3.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index c451216b..db887837 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.864855907780978, - "y": 3.3946109510086457 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": { - "x": 7.8131012715470245, - "y": 0.6645538896677041 + "x": 8.185967783694872, + "y": 0.697048513985274 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 836f4b93..6ee888b9 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.847385496183207, - "y": 0.5119370229007632 + "x": 6.9143416370106765, + "y": 0.6942704626334513 }, "isLocked": false, "linkedName": "Right Trench Score" @@ -52,7 +52,7 @@ "y": 0.5705152671755729 }, "prevControl": { - "x": 7.694956772334294, + "x": 7.694956772334293, "y": 0.5324639769452448 }, "nextControl": null, @@ -62,11 +62,11 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.23027718550106568, + "waypointRelativePos": 0.17051509769094172, "rotationDegrees": 0.0 }, { - "waypointRelativePos": 0.5329512893982814, + "waypointRelativePos": 0.644760213143872, "rotationDegrees": 90.0 }, { diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 99f552e5..f83eeb16 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.864855907780978, - "y": 3.3946109510086457 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": { - "x": 7.773371757925073, - "y": 0.4017723342939483 + "x": 8.14623827007292, + "y": 0.4342669586115182 }, "nextControl": null, "isLocked": false, From b4682fdeadcfab55d3a6620c20b55c391de5d14f Mon Sep 17 00:00:00 2001 From: Daniel M Date: Fri, 10 Apr 2026 00:59:00 -0400 Subject: [PATCH 39/58] feat: target area filtering, default buffer pose is now the middle of the field instead of origin --- .../java/com/stuypulse/robot/constants/Settings.java | 1 + .../subsystems/swerve/CommandSwerveDrivetrain.java | 2 +- .../robot/subsystems/vision/LimelightVision.java | 10 +++++++--- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 8b1c9d5c..c170c8d1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -384,6 +384,7 @@ public interface Vision { public final Translation2d INVALID_POSITION = new Translation2d(8.2705, 4.0345); public final double INVALID_POSITION_TOLERANCE_M = 0.05; public final double MAX_ANGULAR_VELOCITY_RAD_SEC = 2 * Math.PI; + double MIN_TAG_AREA = 5; //TODO: MAKE SURE THIS IS A GOOD VALUE!!! public final double BUZZ_DEBOUNCE = 0.25; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 14fcd227..f3e8b6cc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -100,7 +100,7 @@ public static CommandSwerveDrivetrain getInstance() { private Notifier m_simNotifier = null; private double m_lastSimTime; - private Pose2d lastGoodPose = Pose2d.kZero; + private Pose2d lastGoodPose = new Pose2d(Field.LENGTH / 2.0, Field.WIDTH / 2.0, Rotation2d.kZero); /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_moduleTranslationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 71a179fe..e6fb590f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -21,7 +21,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -256,8 +255,9 @@ public void periodicAfterScheduler() { boolean notNull = false; boolean withinAngularVelocityTolerance = false; boolean withinInvalidPositionTolerance = false; + boolean withinTargetAreaTolerance = false; - if (poseEstimate != null && poseEstimate.tagCount > 0 ) { + if (poseEstimate != null && poseEstimate.tagCount > 0) { notNull = true; if (poseEstimate.pose.getTranslation().getDistance(Settings.Vision.INVALID_POSITION) < Settings.Vision.INVALID_POSITION_TOLERANCE_M){ @@ -268,10 +268,14 @@ public void periodicAfterScheduler() { withinAngularVelocityTolerance = true; } + if(poseEstimate.avgTagArea >= Settings.Vision.MIN_TAG_AREA) { + withinTargetAreaTolerance = true; + } + Pose2d robotPose = poseEstimate.pose; double timestamp = poseEstimate.timestampSeconds; - boolean isAcceptablePose = notNull && withinAngularVelocityTolerance && !withinInvalidPositionTolerance; + boolean isAcceptablePose = notNull && withinAngularVelocityTolerance && !withinInvalidPositionTolerance && withinTargetAreaTolerance; if (megaTagMode == MegaTagMode.MEGATAG1 && isAcceptablePose) { CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT1_STDEVS); From c50e34700ae8be5a21bedb7d973c279e9cf57298 Mon Sep 17 00:00:00 2001 From: Daniel M Date: Fri, 10 Apr 2026 01:05:40 -0400 Subject: [PATCH 40/58] fix: initial last good pose is now 1.5, 1.5 --- .../robot/subsystems/swerve/CommandSwerveDrivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index f3e8b6cc..c9525e85 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -100,7 +100,7 @@ public static CommandSwerveDrivetrain getInstance() { private Notifier m_simNotifier = null; private double m_lastSimTime; - private Pose2d lastGoodPose = new Pose2d(Field.LENGTH / 2.0, Field.WIDTH / 2.0, Rotation2d.kZero); + private Pose2d lastGoodPose = new Pose2d(1.5, 1.5, Rotation2d.kZero); /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_moduleTranslationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); From 073947e8ec0293ff36b69df87b9bb4275724afb0 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 10 Apr 2026 08:59:28 -0400 Subject: [PATCH 41/58] feat: reset pose on manual shots, add 75 rpm to kb shot --- .../java/com/stuypulse/robot/RobotContainer.java | 4 ++++ .../robot/commands/swerve/SwerveResetPoseKBShot.java | 12 ++++++++++++ .../java/com/stuypulse/robot/constants/Settings.java | 2 +- .../robot/subsystems/intake/IntakeImpl.java | 2 ++ 4 files changed, 19 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5f5b7861..d5a29c30 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -42,6 +42,7 @@ import com.stuypulse.robot.commands.swerve.SwerveDriveSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveResetPoseKBShot; import com.stuypulse.robot.commands.swerve.SwerveResetPoseLeftCorner; import com.stuypulse.robot.commands.swerve.SwerveResetPoseRightCorner; import com.stuypulse.robot.commands.swerve.SwerveXMode; @@ -265,6 +266,7 @@ private void configureButtonBindings() { new SuperstructureLeftCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()) .andThen(new SpindexerRun()), + new SwerveResetPoseLeftCorner(), new SwerveXMode() ) ) @@ -275,6 +277,7 @@ private void configureButtonBindings() { .whileTrue(new LEDApplyPattern(Settings.LED.RIGHT_CORNER)) .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) + .onTrue(new SwerveResetPoseRightCorner()) .whileTrue(new SuperstructureRightCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD) .andThen(new SpindexerRun()))) @@ -285,6 +288,7 @@ private void configureButtonBindings() { .whileTrue(new LEDApplyPattern(Settings.LED.KB_DISTANCE)) .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) + .onTrue(new SwerveResetPoseKBShot()) .whileTrue(new SuperstructureKB().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD) .andThen(new SpindexerRun()))) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java new file mode 100644 index 00000000..070fff4a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java @@ -0,0 +1,12 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; + +public class SwerveResetPoseKBShot extends SwerveResetPose { + public SwerveResetPoseKBShot() { + super(new Pose2d(0, Field.WIDTH / 2, CommandSwerveDrivetrain.getInstance().getPose().getRotation())); //TODO: FIELD CAL GET X + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index c170c8d1..7132ea07 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -180,7 +180,7 @@ public interface RPM { public final SmartNumber MANUAL_OVERRIDE = new SmartNumber("InterpolationTesting/Shoot State Target RPM", 3500.0); public final double REVERSE = 0.0; - public final double KB = 2600.0; + public final double KB = 2675.0; public final double LEFT_CORNER = 3650.0; public final double RIGHT_CORNER = 3650.0; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 6e84cf98..ec9ccf42 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -30,6 +30,7 @@ import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.GravityTypeValue; @@ -187,6 +188,7 @@ public void periodicAfterScheduler() { getPivotAngle().getDegrees() <= Settings.Intake.ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE.getDegrees() && rollerState != RollerState.STOP) { pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts + // pivot.setControl(new TorqueCurrentFOC(Settings.Intake.PUSHDOWN_CURRENT)); applyingPushdownVoltage = true; } else if (pivotState == PivotState.HOMING) { pivot.setControl(new VoltageOut(-Settings.Intake.HOMING_VOLTAGE)); From 2a125759b9c01f875bb0973fcc043355b48ed9fc Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 10 Apr 2026 09:15:57 -0400 Subject: [PATCH 42/58] feat: rejection counters --- .../stuypulse/robot/constants/Cameras.java | 40 ++++++++++++++++++- .../subsystems/vision/LimelightVision.java | 13 ++++++ 2 files changed, 51 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index ba29fc41..9a2502ee 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -5,13 +5,13 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import com.stuypulse.stuylib.network.SmartBoolean; - import com.stuypulse.robot.RobotContainer; +import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** This interface stores information about each camera. */ public interface Cameras { @@ -40,6 +40,42 @@ public static class Camera { private Pose3d location; private SmartBoolean isEnabled; + private int rejectedCounterNotNull; + private int rejectedCounterAngularVelocity; + private int rejectedCounterInvalidPosition; + private int rejectedCounterTargetArea; + + public enum RejectionValue { + NOT_NULL, + ANGULAR_VELOCITY, + INVALID_POSITION, + TARGET_AREA + }; + + public void incrementRejection(RejectionValue rejectionValue) { + switch (rejectionValue) { + case NOT_NULL: + rejectedCounterNotNull++; + break; + case ANGULAR_VELOCITY: + rejectedCounterAngularVelocity++; + break; + case INVALID_POSITION: + rejectedCounterInvalidPosition++; + break; + case TARGET_AREA: + rejectedCounterTargetArea++; + break; + } + } + + public void logRejections() { + SmartDashboard.putNumber("Vision/" + name + "/# Rejected Not Null", rejectedCounterNotNull); + SmartDashboard.putNumber("Vision/" + name + "/# Rejected Target Area", rejectedCounterTargetArea); + SmartDashboard.putNumber("Vision/" + name + "/# Rejected Angular Velocity", rejectedCounterAngularVelocity); + SmartDashboard.putNumber("Vision/" + name + "/# Rejected Invalid Position", rejectedCounterInvalidPosition); + } + public Camera(String name, Pose3d location, SmartBoolean isEnabled) { this.name = name; this.location = location; diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index e6fb590f..337b2a6d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -5,12 +5,14 @@ /** ************************************************************ */ package com.stuypulse.robot.subsystems.vision; +import java.nio.channels.Pipe; import java.util.Arrays; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Cameras.Camera.RejectionValue; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.robot.util.vision.LimelightHelpers.IMUData; @@ -54,6 +56,7 @@ public static LimelightVision getInstance() { private BStream debouncedHasData; private Pipeline currentPipeline; + private final Pipeline[] HDRPipelines = {Pipeline.NO_SUN, Pipeline.HIGH_SUN}; public enum MegaTagMode { MEGATAG1, @@ -262,14 +265,20 @@ public void periodicAfterScheduler() { if (poseEstimate.pose.getTranslation().getDistance(Settings.Vision.INVALID_POSITION) < Settings.Vision.INVALID_POSITION_TOLERANCE_M){ withinInvalidPositionTolerance = true; + } else { + Cameras.LimelightCameras[i].incrementRejection(RejectionValue.INVALID_POSITION); } if (CommandSwerveDrivetrain.getInstance().getChassisSpeeds().omegaRadiansPerSecond < Settings.Vision.MAX_ANGULAR_VELOCITY_RAD_SEC) { withinAngularVelocityTolerance = true; + } else { + Cameras.LimelightCameras[i].incrementRejection(RejectionValue.ANGULAR_VELOCITY); } if(poseEstimate.avgTagArea >= Settings.Vision.MIN_TAG_AREA) { withinTargetAreaTolerance = true; + } else { + Cameras.LimelightCameras[i].incrementRejection(RejectionValue.TARGET_AREA); } Pose2d robotPose = poseEstimate.pose; @@ -309,6 +318,7 @@ public void periodicAfterScheduler() { } else { SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); + Cameras.LimelightCameras[i].incrementRejection(RejectionValue.NOT_NULL); } SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); @@ -316,6 +326,9 @@ public void periodicAfterScheduler() { SmartDashboard.putNumber("Vision/Limelight Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); // this is just the yaw of the internal imu SmartDashboard.putNumber("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); + + //Rejection counters + Cameras.LimelightCameras[i].logRejections(); } if (Settings.DEBUG_MODE.get()) { From 0b5e2c26c081f970de02bb75c345a288ae5921a8 Mon Sep 17 00:00:00 2001 From: Apetrock Date: Fri, 10 Apr 2026 09:39:06 -0400 Subject: [PATCH 43/58] FEAT: doglog spindexer non elastic values and update vendor deps --- .../subsystems/spindexer/SpindexerImpl.java | 23 +- vendordeps/DogLog.json | 20 ++ vendordeps/PathplannerLib-2026.1.2.json | 38 +++ vendordeps/PathplannerLib.json | 36 --- vendordeps/REVLib.json | 262 +++++++++--------- vendordeps/StudicaLib.json | 145 +++++----- vendordeps/photonlib-v2026.0.1-beta.json | 71 ----- 7 files changed, 279 insertions(+), 316 deletions(-) create mode 100644 vendordeps/DogLog.json create mode 100644 vendordeps/PathplannerLib-2026.1.2.json delete mode 100644 vendordeps/PathplannerLib.json delete mode 100644 vendordeps/photonlib-v2026.0.1-beta.json diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index 36ca82c1..eb941387 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -29,6 +29,7 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import dev.doglog.DogLog; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -125,13 +126,13 @@ public boolean shouldStop() { boolean turretLaggingSOTM = !superstructure.isTurretAtTolerance() && superstructureState == SuperstructureState.SOTM; - SmartDashboard.putBoolean("Spindexer/Should Stop/turret lagging sotm", turretLaggingSOTM); - SmartDashboard.putBoolean("Spindexer/Should Stop/is Behind Hub While Ferrying?", isBehindHubWhileFerrying); - SmartDashboard.putBoolean("Spindexer/Should Stop/is Turret Wrapping?", isTurretWrapping); - SmartDashboard.putBoolean("Spindexer/Should Stop/is Outside Alliance zone?", isOutsideAllianceZone); - SmartDashboard.putBoolean("Spindexer/Should Stop/is Under Trenche?", isUnderTrench); - SmartDashboard.putBoolean("Spindexer/Should Stop/turret lagging sotm", turretLaggingSOTM); - SmartDashboard.putBoolean("Spindexer/Should Stop/inManualState", inManualState); + DogLog.log("Spindexer/Should Stop/turret lagging sotm", turretLaggingSOTM); + DogLog.log("Spindexer/Should Stop/is Behind Hub While Ferrying?", isBehindHubWhileFerrying); + DogLog.log("Spindexer/Should Stop/is Turret Wrapping?", isTurretWrapping); + DogLog.log("Spindexer/Should Stop/is Outside Alliance zone?", isOutsideAllianceZone); + DogLog.log("Spindexer/Should Stop/is Under Trenche?", isUnderTrench); + DogLog.log("Spindexer/Should Stop/turret lagging sotm", turretLaggingSOTM); + DogLog.log("Spindexer/Should Stop/inManualState", inManualState); return isStopState || isTurretWrapping || (isBehindHubWhileFerrying && !inManualState) || @@ -179,12 +180,12 @@ public void periodicAfterScheduler() { leaderMotor.stopMotor(); } - SmartDashboard.putNumber("Spindexer/Leader Motor RPM", getMotorRPM()); + DogLog.log("Spindexer/Leader Motor RPM", getMotorRPM()); // SmartDashboard.putBoolean("Spindexer/Unjamming", unJamming); - SmartDashboard.putNumber("Spindexer/Leader Voltage (volts)", leaderMotorVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Spindexer/Leader Supply Current (amps)", leaderSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Spindexer/Leader Stator Current (amps)", leaderStatorCurrent.getValueAsDouble()); + DogLog.log("Spindexer/Leader Voltage (volts)", leaderMotorVoltage.getValueAsDouble()); + DogLog.log("Spindexer/Leader Supply Current (amps)", leaderSupplyCurrent.getValueAsDouble()); + DogLog.log("Spindexer/Leader Stator Current (amps)", leaderStatorCurrent.getValueAsDouble()); SmartDashboard.putBoolean("Spindexer/Should Stop?", shouldStop()); diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json new file mode 100644 index 00000000..7201ed05 --- /dev/null +++ b/vendordeps/DogLog.json @@ -0,0 +1,20 @@ +{ + "javaDependencies": [ + { + "groupId": "com.github.jonahsnider", + "artifactId": "doglog", + "version": "2026.5.0" + } + ], + "fileName": "DogLog.json", + "frcYear": "2026", + "jsonUrl": "https://doglog.dev/vendordep.json", + "name": "DogLog", + "jniDependencies": [], + "mavenUrls": [ + "https://jitpack.io" + ], + "cppDependencies": [], + "version": "2026.5.0", + "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2026.1.2.json b/vendordeps/PathplannerLib-2026.1.2.json new file mode 100644 index 00000000..f72fa41f --- /dev/null +++ b/vendordeps/PathplannerLib-2026.1.2.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2026.1.2.json", + "name": "PathplannerLib", + "version": "2026.1.2", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2026", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2026.1.2" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2026.1.2", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json deleted file mode 100644 index c018b776..00000000 --- a/vendordeps/PathplannerLib.json +++ /dev/null @@ -1,36 +0,0 @@ -{ - "fileName": "PathplannerLib.json", - "name": "PathplannerLib", - "version": "2026.1.1", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2026", - "mavenUrls": ["https://3015rangerrobotics.github.io/pathplannerlib/repo"], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2026.1.1" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2026.1.1", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index ce16d37c..c6b1b9e4 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,131 +1,133 @@ { - "fileName": "REVLib.json", - "name": "REVLib", - "version": "2026.0.0", - "frcYear": "2026", - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "mavenUrls": ["https://maven.revrobotics.com/"], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-java", - "version": "2026.0.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2026.0.0", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "RevLibBackendDriver", - "version": "2026.0.0", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.0", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-cpp", - "version": "2026.0.0", - "libName": "REVLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2026.0.0", - "libName": "REVLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "RevLibBackendDriver", - "version": "2026.0.0", - "libName": "BackendDriver", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.0", - "libName": "REVLibWpi", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] -} + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2026.0.5", + "frcYear": "2026", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2026.0.5" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2026.0.5", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.5", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.5", + "libName": "BackendDriver", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.5", + "libName": "REVLibWpi", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/StudicaLib.json b/vendordeps/StudicaLib.json index ecf9f156..52228bdc 100644 --- a/vendordeps/StudicaLib.json +++ b/vendordeps/StudicaLib.json @@ -1,69 +1,78 @@ { - "fileName": "StudicaLib.json", - "name": "StudicaLib", - "version": "2026.0.1-beta", - "frcYear": "2026", - "uuid": "963ef341-8abd-4981-a969-c8ae3f592e82", - "mavenUrls": ["https://dev.studica.com/maven/release/2026/"], - "jsonUrl": "https://dev.studica.com/maven/release/2026/json/StudicaLib-2026.0.1-beta.json", - "javaDependencies": [ - { - "groupId": "com.studica.frc", - "artifactId": "StudicaLib-java", - "version": "2026.0.1-beta" - } - ], - "jniDependencies": [ - { - "groupId": "com.studica.frc", - "artifactId": "StudicaLib-driver", - "version": "2026.0.1-beta", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.studica.frc", - "artifactId": "StudicaLib-cpp", - "version": "2026.0.1-beta", - "libName": "StudicaLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.studica.frc", - "artifactId": "StudicaLib-driver", - "version": "2026.0.1-beta", - "libName": "StudicaLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] -} + "fileName": "StudicaLib.json", + "name": "StudicaLib", + "version": "2026.0.2", + "frcYear": "2026", + "uuid": "963ef341-8abd-4981-a969-c8ae3f592e82", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2026/" + ], + "jsonUrl": "https://dev.studica.com/maven/release/2026/json/StudicaLib-2026.0.2.json", + "conflictsWith": [ + { + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "errorMessage": "StudicaLib (newer devices) is not compatible with Studica (NavX, NavX2)", + "offlineFileName": "Studica.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "StudicaLib-java", + "version": "2026.0.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "StudicaLib-driver", + "version": "2026.0.2", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "StudicaLib-cpp", + "version": "2026.0.2", + "libName": "StudicaLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.studica.frc", + "artifactId": "StudicaLib-driver", + "version": "2026.0.2", + "libName": "StudicaLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/photonlib-v2026.0.1-beta.json b/vendordeps/photonlib-v2026.0.1-beta.json deleted file mode 100644 index 8b1d42b8..00000000 --- a/vendordeps/photonlib-v2026.0.1-beta.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2026.0.1-beta", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2026", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2026.0.1-beta", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2026.0.1-beta", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2026.0.1-beta", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2026.0.1-beta" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2026.0.1-beta" - } - ] -} From a23b200ca489877b4d9819b7f8c16bcadf1fa403 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 10 Apr 2026 09:47:21 -0400 Subject: [PATCH 44/58] feat: hdr --- .../stuypulse/robot/constants/Settings.java | 3 +++ .../subsystems/vision/LimelightVision.java | 18 +++++++++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 7132ea07..81edc47d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -386,6 +386,9 @@ public interface Vision { public final double MAX_ANGULAR_VELOCITY_RAD_SEC = 2 * Math.PI; double MIN_TAG_AREA = 5; //TODO: MAKE SURE THIS IS A GOOD VALUE!!! + SmartBoolean HDR_ENABLED = new SmartBoolean("Vision/HDR Enabled?", true); + double HDR_TIMEOUT_SEC = 0.25; + public final double BUZZ_DEBOUNCE = 0.25; } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 337b2a6d..d53e6fbc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -25,6 +25,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.math.util.Units; @@ -56,7 +57,7 @@ public static LimelightVision getInstance() { private BStream debouncedHasData; private Pipeline currentPipeline; - private final Pipeline[] HDRPipelines = {Pipeline.NO_SUN, Pipeline.HIGH_SUN}; + private Timer hdrTimer = new Timer(); public enum MegaTagMode { MEGATAG1, @@ -116,6 +117,8 @@ public LimelightVision() { megaTagMode = MegaTagMode.MEGATAG1; setIMUMode(1); + hdrTimer.reset(); + debouncedHasData = BStream.create( () -> hasData) .filtered(new BDebounce.Both(Settings.Vision.BUZZ_DEBOUNCE)); @@ -342,6 +345,19 @@ public void periodicAfterScheduler() { } } + if (Settings.Vision.HDR_ENABLED.get()) { + Pipeline nextHdrPipeline = Pipeline.NO_SUN; + if (!hdrTimer.hasElapsed(Settings.Vision.HDR_TIMEOUT_SEC)) { + + if (currentPipeline == Pipeline.NO_SUN) { + nextHdrPipeline = Pipeline.HIGH_SUN; + } + + setPipeline(nextHdrPipeline); + hdrTimer.reset(); + } + } + SmartDashboard.putBoolean("Vision/Has Data", hasData); } } From 3fdddfc5979d97d0ae6cc5e8ad0b4a573de25932 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 10 Apr 2026 13:38:43 -0400 Subject: [PATCH 45/58] fix: kill ta filtering --- .../deploy/pathplanner/paths/Depot To Score.path | 15 ++++++++++----- .../robot/commands/auton/regular/LeftMiddy.java | 4 ++-- .../robot/commands/auton/regular/RightMiddy.java | 4 ++-- .../commands/swerve/SwerveResetPoseKBShot.java | 8 ++++++-- .../java/com/stuypulse/robot/constants/Field.java | 2 +- .../com/stuypulse/robot/constants/Settings.java | 1 + .../robot/subsystems/vision/LimelightVision.java | 14 +++++++------- 7 files changed, 29 insertions(+), 19 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Depot To Score.path b/src/main/deploy/pathplanner/paths/Depot To Score.path index 44d6a43f..84570452 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Score.path +++ b/src/main/deploy/pathplanner/paths/Depot To Score.path @@ -28,7 +28,12 @@ "linkedName": "Depot Out" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.3262260127931784, + "rotationDegrees": 180.0 + } + ], "constraintZones": [ { "name": "Constraints Zone", @@ -36,9 +41,9 @@ "maxWaypointRelativePos": 0.9448818897637792, "constraints": { "maxVelocity": 0.5, - "maxAcceleration": 1.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, + "maxAcceleration": 0.75, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0, "nominalVoltage": 12.5, "unlimited": false } @@ -56,7 +61,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "To Score", diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java index b6fe1713..44b4abea 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java @@ -28,7 +28,7 @@ public LeftMiddy(PathPlannerPath... paths) { // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(1.0).andThen(new IntakeDeploy()) + new WaitCommand(0.75).andThen(new IntakeDeploy()) ), // Trip 1 To Score @@ -54,7 +54,7 @@ public LeftMiddy(PathPlannerPath... paths) { new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new WaitCommand(1.0).andThen(new HandoffRun().alongWith(new SpindexerRun())), - new WaitCommand(2.5).andThen(new IntakeAutoDigest().repeatedly()) + new WaitCommand(3.0).andThen(new IntakeAutoDigest().repeatedly()) ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java index f61c6f3d..2cbffe4f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java @@ -28,7 +28,7 @@ public RightMiddy(PathPlannerPath... paths) { // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(1.0).andThen(new IntakeDeploy()) + new WaitCommand(0.75).andThen(new IntakeDeploy()) ), // Trip 1 To Score @@ -54,7 +54,7 @@ public RightMiddy(PathPlannerPath... paths) { new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new WaitCommand(1.0).andThen(new HandoffRun().alongWith(new SpindexerRun())), - new WaitCommand(2.5).andThen(new IntakeAutoDigest().repeatedly()) + new WaitCommand(3.0).andThen(new IntakeAutoDigest().repeatedly()) ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java index 070fff4a..818e36c1 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java @@ -4,9 +4,13 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.util.Units; public class SwerveResetPoseKBShot extends SwerveResetPose { public SwerveResetPoseKBShot() { - super(new Pose2d(0, Field.WIDTH / 2, CommandSwerveDrivetrain.getInstance().getPose().getRotation())); //TODO: FIELD CAL GET X - } + super(new Pose2d( + Field.HUB_CENTER.getX() - Field.HUB_RADIUS - Units.inchesToMeters(23.5), + Field.WIDTH / 2 - Units.inchesToMeters(6.5), + CommandSwerveDrivetrain.getInstance().getPose().getRotation())); + } } diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index d0ab89a2..459e27a5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -39,7 +39,7 @@ public interface Field { public static final Pose2d HUB_FAR_RIGHT_CORNER = new Pose2d(Units.inchesToMeters(205.6), WIDTH / 2.0 - Units.inchesToMeters(47 / 2.0), Rotation2d.kZero); public static final Pose2d HUB_FAR_LEFT_CORNER = new Pose2d(Units.inchesToMeters(205.6), WIDTH / 2.0 + Units.inchesToMeters(47 / 2.0), Rotation2d.kZero); - public static final double HUB_RADIUS = Units.inchesToMeters(41.7 / 2); + public static final double HUB_RADIUS = Units.inchesToMeters(41.7 / 2.0); public static final double OPPONENT_ZONE_X = LENGTH - Units.inchesToMeters(158.6); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 81edc47d..45cf8c19 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -141,6 +141,7 @@ public interface TOFInterpolation{ public interface FerryRPMInterpolation { double[][] ferryDistanceRPMInterpolation = { + {1, 2000}, {5.16, 3300.0}, {6.94, 3600.0}, {7.87, 3800.0}, diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index d53e6fbc..059283e7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -261,7 +261,7 @@ public void periodicAfterScheduler() { boolean notNull = false; boolean withinAngularVelocityTolerance = false; boolean withinInvalidPositionTolerance = false; - boolean withinTargetAreaTolerance = false; + // boolean withinTargetAreaTolerance = false; if (poseEstimate != null && poseEstimate.tagCount > 0) { notNull = true; @@ -278,16 +278,16 @@ public void periodicAfterScheduler() { Cameras.LimelightCameras[i].incrementRejection(RejectionValue.ANGULAR_VELOCITY); } - if(poseEstimate.avgTagArea >= Settings.Vision.MIN_TAG_AREA) { - withinTargetAreaTolerance = true; - } else { - Cameras.LimelightCameras[i].incrementRejection(RejectionValue.TARGET_AREA); - } + // if (poseEstimate.avgTagArea >= Settings.Vision.MIN_TAG_AREA) { + // withinTargetAreaTolerance = true; + // } else { + // Cameras.LimelightCameras[i].incrementRejection(RejectionValue.TARGET_AREA); + // } Pose2d robotPose = poseEstimate.pose; double timestamp = poseEstimate.timestampSeconds; - boolean isAcceptablePose = notNull && withinAngularVelocityTolerance && !withinInvalidPositionTolerance && withinTargetAreaTolerance; + boolean isAcceptablePose = notNull && withinAngularVelocityTolerance && !withinInvalidPositionTolerance;// && withinTargetAreaTolerance; if (megaTagMode == MegaTagMode.MEGATAG1 && isAcceptablePose) { CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT1_STDEVS); From 26c35f7c3492dc2c9a43383099d98859d31d3a82 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 10 Apr 2026 13:39:13 -0400 Subject: [PATCH 46/58] Revert "FEAT: doglog spindexer non elastic values and update vendor deps" This reverts commit 0b5e2c26c081f970de02bb75c345a288ae5921a8. --- .../stuypulse/robot/constants/Settings.java | 2 +- .../subsystems/spindexer/SpindexerImpl.java | 23 +- vendordeps/DogLog.json | 20 -- vendordeps/PathplannerLib-2026.1.2.json | 38 --- vendordeps/PathplannerLib.json | 36 +++ vendordeps/REVLib.json | 262 +++++++++--------- vendordeps/StudicaLib.json | 145 +++++----- vendordeps/photonlib-v2026.0.1-beta.json | 71 +++++ 8 files changed, 317 insertions(+), 280 deletions(-) delete mode 100644 vendordeps/DogLog.json delete mode 100644 vendordeps/PathplannerLib-2026.1.2.json create mode 100644 vendordeps/PathplannerLib.json create mode 100644 vendordeps/photonlib-v2026.0.1-beta.json diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 45cf8c19..df02f2d4 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -387,7 +387,7 @@ public interface Vision { public final double MAX_ANGULAR_VELOCITY_RAD_SEC = 2 * Math.PI; double MIN_TAG_AREA = 5; //TODO: MAKE SURE THIS IS A GOOD VALUE!!! - SmartBoolean HDR_ENABLED = new SmartBoolean("Vision/HDR Enabled?", true); + SmartBoolean HDR_ENABLED = new SmartBoolean("Vision/HDR Enabled?", false); double HDR_TIMEOUT_SEC = 0.25; public final double BUZZ_DEBOUNCE = 0.25; diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index eb941387..36ca82c1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -29,7 +29,6 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; -import dev.doglog.DogLog; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -126,13 +125,13 @@ public boolean shouldStop() { boolean turretLaggingSOTM = !superstructure.isTurretAtTolerance() && superstructureState == SuperstructureState.SOTM; - DogLog.log("Spindexer/Should Stop/turret lagging sotm", turretLaggingSOTM); - DogLog.log("Spindexer/Should Stop/is Behind Hub While Ferrying?", isBehindHubWhileFerrying); - DogLog.log("Spindexer/Should Stop/is Turret Wrapping?", isTurretWrapping); - DogLog.log("Spindexer/Should Stop/is Outside Alliance zone?", isOutsideAllianceZone); - DogLog.log("Spindexer/Should Stop/is Under Trenche?", isUnderTrench); - DogLog.log("Spindexer/Should Stop/turret lagging sotm", turretLaggingSOTM); - DogLog.log("Spindexer/Should Stop/inManualState", inManualState); + SmartDashboard.putBoolean("Spindexer/Should Stop/turret lagging sotm", turretLaggingSOTM); + SmartDashboard.putBoolean("Spindexer/Should Stop/is Behind Hub While Ferrying?", isBehindHubWhileFerrying); + SmartDashboard.putBoolean("Spindexer/Should Stop/is Turret Wrapping?", isTurretWrapping); + SmartDashboard.putBoolean("Spindexer/Should Stop/is Outside Alliance zone?", isOutsideAllianceZone); + SmartDashboard.putBoolean("Spindexer/Should Stop/is Under Trenche?", isUnderTrench); + SmartDashboard.putBoolean("Spindexer/Should Stop/turret lagging sotm", turretLaggingSOTM); + SmartDashboard.putBoolean("Spindexer/Should Stop/inManualState", inManualState); return isStopState || isTurretWrapping || (isBehindHubWhileFerrying && !inManualState) || @@ -180,12 +179,12 @@ public void periodicAfterScheduler() { leaderMotor.stopMotor(); } - DogLog.log("Spindexer/Leader Motor RPM", getMotorRPM()); + SmartDashboard.putNumber("Spindexer/Leader Motor RPM", getMotorRPM()); // SmartDashboard.putBoolean("Spindexer/Unjamming", unJamming); - DogLog.log("Spindexer/Leader Voltage (volts)", leaderMotorVoltage.getValueAsDouble()); - DogLog.log("Spindexer/Leader Supply Current (amps)", leaderSupplyCurrent.getValueAsDouble()); - DogLog.log("Spindexer/Leader Stator Current (amps)", leaderStatorCurrent.getValueAsDouble()); + SmartDashboard.putNumber("Spindexer/Leader Voltage (volts)", leaderMotorVoltage.getValueAsDouble()); + SmartDashboard.putNumber("Spindexer/Leader Supply Current (amps)", leaderSupplyCurrent.getValueAsDouble()); + SmartDashboard.putNumber("Spindexer/Leader Stator Current (amps)", leaderStatorCurrent.getValueAsDouble()); SmartDashboard.putBoolean("Spindexer/Should Stop?", shouldStop()); diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json deleted file mode 100644 index 7201ed05..00000000 --- a/vendordeps/DogLog.json +++ /dev/null @@ -1,20 +0,0 @@ -{ - "javaDependencies": [ - { - "groupId": "com.github.jonahsnider", - "artifactId": "doglog", - "version": "2026.5.0" - } - ], - "fileName": "DogLog.json", - "frcYear": "2026", - "jsonUrl": "https://doglog.dev/vendordep.json", - "name": "DogLog", - "jniDependencies": [], - "mavenUrls": [ - "https://jitpack.io" - ], - "cppDependencies": [], - "version": "2026.5.0", - "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" -} \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2026.1.2.json b/vendordeps/PathplannerLib-2026.1.2.json deleted file mode 100644 index f72fa41f..00000000 --- a/vendordeps/PathplannerLib-2026.1.2.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib-2026.1.2.json", - "name": "PathplannerLib", - "version": "2026.1.2", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2026", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2026.1.2" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2026.1.2", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 00000000..c018b776 --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,36 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2026.1.1", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2026", + "mavenUrls": ["https://3015rangerrobotics.github.io/pathplannerlib/repo"], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2026.1.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2026.1.1", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index c6b1b9e4..ce16d37c 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,133 +1,131 @@ { - "fileName": "REVLib.json", - "name": "REVLib", - "version": "2026.0.5", - "frcYear": "2026", - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "mavenUrls": [ - "https://maven.revrobotics.com/" - ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-java", - "version": "2026.0.5" - } - ], - "jniDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2026.0.5", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "RevLibBackendDriver", - "version": "2026.0.5", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.5", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-cpp", - "version": "2026.0.5", - "libName": "REVLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2026.0.5", - "libName": "REVLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "RevLibBackendDriver", - "version": "2026.0.5", - "libName": "BackendDriver", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.5", - "libName": "REVLibWpi", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] -} \ No newline at end of file + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2026.0.0", + "frcYear": "2026", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": ["https://maven.revrobotics.com/"], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2026.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2026.0.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.0", + "libName": "BackendDriver", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.0", + "libName": "REVLibWpi", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/StudicaLib.json b/vendordeps/StudicaLib.json index 52228bdc..ecf9f156 100644 --- a/vendordeps/StudicaLib.json +++ b/vendordeps/StudicaLib.json @@ -1,78 +1,69 @@ { - "fileName": "StudicaLib.json", - "name": "StudicaLib", - "version": "2026.0.2", - "frcYear": "2026", - "uuid": "963ef341-8abd-4981-a969-c8ae3f592e82", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2026/" - ], - "jsonUrl": "https://dev.studica.com/maven/release/2026/json/StudicaLib-2026.0.2.json", - "conflictsWith": [ - { - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "errorMessage": "StudicaLib (newer devices) is not compatible with Studica (NavX, NavX2)", - "offlineFileName": "Studica.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.studica.frc", - "artifactId": "StudicaLib-java", - "version": "2026.0.2" - } - ], - "jniDependencies": [ - { - "groupId": "com.studica.frc", - "artifactId": "StudicaLib-driver", - "version": "2026.0.2", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.studica.frc", - "artifactId": "StudicaLib-cpp", - "version": "2026.0.2", - "libName": "StudicaLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.studica.frc", - "artifactId": "StudicaLib-driver", - "version": "2026.0.2", - "libName": "StudicaLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] -} \ No newline at end of file + "fileName": "StudicaLib.json", + "name": "StudicaLib", + "version": "2026.0.1-beta", + "frcYear": "2026", + "uuid": "963ef341-8abd-4981-a969-c8ae3f592e82", + "mavenUrls": ["https://dev.studica.com/maven/release/2026/"], + "jsonUrl": "https://dev.studica.com/maven/release/2026/json/StudicaLib-2026.0.1-beta.json", + "javaDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "StudicaLib-java", + "version": "2026.0.1-beta" + } + ], + "jniDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "StudicaLib-driver", + "version": "2026.0.1-beta", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "StudicaLib-cpp", + "version": "2026.0.1-beta", + "libName": "StudicaLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.studica.frc", + "artifactId": "StudicaLib-driver", + "version": "2026.0.1-beta", + "libName": "StudicaLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/photonlib-v2026.0.1-beta.json b/vendordeps/photonlib-v2026.0.1-beta.json new file mode 100644 index 00000000..8b1d42b8 --- /dev/null +++ b/vendordeps/photonlib-v2026.0.1-beta.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2026.0.1-beta", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2026", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.0.1-beta", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2026.0.1-beta", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.0.1-beta", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2026.0.1-beta" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2026.0.1-beta" + } + ] +} From ac9410ad7066b07006eb12e1d86bf9568d56427b Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 10 Apr 2026 15:30:37 -0400 Subject: [PATCH 47/58] feat: jopy does autons :thumpsup: --- .../pathplanner/paths/Left NZ To Score.path | 20 +++------------- .../paths/Left Score To NZ (F).path | 4 ++-- .../paths/Left Score To Score.path | 9 +++---- .../pathplanner/paths/Right NZ To Score.path | 24 ++++--------------- .../paths/Right Score To NZ (F).path | 8 +++---- .../paths/Right Score To Score.path | 16 ++++++------- 6 files changed, 27 insertions(+), 54 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 800f2a1d..bf306c4c 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 3.583740458015267, + "x": 3.63796005706134, "y": 7.440906488549619 }, "prevControl": { - "x": 5.8276631079478065, + "x": 5.8818827069938795, "y": 7.504839857651245 }, "nextControl": null, @@ -54,21 +54,7 @@ "rotationDegrees": 0.0 } ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.65, - "maxWaypointRelativePos": 2.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index c525192a..dc204719 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.583740458015267, + "x": 3.63796005706134, "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 6.792527904520259, + "x": 6.846747503566332, "y": 7.440906488549619 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index 71756025..a8859545 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.583740458015267, + "x": 3.63796005706134, "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 6.839027283511269, + "x": 6.893246882557342, "y": 7.472562277580071 }, "isLocked": false, @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 3.583740458015267, + "x": 3.63796005706134, "y": 7.440906488549619 }, "prevControl": { - "x": 5.239888817501716, + "x": 5.294108416547789, "y": 7.399604078672524 }, "nextControl": null, @@ -124,4 +124,5 @@ "rotation": 0.0 }, "useDefaultConstraints": false +} } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 34242f5c..15f637d7 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.633950381679389, - "y": 0.5705152671755729 + "x": 3.6120827389443653, + "y": 0.5868473609129818 }, "prevControl": { - "x": 6.225753262158957, - "y": 0.5974377224199281 + "x": 6.203885619423932, + "y": 0.6137698161573371 }, "nextControl": null, "isLocked": false, @@ -54,21 +54,7 @@ "rotationDegrees": 0.0 } ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 2.7, - "maxWaypointRelativePos": 3.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index db887837..fe329dc4 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.633950381679389, - "y": 0.5705152671755729 + "x": 3.6120827389443653, + "y": 0.5868473609129818 }, "prevControl": null, "nextControl": { - "x": 7.2050202818220415, - "y": 0.5575766081170854 + "x": 7.183152639087018, + "y": 0.5739087018544944 }, "isLocked": false, "linkedName": "Right Trench Score" diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 6ee888b9..314305a2 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.633950381679389, - "y": 0.5705152671755729 + "x": 3.6120827389443653, + "y": 0.5868473609129818 }, "prevControl": null, "nextControl": { - "x": 6.9143416370106765, - "y": 0.6942704626334513 + "x": 6.892473994275653, + "y": 0.7106025563708602 }, "isLocked": false, "linkedName": "Right Trench Score" @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 3.633950381679389, - "y": 0.5705152671755729 + "x": 3.6120827389443653, + "y": 0.5868473609129818 }, "prevControl": { - "x": 7.694956772334293, - "y": 0.5324639769452448 + "x": 7.673089129599269, + "y": 0.5487960706826538 }, "nextControl": null, "isLocked": false, From fe06cc01d94d26a17f8ecc7d29c0f3d9ce138e5a Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 10 Apr 2026 18:15:56 -0400 Subject: [PATCH 48/58] feat: revert doglog + no sun by default --- elastic-layout.json | 74 +++++++++++++++---- .../paths/Left Score To Score.path | 1 - .../subsystems/vision/LimelightVision.java | 10 ++- 3 files changed, 65 insertions(+), 20 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 36122ead..0e519df8 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -183,6 +183,8 @@ "robot_color": 4294198070, "trajectory_color": 4294967295, "show_robot_outside_widget": true + "trajectory_color": 4294967295, + "show_robot_outside_widget": true } }, { @@ -381,24 +383,12 @@ "topic": "/SmartDashboard/FMSUtil/time left in shift", "period": 0.06, "data_type": "double", + "data_type": "double", "time_display_mode": "Minutes and Seconds", "red_start_time": 20, "yellow_start_time": 30 } }, - { - "title": "Autonomous", - "x": 560.0, - "y": 420.0, - "width": 210.0, - "height": 140.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/SmartDashboard/Autonomous", - "period": 0.06, - "sort_options": false - } - }, { "title": "Set Megatag 2", "x": 770.0, @@ -411,6 +401,8 @@ "period": 0.5, "show_type": true, "maximize_button_space": false + "show_type": true, + "maximize_button_space": false } }, { @@ -425,6 +417,8 @@ "period": 0.5, "show_type": true, "maximize_button_space": false + "show_type": true, + "maximize_button_space": false } }, { @@ -524,6 +518,19 @@ "true_icon": "None", "false_icon": "None" } + }, + { + "title": "Autonomous", + "x": 560.0, + "y": 420.0, + "width": 210.0, + "height": 140.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Autonomous", + "period": 0.5, + "sort_options": false + } } ] } @@ -696,6 +703,8 @@ "robot_color": 4294198070, "trajectory_color": 4294967295, "show_robot_outside_widget": true + "trajectory_color": 4294967295, + "show_robot_outside_widget": true } }, { @@ -879,6 +888,7 @@ "topic": "/SmartDashboard/FMSUtil/time left in shift", "period": 0.06, "data_type": "double", + "data_type": "double", "time_display_mode": "Minutes and Seconds", "red_start_time": 20, "yellow_start_time": 30 @@ -895,6 +905,7 @@ "topic": "/SmartDashboard/FMSUtil/No Auto Winner Data", "period": 0.06, "data_type": "boolean", + "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -913,6 +924,8 @@ "period": 0.06, "show_type": false, "maximize_button_space": true + "show_type": false, + "maximize_button_space": true } }, { @@ -927,6 +940,8 @@ "period": 0.06, "show_type": false, "maximize_button_space": true + "show_type": false, + "maximize_button_space": true } }, { @@ -941,6 +956,8 @@ "period": 0.06, "show_type": false, "maximize_button_space": true + "show_type": false, + "maximize_button_space": true } }, { @@ -982,7 +999,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Current Angle (deg)", + "topic": "/Robot/Intake/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -1012,7 +1029,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", + "topic": "/Robot/Superstructure/Hood/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -1030,6 +1047,8 @@ "period": 0.5, "show_type": false, "maximize_button_space": true + "show_type": false, + "maximize_button_space": true } }, { @@ -1044,6 +1063,8 @@ "period": 0.5, "show_type": false, "maximize_button_space": true + "show_type": false, + "maximize_button_space": true } }, { @@ -1058,6 +1079,8 @@ "period": 0.5, "show_type": false, "maximize_button_space": true + "show_type": false, + "maximize_button_space": true } }, { @@ -1286,6 +1309,8 @@ "robot_color": 4294198070, "trajectory_color": 4294967295, "show_robot_outside_widget": true + "trajectory_color": 4294967295, + "show_robot_outside_widget": true } }, { @@ -1486,6 +1511,8 @@ "period": 0.5, "show_type": false, "maximize_button_space": true + "show_type": false, + "maximize_button_space": true } }, { @@ -1514,6 +1541,8 @@ "period": 0.5, "show_type": false, "maximize_button_space": true + "show_type": false, + "maximize_button_space": true } }, { @@ -1528,6 +1557,8 @@ "period": 0.5, "show_type": false, "maximize_button_space": true + "show_type": false, + "maximize_button_space": true } }, { @@ -1669,6 +1700,19 @@ "data_type": "double", "show_submit_button": false } + }, + { + "title": "HDR Enabled?", + "x": 1260.0, + "y": 630.0, + "width": 140.0, + "height": 140.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Vision/HDR Enabled?", + "period": 0.5, + "data_type": "boolean" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index a8859545..04316942 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -125,4 +125,3 @@ }, "useDefaultConstraints": false } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 059283e7..649eb571 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -73,10 +73,10 @@ public enum Pipeline { private int getCurrentPipelineID() { return switch(this.currentPipeline) { - case NO_SUN -> 0; - case LOW_SUN -> 1; - case MED_SUN -> 2; - case HIGH_SUN -> 3; + case NO_SUN -> 3; + case LOW_SUN -> 2; + case MED_SUN -> 1; + case HIGH_SUN -> 0; }; } @@ -122,6 +122,8 @@ public LimelightVision() { debouncedHasData = BStream.create( () -> hasData) .filtered(new BDebounce.Both(Settings.Vision.BUZZ_DEBOUNCE)); + + setPipeline(Pipeline.NO_SUN); } public void setAllLTagWhitelist(int... ids) { From b2bb90b97c474592fc2346b8faeee963ae9ca673 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 11 Apr 2026 12:18:18 -0400 Subject: [PATCH 49/58] feat: ferry zone + torquecurrent for pivot + kill hoot log --- elastic-layout.json | 340 ++++++++---------- simgui.json | 4 + .../pathplanner/paths/Left Trench To NZ.path | 2 +- .../pathplanner/paths/Right Trench To NZ.path | 2 +- src/main/java/com/stuypulse/robot/Robot.java | 16 +- .../com/stuypulse/robot/constants/Field.java | 4 +- .../stuypulse/robot/constants/Settings.java | 1 + .../robot/subsystems/intake/IntakeImpl.java | 4 +- .../subsystems/spindexer/SpindexerImpl.java | 9 - .../superstructure/turret/TurretImpl.java | 2 +- .../swerve/CommandSwerveDrivetrain.java | 10 +- 11 files changed, 172 insertions(+), 222 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 0e519df8..be17da66 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -174,7 +174,7 @@ "properties": { "topic": "/SmartDashboard/Field", "period": 0.06, - "field_game": "Rebuilt (No Fuel)", + "field_game": "Rebuilt", "robot_width": 0.85, "robot_length": 0.85, "show_other_objects": true, @@ -183,8 +183,6 @@ "robot_color": 4294198070, "trajectory_color": 4294967295, "show_robot_outside_widget": true - "trajectory_color": 4294967295, - "show_robot_outside_widget": true } }, { @@ -195,9 +193,8 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Handoff/At Tolerance?", + "topic": "/Robot/Handoff/At Tolerance?", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -212,7 +209,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Handoff/State", + "topic": "/Robot/Handoff/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -226,7 +223,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "topic": "/Robot/Intake/Pivot At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -243,7 +240,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Pivot State", + "topic": "/Robot/Intake/Pivot State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -257,7 +254,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/State", + "topic": "/Robot/Superstructure/Hood/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -271,7 +268,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter/State", + "topic": "/Robot/Superstructure/Shooter/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -285,7 +282,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret/State", + "topic": "/Robot/Superstructure/Turret/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -299,7 +296,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "topic": "/Robot/Superstructure/Hood At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -316,7 +313,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "topic": "/Robot/Superstructure/Shooter At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -333,7 +330,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "topic": "/Robot/Superstructure/Turret At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -351,8 +348,7 @@ "type": "Large Text Display", "properties": { "topic": "/SmartDashboard/FMSUtil/Field State", - "period": 0.06, - "data_type": "string" + "period": 0.06 } }, { @@ -363,9 +359,8 @@ "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/FMSUtil/Is Active Shift?", + "topic": "/Robot/FMSUtil/Is Active Shift?", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -380,10 +375,9 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/FMSUtil/time left in shift", + "topic": "/Robot/FMSUtil/time left in shift", "period": 0.06, "data_type": "double", - "data_type": "double", "time_display_mode": "Minutes and Seconds", "red_start_time": 20, "yellow_start_time": 30 @@ -399,10 +393,8 @@ "properties": { "topic": "/SmartDashboard/Robot/Set Megatag 2", "period": 0.5, - "show_type": true, - "maximize_button_space": false - "show_type": true, - "maximize_button_space": false + "show_type": false, + "maximize_button_space": true } }, { @@ -415,10 +407,8 @@ "properties": { "topic": "/SmartDashboard/Robot/Set Megatag 1", "period": 0.5, - "show_type": true, - "maximize_button_space": false - "show_type": true, - "maximize_button_space": false + "show_type": false, + "maximize_button_space": true } }, { @@ -429,9 +419,8 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Spindexer/At Tolerance", + "topic": "/Robot/Spindexer/At Tolerance", "period": 0.5, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -446,7 +435,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Spindexer/State", + "topic": "/Robot/Spindexer/State", "period": 0.5, "data_type": "string", "show_submit_button": false @@ -460,23 +449,22 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Robot/Match Time", + "topic": "/Robot/Robot/Match Time", "period": 0.5, - "data_type": "double", "time_display_mode": "Minutes and Seconds", "red_start_time": 20, "yellow_start_time": 30 } }, { - "title": "limelight-back Has Data", + "title": "Back Has Data", "x": 420.0, "y": 560.0, "width": 140.0, "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Vision/limelight-back Has Data", + "topic": "/Robot/Vision/limelight-back Has Data", "period": 0.5, "data_type": "boolean", "true_color": 4283215696, @@ -486,14 +474,14 @@ } }, { - "title": "limelight-left Has Data", + "title": "Left Has Data", "x": 560.0, "y": 560.0, "width": 140.0, "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Vision/limelight-left Has Data", + "topic": "/Robot/Vision/limelight-left Has Data", "period": 0.5, "data_type": "boolean", "true_color": 4283215696, @@ -503,14 +491,14 @@ } }, { - "title": "limelight-right Has Data", + "title": "Right Has Data", "x": 700.0, "y": 560.0, "width": 140.0, "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Vision/limelight-right Has Data", + "topic": "/Robot/Vision/limelight-right Has Data", "period": 0.5, "data_type": "boolean", "true_color": 4283215696, @@ -694,7 +682,7 @@ "properties": { "topic": "/SmartDashboard/Field", "period": 0.06, - "field_game": "Rebuilt (No Fuel)", + "field_game": "Rebuilt", "robot_width": 0.85, "robot_length": 0.85, "show_other_objects": true, @@ -703,8 +691,6 @@ "robot_color": 4294198070, "trajectory_color": 4294967295, "show_robot_outside_widget": true - "trajectory_color": 4294967295, - "show_robot_outside_widget": true } }, { @@ -715,9 +701,8 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Handoff/At Tolerance?", + "topic": "/Robot/Handoff/At Tolerance?", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -732,7 +717,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Handoff/State", + "topic": "/Robot/Handoff/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -746,7 +731,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "topic": "/Robot/Intake/Pivot At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -763,7 +748,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Pivot State", + "topic": "/Robot/Intake/Pivot State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -777,7 +762,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/State", + "topic": "/Robot/Superstructure/Hood/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -791,7 +776,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret/State", + "topic": "/Robot/Superstructure/Turret/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -805,7 +790,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "topic": "/Robot/Superstructure/Hood At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -822,7 +807,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "topic": "/Robot/Superstructure/Shooter At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -839,7 +824,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "topic": "/Robot/Superstructure/Turret At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -848,19 +833,6 @@ "false_icon": "None" } }, - { - "title": "Shift", - "x": 0.0, - "y": 560.0, - "width": 280.0, - "height": 140.0, - "type": "Large Text Display", - "properties": { - "topic": "/SmartDashboard/FMSUtil/Field State", - "period": 0.06, - "data_type": "string" - } - }, { "title": "Is Active Shift?", "x": 280.0, @@ -869,7 +841,7 @@ "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/is active shift", + "topic": "/Robot/is active shift", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -885,10 +857,9 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/FMSUtil/time left in shift", + "topic": "/Robot/FMSUtil/time left in shift", "period": 0.06, "data_type": "double", - "data_type": "double", "time_display_mode": "Minutes and Seconds", "red_start_time": 20, "yellow_start_time": 30 @@ -902,10 +873,9 @@ "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/FMSUtil/No Auto Winner Data", + "topic": "/Robot/FMSUtil/No Auto Winner Data", "period": 0.06, "data_type": "boolean", - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -924,8 +894,6 @@ "period": 0.06, "show_type": false, "maximize_button_space": true - "show_type": false, - "maximize_button_space": true } }, { @@ -940,8 +908,6 @@ "period": 0.06, "show_type": false, "maximize_button_space": true - "show_type": false, - "maximize_button_space": true } }, { @@ -956,8 +922,6 @@ "period": 0.06, "show_type": false, "maximize_button_space": true - "show_type": false, - "maximize_button_space": true } }, { @@ -968,9 +932,8 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Spindexer/At Tolerance", + "topic": "/Robot/Spindexer/At Tolerance", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -985,7 +948,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Spindexer/State", + "topic": "/Robot/Spindexer/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -999,7 +962,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/Robot/Intake/Current Angle (deg)", + "topic": "/SmartDashboard/Intake/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -1013,9 +976,8 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Robot/Match Time", + "topic": "/Robot/Robot/Match Time", "period": 0.5, - "data_type": "double", "time_display_mode": "Minutes and Seconds", "red_start_time": 20, "yellow_start_time": 30 @@ -1029,7 +991,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/Robot/Superstructure/Hood/Current Angle (deg)", + "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -1047,8 +1009,6 @@ "period": 0.5, "show_type": false, "maximize_button_space": true - "show_type": false, - "maximize_button_space": true } }, { @@ -1063,8 +1023,6 @@ "period": 0.5, "show_type": false, "maximize_button_space": true - "show_type": false, - "maximize_button_space": true } }, { @@ -1079,8 +1037,6 @@ "period": 0.5, "show_type": false, "maximize_button_space": true - "show_type": false, - "maximize_button_space": true } }, { @@ -1091,7 +1047,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter/State", + "topic": "/Robot/Superstructure/Shooter/State", "period": 0.5, "data_type": "string", "show_submit_button": false @@ -1101,7 +1057,7 @@ "title": "Enable Distance Check?", "x": 560.0, "y": 560.0, - "width": 140.0, + "width": 280.0, "height": 140.0, "type": "Toggle Switch", "properties": { @@ -1120,8 +1076,8 @@ "properties": { "topic": "/SmartDashboard/Robot/Set Robot Pose Right Corner", "period": 0.5, - "show_type": true, - "maximize_button_space": false + "show_type": false, + "maximize_button_space": true } }, { @@ -1134,8 +1090,20 @@ "properties": { "topic": "/SmartDashboard/Robot/Set Robot Pose Left Corner", "period": 0.5, - "show_type": true, - "maximize_button_space": false + "show_type": false, + "maximize_button_space": true + } + }, + { + "title": "Shift", + "x": 0.0, + "y": 560.0, + "width": 280.0, + "height": 140.0, + "type": "Large Text Display", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Field State", + "period": 0.06 } } ] @@ -1300,7 +1268,7 @@ "properties": { "topic": "/SmartDashboard/Field", "period": 0.06, - "field_game": "Rebuilt (No Fuel)", + "field_game": "Rebuilt", "robot_width": 0.85, "robot_length": 0.85, "show_other_objects": true, @@ -1309,8 +1277,6 @@ "robot_color": 4294198070, "trajectory_color": 4294967295, "show_robot_outside_widget": true - "trajectory_color": 4294967295, - "show_robot_outside_widget": true } }, { @@ -1321,9 +1287,8 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Handoff/At Tolerance?", + "topic": "/Robot/Handoff/At Tolerance?", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -1338,7 +1303,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Handoff/State", + "topic": "/Robot/Handoff/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1352,7 +1317,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "topic": "/Robot/Intake/Pivot At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1369,7 +1334,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Pivot State", + "topic": "/Robot/Intake/Pivot State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1383,7 +1348,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/State", + "topic": "/Robot/Superstructure/Hood/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1397,7 +1362,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter/State", + "topic": "/Robot/Superstructure/Shooter/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1411,7 +1376,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret/State", + "topic": "/Robot/Superstructure/Turret/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1425,7 +1390,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "topic": "/Robot/Superstructure/Hood At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1442,7 +1407,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "topic": "/Robot/Superstructure/Shooter At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1459,7 +1424,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "topic": "/Robot/Superstructure/Turret At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1476,9 +1441,8 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Spindexer/At Tolerance", + "topic": "/Robot/Spindexer/At Tolerance", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -1493,7 +1457,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Spindexer/State", + "topic": "/Robot/Spindexer/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1511,15 +1475,13 @@ "period": 0.5, "show_type": false, "maximize_button_space": true - "show_type": false, - "maximize_button_space": true } }, { "title": "Current Turret Angle", "x": 980.0, "y": 560.0, - "width": 280.0, + "width": 210.0, "height": 140.0, "type": "Text Display", "properties": { @@ -1541,8 +1503,6 @@ "period": 0.5, "show_type": false, "maximize_button_space": true - "show_type": false, - "maximize_button_space": true } }, { @@ -1557,162 +1517,160 @@ "period": 0.5, "show_type": false, "maximize_button_space": true - "show_type": false, - "maximize_button_space": true } }, { - "title": "Current Intake Angle", - "x": 770.0, + "title": "Current Hood Angle", + "x": 0.0, "y": 560.0, "width": 210.0, "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Current Angle (deg)", + "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false } }, { - "title": "Current Hood Angle", + "title": "Seed Hood At Lower Limit", "x": 0.0, - "y": 560.0, - "width": 210.0, + "y": 420.0, + "width": 280.0, "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Seed Hood Relative Encoder At Lower Hardstop", + "period": 0.5, + "show_type": false, + "maximize_button_space": true + } + }, + { + "title": "Pipeline back", + "x": 1260.0, + "y": 560.0, + "width": 140.0, + "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", + "topic": "/limelight-back/getpipe", "period": 0.5, "data_type": "double", "show_submit_button": false } }, { - "title": "Seed Hood Relative Encoder At Lower Hardstop", - "x": 0.0, + "title": "Pipeline right", + "x": 1260.0, + "y": 490.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/limelight-right/getpipe", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pipeline Left", + "x": 1260.0, "y": 420.0, - "width": 280.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/limelight-left/getpipe", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HDR Enabled?", + "x": 1260.0, + "y": 630.0, + "width": 140.0, "height": 140.0, - "type": "Command", + "type": "Toggle Switch", "properties": { - "topic": "/SmartDashboard/Robot/Seed Hood Relative Encoder At Lower Hardstop", + "topic": "SmartDashboard/Vision/HDR Enabled?", "period": 0.5, - "show_type": true, - "maximize_button_space": false + "data_type": "boolean" } }, { - "title": "Set Exposure 25", + "title": "Set Pipeline High Sun", "x": 0.0, "y": 700.0, "width": 280.0, "height": 140.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Robot/Set Exposure 25", + "topic": "/SmartDashboard/Robot/Set Pipeline High Sun", "period": 0.5, "show_type": true, - "maximize_button_space": false + "maximize_button_space": true } }, { - "title": "Set Exposure 80", + "title": "Set Pipeline Med Sun", "x": 280.0, "y": 700.0, "width": 280.0, "height": 140.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Robot/Set Exposure 80", + "topic": "/SmartDashboard/Robot/Set Pipeline Med Sun", "period": 0.5, "show_type": true, - "maximize_button_space": false + "maximize_button_space": true } }, { - "title": "Set Exposure 100", + "title": "Set Pipeline Low Sun", "x": 560.0, "y": 700.0, "width": 280.0, "height": 140.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Robot/Set Exposure 100", + "topic": "/SmartDashboard/Robot/Set Pipeline Low Sun", "period": 0.5, "show_type": true, - "maximize_button_space": false + "maximize_button_space": true } }, { - "title": "Set Exposure 130", + "title": "Set Pipeline No Sun", "x": 840.0, "y": 700.0, "width": 280.0, "height": 140.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Robot/Set Exposure 130", + "topic": "/SmartDashboard/Robot/Set Pipeline No Sun", "period": 0.5, "show_type": true, - "maximize_button_space": false + "maximize_button_space": true } }, { - "title": "Pipeline back", - "x": 1260.0, + "title": "Current Intake Angle ", + "x": 700.0, "y": 560.0, - "width": 140.0, - "height": 70.0, - "type": "Text Display", - "properties": { - "topic": "/limelight-back/getpipe", - "period": 0.5, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Pipeline right", - "x": 1260.0, - "y": 490.0, - "width": 140.0, - "height": 70.0, - "type": "Text Display", - "properties": { - "topic": "/limelight-right/getpipe", - "period": 0.5, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Pipeline Left", - "x": 1260.0, - "y": 420.0, - "width": 140.0, - "height": 70.0, + "width": 210.0, + "height": 140.0, "type": "Text Display", "properties": { - "topic": "/limelight-left/getpipe", + "topic": "/SmartDashboard/Intake/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false } - }, - { - "title": "HDR Enabled?", - "x": 1260.0, - "y": 630.0, - "width": 140.0, - "height": 140.0, - "type": "Toggle Switch", - "properties": { - "topic": "SmartDashboard/Vision/HDR Enabled?", - "period": 0.5, - "data_type": "boolean" - } } ] } diff --git a/simgui.json b/simgui.json index a3bae0c6..5bbb6609 100644 --- a/simgui.json +++ b/simgui.json @@ -38,6 +38,10 @@ "/SmartDashboard/Robot/Set Left LL PF": "Command", "/SmartDashboard/Robot/Set Megatag 1": "Command", "/SmartDashboard/Robot/Set Megatag 2": "Command", + "/SmartDashboard/Robot/Set Pipeline High Sun": "Command", + "/SmartDashboard/Robot/Set Pipeline Low Sun": "Command", + "/SmartDashboard/Robot/Set Pipeline Med Sun": "Command", + "/SmartDashboard/Robot/Set Pipeline No Sun": "Command", "/SmartDashboard/Robot/Set Right LL PF": "Command", "/SmartDashboard/Robot/Set Robot Pose Left Corner": "Command", "/SmartDashboard/Robot/Set Robot Pose Right Corner": "Command", diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 333b8916..c6f5b517 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -37,7 +37,7 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.629304523970286, + "minWaypointRelativePos": 0.65, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 1.5, diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index f83eeb16..de8abb4b 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -37,7 +37,7 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.7346387575962167, + "minWaypointRelativePos": 0.65, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 1.5, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 1991fd9b..0dd44eee 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -97,7 +97,7 @@ public void robotInit() { gcStatsCollector = new GcStatsCollector(); DataLogManager.start(); - SignalLogger.start(); + // SignalLogger.start(); CommandScheduler.getInstance().schedule(new SwerveAutonInit()); CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand()); @@ -115,11 +115,7 @@ public void robotPeriodic() { if (periodicCounter % 50 == 0) { DataLogManager.getLog().resume(); } - // if (cameras.getSelected() != selected) { - // PortForwarder.remove(5801); - // selected = cameras.getSelected(); - // PortForwarder.add(5801, selected + ".local:5801", 5801); - // } + periodicCounter++; double batteryVoltage = RobotController.getBatteryVoltage(); @@ -137,15 +133,11 @@ public void robotPeriodic() { SmartDashboard.putData(CommandScheduler.getInstance()); } - if (DriverStation.getAlliance().isPresent()) { - alliance = DriverStation.getAlliance().get(); - } SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime()); SmartDashboard.putData("Robot/Scheduled Commands", CommandScheduler.getInstance()); SmartDashboard.putNumber("Robot/Battery Voltage", batteryVoltage); SmartDashboard.putNumber("Robot/CPU Temperature (C)", RobotController.getCPUTemp()); - SmartDashboard.putNumber("Robot/Times Disconnected", HAL.getCommsDisableCount()); robot.periodicAfterScheduler(); energyUtil.periodic(); @@ -169,6 +161,10 @@ public void disabledInit() { public void disabledPeriodic() { if (periodicCounter % Settings.LOGGING_FREQUENCY == 0) { auto = robot.getAutonomousCommand(); + + if (DriverStation.getAlliance().isPresent()) { + alliance = DriverStation.getAlliance().get(); + } } } diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 459e27a5..0ce7ce96 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -66,13 +66,13 @@ public static boolean closerToTop() { public final Pose2d LEFT_FERRY_ZONE = new Pose2d( Units.inchesToMeters(31.5), - WIDTH - Units.inchesToMeters(34.5), + WIDTH - Units.inchesToMeters(34.5) - Units.inchesToMeters(12), new Rotation2d() ); public final Pose2d RIGHT_FERRY_ZONE = new Pose2d( Units.inchesToMeters(20.75), - Units.inchesToMeters(76), + Units.inchesToMeters(76) + Units.inchesToMeters(12), new Rotation2d() ); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index df02f2d4..dc5718df 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -74,6 +74,7 @@ public interface Intake { double HOMING_VOLTAGE = 3.0; double PUSHDOWN_VOLTAGE = 3.0; + double PUSHDOWN_CURRENT = 15; //TODO: GET ACTUAL TYTY double GEAR_RATIO = 37.93; diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index ec9ccf42..ad44f31f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -187,8 +187,8 @@ public void periodicAfterScheduler() { if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE.getDegrees() && rollerState != RollerState.STOP) { - pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts - // pivot.setControl(new TorqueCurrentFOC(Settings.Intake.PUSHDOWN_CURRENT)); + // pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts + pivot.setControl(new TorqueCurrentFOC(Settings.Intake.PUSHDOWN_CURRENT)); applyingPushdownVoltage = true; } else if (pivotState == PivotState.HOMING) { pivot.setControl(new VoltageOut(-Settings.Intake.HOMING_VOLTAGE)); diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index 36ca82c1..2466d917 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -8,7 +8,6 @@ import java.util.Optional; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -20,7 +19,6 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.handoff.Handoff; -import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -38,14 +36,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import java.util.Optional; public class SpindexerImpl extends Spindexer { private final Motors.TalonFXConfig spindexerLeadConfig; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 8b85667d..e9c010ff 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -296,7 +296,7 @@ public void periodicAfterScheduler() { if (Settings.DEBUG_MODE.get()) { SmartDashboard.putNumber("Superstructure/Turret/Stator Current (amps)", turretMotorStatorCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Supply Curren (amps)", + SmartDashboard.putNumber("Superstructure/Turret/Supply Current (amps)", turretMotorSupplyCurrent.getValueAsDouble()); if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index c9525e85..b58a9da9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -76,7 +76,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su static { instance = TunerConstants.createDrivetrain(); - instance.registerTelemetry(instance::telemeterize); + // instance.registerTelemetry(instance::telemeterize); } public void telemeterize(SwerveDriveState state) { @@ -377,8 +377,8 @@ private boolean checkIfVisionMeasurementValid(Pose2d visionPose) { */ @Override public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); - SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); + // SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); + // SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); if(checkIfVisionMeasurementValid(visionRobotPoseMeters)) { super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); @@ -405,8 +405,8 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS */ @Override public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { - SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); - SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); + // SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); + // SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); if(checkIfVisionMeasurementValid(visionRobotPoseMeters)) { super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), From 696f9053145481b6afda08ab0cd0a220ad6c5092 Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Sat, 11 Apr 2026 17:22:58 -0400 Subject: [PATCH 50/58] perF: cash the fmsAttached, use the same object as opposed to making new objects for the intakeImpl --- src/main/java/com/stuypulse/robot/Robot.java | 41 +++++++--------- .../robot/subsystems/handoff/HandoffImpl.java | 3 +- .../robot/subsystems/intake/IntakeImpl.java | 48 +++++++++++-------- .../subsystems/spindexer/SpindexerImpl.java | 7 +-- .../superstructure/hood/HoodImpl.java | 25 +++++----- .../superstructure/shooter/ShooterImpl.java | 3 +- .../superstructure/turret/TurretImpl.java | 27 +++++------ .../swerve/CommandSwerveDrivetrain.java | 3 +- 8 files changed, 73 insertions(+), 84 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 0dd44eee..96cc5e8e 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -5,48 +5,38 @@ /***************************************************************/ package com.stuypulse.robot; +import java.lang.management.GarbageCollectorMXBean; +import java.lang.management.ManagementFactory; +import java.util.List; + +import com.pathplanner.lib.commands.FollowPathCommand; +import com.pathplanner.lib.commands.PathfindingCommand; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.swerve.SwerveAutonInit; import com.stuypulse.robot.commands.swerve.SwerveTeleopInit; +import com.stuypulse.robot.commands.vision.BlackListAllTagsForAllCameras; import com.stuypulse.robot.commands.vision.SetMegaTagMode; import com.stuypulse.robot.commands.vision.WhitelistAllTagsForAllCameras; -import com.stuypulse.robot.commands.vision.WhitelistRoutineLeftSideAuto; -import com.stuypulse.robot.commands.vision.WhitelistRoutineRightSideAuto; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Cameras.Camera; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.util.EnergyUtil; import com.stuypulse.robot.util.FMSUtil; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.superstructure.SOTMCalculator; -import com.stuypulse.stuylib.network.SmartBoolean; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.net.PortForwarder; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; - -import java.lang.management.GarbageCollectorMXBean; -import java.lang.management.ManagementFactory; -import java.util.List; - -import com.ctre.phoenix6.SignalLogger; -import com.pathplanner.lib.commands.FollowPathCommand; -import com.pathplanner.lib.commands.PathfindingCommand; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; -import com.stuypulse.robot.commands.vision.BlackListAllTagsForAllCameras; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; public class Robot extends TimedRobot { public enum RobotMode { @@ -62,9 +52,8 @@ public enum RobotMode { private static RobotMode mode; private static EnergyUtil energyUtil; private FMSUtil fmsUtil; - private SendableChooser cameras = new SendableChooser(); - private Camera selected; private GcStatsCollector gcStatsCollector; + public static boolean fmsAttached; private static int periodicCounter = 0; @@ -165,6 +154,10 @@ public void disabledPeriodic() { if (DriverStation.getAlliance().isPresent()) { alliance = DriverStation.getAlliance().get(); } + + if (DriverStation.isFMSAttached()) { + fmsAttached = true; + } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java index 4b729b74..3ebb2b3a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -33,7 +33,6 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -187,7 +186,7 @@ public void periodicAfterScheduler() { SmartDashboard.putNumber("Handoff/Follow Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); SmartDashboard.putNumber("Handoff/Follow Stator Current", motorLeadStatorCurrent.getValueAsDouble()); - if(Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + if(Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { SmartDashboard.putBoolean("Robot/CAN/Main/Handoff Lead Motor Connected? (ID " + String.valueOf(Ports.Handoff.MOTOR_LEAD) + ")", motorLead.isConnected()); SmartDashboard.putBoolean("Robot/CAN/Main/Handoff Follow Motor Connected? (ID " + String.valueOf(Ports.Handoff.MOTOR_FOLLOW) + ")", motorFollow.isConnected()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index ad44f31f..cdb73043 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -5,8 +5,20 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.intake; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import java.util.Optional; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; @@ -16,30 +28,17 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.SysId; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.MotorAlignmentValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import java.util.Optional; - public class IntakeImpl extends Intake { private Motors.TalonFXConfig pivotConfig; private Motors.TalonFXConfig rollerConfig; @@ -52,6 +51,9 @@ public class IntakeImpl extends Intake { private final Follower follower; private Optional pivotVoltageOverride; + private TorqueCurrentFOC torqueCurrentFOC; + private VoltageOut voltageOut; + private PositionVoltage positionVoltage; private BStream pivotStalling; @@ -94,6 +96,10 @@ public IntakeImpl() { .withStatorCurrentLimitEnabled(false) .withRampRate(0.50); + torqueCurrentFOC = new TorqueCurrentFOC(0.0); + voltageOut = new VoltageOut(0.0); + positionVoltage = new PositionVoltage(0.0); + pivot = new TalonFX(Ports.Intake.PIVOT, Ports.RIO); pivotConfig.configure(pivot); @@ -188,12 +194,12 @@ public void periodicAfterScheduler() { getPivotAngle().getDegrees() <= Settings.Intake.ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE.getDegrees() && rollerState != RollerState.STOP) { // pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts - pivot.setControl(new TorqueCurrentFOC(Settings.Intake.PUSHDOWN_CURRENT)); + pivot.setControl(torqueCurrentFOC.withOutput(Settings.Intake.PUSHDOWN_CURRENT)); applyingPushdownVoltage = true; } else if (pivotState == PivotState.HOMING) { - pivot.setControl(new VoltageOut(-Settings.Intake.HOMING_VOLTAGE)); + pivot.setControl(voltageOut.withOutput(-Settings.Intake.HOMING_VOLTAGE)); } else { - pivot.setControl(new PositionVoltage(pivotState.getTargetAngle().getRotations())); + pivot.setControl(positionVoltage.withPosition(getPivotState().getTargetAngle().getRotations())); } // ROLLERS @@ -255,7 +261,7 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge SmartDashboard.putNumber("Intake/Pivot Stator Current (amps)", pivotStatorCurrent.getValueAsDouble()); - if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { SmartDashboard.putBoolean("Robot/CAN/Main/Intake Pivot Motor Connected? (ID " + String.valueOf(Ports.Intake.PIVOT) + ")", pivot.isConnected()); SmartDashboard.putBoolean("Robot/CAN/Main/Intake Roller Leader Motor Connected? (ID " diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index 2466d917..463d3310 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -8,6 +8,7 @@ import java.util.Optional; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -30,14 +31,10 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - -import com.ctre.phoenix6.controls.DutyCycleOut; - public class SpindexerImpl extends Spindexer { private final Motors.TalonFXConfig spindexerLeadConfig; @@ -180,7 +177,7 @@ public void periodicAfterScheduler() { SmartDashboard.putBoolean("Spindexer/Should Stop?", shouldStop()); if (Settings.DEBUG_MODE.get()) { - if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { SmartDashboard.putBoolean( "Robot/CAN/Canivore/Spindexer Leader Motor Connected? (ID " + String.valueOf(Ports.Spindexer.MOTOR) + ")", diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 270cbb67..0950ddef 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -5,8 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.hood; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import java.util.Optional; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; @@ -16,24 +23,16 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.SysId; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import java.util.Optional; - public class HoodImpl extends Hood { private final Motors.TalonFXConfig hoodConfig; // private final Motors.CANCoderConfig hoodEncoderConfig; @@ -195,7 +194,7 @@ public void periodicAfterScheduler() { SmartDashboard.putBoolean("Superstructure/Hood/is stalling", isStalling()); Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); - if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { SmartDashboard.putBoolean("Robot/CAN/Rio/Hood Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Hood.MOTOR) + ")", hoodMotor.isConnected()); // SmartDashboard.putBoolean("Robot/CAN/Rio/Hood Encoder Connected? (ID " + String.valueOf(hoodEncoder.getDeviceID()) + ")", hoodEncoder.isConnected()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 5cce22f6..e1f454f7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -27,7 +27,6 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -159,7 +158,7 @@ public void periodicAfterScheduler() { SmartDashboard.putNumber("Superstructure/Shooter/Follower Stator Current (amps)", shooterFollowStatorCurrent.getValueAsDouble()); - if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { SmartDashboard.putBoolean( "Robot/CAN/Main/Shooter Leader Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Shooter.MOTOR_LEAD) + ")", diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index e9c010ff..fbbb3d88 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -5,9 +5,19 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.turret; +import java.util.Optional; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.stuypulse.robot.Robot; -import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.Robot.RobotMode; +import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.DriverConstants; import com.stuypulse.robot.constants.Gains; @@ -15,7 +25,6 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.EnergyUtil; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.SysId; import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; @@ -24,21 +33,9 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; - -import java.util.Optional; - public class TurretImpl extends Turret { private final Motors.TalonFXConfig turretConfig; private final Motors.CANCoderConfig encoder17tConfig; @@ -299,7 +296,7 @@ public void periodicAfterScheduler() { SmartDashboard.putNumber("Superstructure/Turret/Supply Current (amps)", turretMotorSupplyCurrent.getValueAsDouble()); - if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { SmartDashboard.putBoolean( "Robot/CAN/Main/Turret Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Turret.MOTOR) + ")", turretMotor.isConnected()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index b58a9da9..dbed2787 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -49,7 +49,6 @@ import edu.wpi.first.networktables.StructPublisher; import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Volts; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -737,7 +736,7 @@ public void periodicAfterScheduler() { Robot.getEnergyUtil().logEnergyUsage(getName() + " Turn", getTotalSteerSupplyCurrent()); // CAN SIGNAL LOGGING - if (Settings.DEBUG_MODE.get() && Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + if (Settings.DEBUG_MODE.get() && Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { SmartDashboard.putBoolean( "Robot/CAN/Canivore/Front Left Drive Motor Connected? (ID " + String.valueOf(TunerConstants.kFrontLeftDriveMotorId) + ")", From fb49ae598327c297b08e10fe3705973d86509fac Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 11 Apr 2026 17:49:43 -0400 Subject: [PATCH 51/58] fix: paths, commented out switching intake torqueCurrent pushdown --- .../pathplanner/paths/Depot To Score.path | 4 +- .../paths/Left Bump Score To Depot.path | 4 +- .../pathplanner/paths/Left Bump To NZ.path | 4 +- .../paths/Left Middy To Bump Score.path | 4 +- .../pathplanner/paths/Left NZ To Score.path | 18 ++++----- .../paths/Left Score To NZ (F).path | 4 +- .../paths/Left Score To Score.path | 38 +++++++++---------- .../pathplanner/paths/Left Trench To NZ.path | 4 +- .../paths/Right Bump Score To Depot.path | 4 +- .../pathplanner/paths/Right Bump To NZ.path | 4 +- .../paths/Right Middy To Bump Score.path | 4 +- .../pathplanner/paths/Right NZ To Score.path | 18 ++++----- .../paths/Right Score To NZ (F).path | 4 +- .../paths/Right Score To Score.path | 10 ++--- .../pathplanner/paths/Right Trench To NZ.path | 4 +- src/main/deploy/pathplanner/settings.json | 4 +- src/main/java/com/stuypulse/robot/Robot.java | 2 + .../com/stuypulse/robot/constants/Gains.java | 2 +- .../stuypulse/robot/constants/Settings.java | 5 ++- .../robot/subsystems/intake/IntakeImpl.java | 9 ++++- 20 files changed, 79 insertions(+), 71 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Depot To Score.path b/src/main/deploy/pathplanner/paths/Depot To Score.path index 84570452..1dd5f402 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Score.path +++ b/src/main/deploy/pathplanner/paths/Depot To Score.path @@ -52,11 +52,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path index 7bb6954b..1344dbad 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path @@ -52,11 +52,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Left Bump To NZ.path b/src/main/deploy/pathplanner/paths/Left Bump To NZ.path index 75261865..097508b0 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Bump To NZ.path @@ -63,11 +63,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path b/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path index 8ccc86c4..9e7c56aa 100644 --- a/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path +++ b/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path @@ -33,11 +33,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index bf306c4c..de38508d 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -20,12 +20,12 @@ "y": 6.378129770992366 }, "prevControl": { - "x": 6.075124555160143, - "y": 5.761850533807828 + "x": 6.055976129616249, + "y": 5.761441920527078 }, "nextControl": { - "x": 6.006129596458399, - "y": 7.632552869140023 + "x": 6.044550641940085, + "y": 7.7289871611982885 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 7.440906488549619 }, "prevControl": { - "x": 5.8818827069938795, - "y": 7.504839857651245 + "x": 6.018673323823109, + "y": 7.444336661911555 }, "nextControl": null, "isLocked": false, @@ -50,7 +50,7 @@ "rotationDegrees": -90.0 }, { - "waypointRelativePos": 1.6, + "waypointRelativePos": 1.3432835820895521, "rotationDegrees": 0.0 } ], @@ -58,11 +58,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index dc204719..2e148f18 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -38,11 +38,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index 04316942..eaa8033c 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.893246882557342, - "y": 7.472562277580071 + "x": 6.717360912981454, + "y": 7.521968616262482 }, "isLocked": false, "linkedName": "Left Trench Score" @@ -32,32 +32,32 @@ }, { "anchor": { - "x": 7.185259365994238, - "y": 4.518559077809798 + "x": 6.937318116975749, + "y": 4.571954350927247 }, "prevControl": { - "x": 6.8149542891317, - "y": 4.24230493639618 + "x": 6.567013040113212, + "y": 4.295700209513629 }, "nextControl": { - "x": 7.826973879910546, - "y": 4.997289462973852 + "x": 7.5790326308920575, + "y": 5.0506847360913 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.185259365994238, - "y": 7.366704707560627 + "x": 7.0667047075606275, + "y": 7.440906488549619 }, "prevControl": { - "x": 7.483027922040206, - "y": 7.358919908709752 + "x": 7.3644732636065955, + "y": 7.433121689698743 }, "nextControl": { - "x": 5.205644530045594, - "y": 7.418459343794579 + "x": 5.087089871611983, + "y": 7.49266112478357 }, "isLocked": false, "linkedName": null @@ -86,19 +86,19 @@ "rotationDegrees": -90.0 }, { - "waypointRelativePos": 0.9593360995850618, + "waypointRelativePos": 0.9, "rotationDegrees": -90.0 }, { - "waypointRelativePos": 2.2942430703624668, + "waypointRelativePos": 2.3, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 2.6268656716418013, + "waypointRelativePos": 2.5, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 3.1047812817904377, + "waypointRelativePos": 3.2, "rotationDegrees": 0.0 } ], @@ -124,4 +124,4 @@ "rotation": 0.0 }, "useDefaultConstraints": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index c6f5b517..4fce49ab 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -52,11 +52,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path index 0ef89ef7..b11d1b8e 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path @@ -68,11 +68,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Right Bump To NZ.path b/src/main/deploy/pathplanner/paths/Right Bump To NZ.path index bb98e1a0..4b0a9250 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Bump To NZ.path @@ -63,11 +63,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path b/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path index cf4dfedf..d461f0f7 100644 --- a/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path +++ b/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path @@ -33,11 +33,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 15f637d7..b5d6b858 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -20,12 +20,12 @@ "y": 1.6151808747904899 }, "prevControl": { - "x": 6.053606168446026, - "y": 2.684721233689205 + "x": 6.040480921648686, + "y": 2.6846376869648685 }, "nextControl": { - "x": 6.054109349362734, - "y": 0.48903252043804946 + "x": 6.070427960057061, + "y": 0.289258202567761 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 0.5868473609129818 }, "prevControl": { - "x": 6.203885619423932, - "y": 0.6137698161573371 + "x": 6.290385164051354, + "y": 0.6127246790299581 }, "nextControl": null, "isLocked": false, @@ -50,7 +50,7 @@ "rotationDegrees": 90.0 }, { - "waypointRelativePos": 1.6376554174067646, + "waypointRelativePos": 1.488272921108742, "rotationDegrees": 0.0 } ], @@ -58,11 +58,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index fe329dc4..82a73da8 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -38,11 +38,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 314305a2..38bdf06e 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.892473994275653, - "y": 0.7106025563708602 + "x": 6.575035663338087, + "y": 0.6515406562054205 }, "isLocked": false, "linkedName": "Right Trench Score" @@ -52,7 +52,7 @@ "y": 0.5868473609129818 }, "prevControl": { - "x": 7.673089129599269, + "x": 7.673089129599266, "y": 0.5487960706826538 }, "nextControl": null, @@ -70,11 +70,11 @@ "rotationDegrees": 90.0 }, { - "waypointRelativePos": 1.031518624641834, + "waypointRelativePos": 0.9, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 1.9829424307036287, + "waypointRelativePos": 2.05, "rotationDegrees": -90.0 }, { diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index de8abb4b..ad2922fd 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -52,11 +52,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.75, + "maxVelocity": 4.0, "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index beb42229..bcf7dddd 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -8,11 +8,11 @@ "To Score" ], "autoFolders": [], - "defaultMaxVel": 3.75, + "defaultMaxVel": 4.0, "defaultMaxAccel": 7.0, "defaultMaxAngVel": 300.0, "defaultMaxAngAccel": 900.0, - "defaultNominalVoltage": 12.5, + "defaultNominalVoltage": 12.7, "robotMass": 64.86, "robotMOI": 6.883, "robotTrackwidth": 0.546, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 0dd44eee..a3052a43 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -43,6 +43,7 @@ import com.pathplanner.lib.commands.FollowPathCommand; import com.pathplanner.lib.commands.PathfindingCommand; import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.vision.BlackListAllTagsForAllCameras; @@ -205,6 +206,7 @@ public void teleopInit() { fmsUtil.restartTimer(false); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); + CommandScheduler.getInstance().schedule(new IntakeDeploy()); if (auto != null) { auto.cancel(); diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 0eb92fb6..ac914d08 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -115,7 +115,7 @@ public interface Turn { } public interface Alignment { - PIDConstants XY = new PIDConstants(4.0, 0, 0); + PIDConstants XY = new PIDConstants(5.0, 0, 0); // 4.0 P PIDConstants THETA = new PIDConstants(4.0, 0, 0.0); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index dc5718df..1d1d00c5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -73,8 +73,9 @@ public interface Intake { Rotation2d ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE = Rotation2d.fromDegrees(15.0); double HOMING_VOLTAGE = 3.0; - double PUSHDOWN_VOLTAGE = 3.0; - double PUSHDOWN_CURRENT = 15; //TODO: GET ACTUAL TYTY + double PUSHDOWN_VOLTAGE = -3.0; + double PUSHDOWN_CURRENT_TELEOP = -65.0;//new SmartNumber("Intake/Pushdown Current", -65.0); //TODO: GET ACTUAL TYTY + double PUSHDOWN_CURRENT_AUTON = -80.0; double GEAR_RATIO = 37.93; diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index ad44f31f..e9c58d87 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -187,8 +187,13 @@ public void periodicAfterScheduler() { if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE.getDegrees() && rollerState != RollerState.STOP) { - // pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts - pivot.setControl(new TorqueCurrentFOC(Settings.Intake.PUSHDOWN_CURRENT)); + // pivot.setControl(new VoltageOut(Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts + // double pushdownCurrent = + // Robot.getMode() == RobotMode.AUTON ? + // Settings.Intake.PUSHDOWN_CURRENT_AUTON : Settings.Intake.PUSHDOWN_CURRENT_TELEOP; + + + pivot.setControl(new TorqueCurrentFOC(Settings.Intake.PUSHDOWN_CURRENT_TELEOP)); applyingPushdownVoltage = true; } else if (pivotState == PivotState.HOMING) { pivot.setControl(new VoltageOut(-Settings.Intake.HOMING_VOLTAGE)); From c719ef7c76bcd84fa1c9cf6bb1fc9194eb7132ef Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 11 Apr 2026 17:53:22 -0400 Subject: [PATCH 52/58] feat: need to do offsetting on hdr start, ts commit made pipelines a camera level thing --- .../stuypulse/robot/constants/Cameras.java | 34 +++++++++++++++ .../subsystems/vision/LimelightVision.java | 41 +++++++------------ 2 files changed, 48 insertions(+), 27 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 9a2502ee..5176c811 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -6,6 +6,7 @@ package com.stuypulse.robot.constants; import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.math.geometry.Pose3d; @@ -45,6 +46,24 @@ public static class Camera { private int rejectedCounterInvalidPosition; private int rejectedCounterTargetArea; + private Pipeline currentPipeline; + + public enum Pipeline { + NO_SUN, + LOW_SUN, + MED_SUN, + HIGH_SUN + } + + private int getCurrentPipelineID() { + return switch(this.currentPipeline) { + case NO_SUN -> 3; + case LOW_SUN -> 2; + case MED_SUN -> 1; + case HIGH_SUN -> 0; + }; + } + public enum RejectionValue { NOT_NULL, ANGULAR_VELOCITY, @@ -52,6 +71,21 @@ public enum RejectionValue { TARGET_AREA }; + public void setPipeline(Pipeline pipeline) { + this.currentPipeline = pipeline; + LimelightHelpers.setPipelineIndex(name, getCurrentPipelineID()); + } + + public void performHDR() { + Pipeline nextHdrPipeline = Pipeline.NO_SUN; + + if (currentPipeline == Pipeline.NO_SUN) { + nextHdrPipeline = Pipeline.HIGH_SUN; + } + + setPipeline(nextHdrPipeline); + } + public void incrementRejection(RejectionValue rejectionValue) { switch (rejectionValue) { case NOT_NULL: diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 649eb571..7cbf728e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -12,6 +12,8 @@ import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Cameras.Camera; +import com.stuypulse.robot.constants.Cameras.Camera.Pipeline; import com.stuypulse.robot.constants.Cameras.Camera.RejectionValue; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.vision.LimelightHelpers; @@ -56,34 +58,17 @@ public static LimelightVision getInstance() { private boolean hasData; private BStream debouncedHasData; - private Pipeline currentPipeline; private Timer hdrTimer = new Timer(); + private boolean lastHdrEnabledVal = false; public enum MegaTagMode { MEGATAG1, MEGATAG2 } - public enum Pipeline { - NO_SUN, - LOW_SUN, - MED_SUN, - HIGH_SUN - } - - private int getCurrentPipelineID() { - return switch(this.currentPipeline) { - case NO_SUN -> 3; - case LOW_SUN -> 2; - case MED_SUN -> 1; - case HIGH_SUN -> 0; - }; - } - public void setPipeline(Pipeline pipeline) { - for(String camName: names) { - this.currentPipeline = pipeline; - LimelightHelpers.setPipelineIndex(camName, getCurrentPipelineID()); + for(Camera camera: Cameras.LimelightCameras) { + camera.setPipeline(pipeline); } } @@ -347,15 +332,17 @@ public void periodicAfterScheduler() { } } - if (Settings.Vision.HDR_ENABLED.get()) { - Pipeline nextHdrPipeline = Pipeline.NO_SUN; - if (!hdrTimer.hasElapsed(Settings.Vision.HDR_TIMEOUT_SEC)) { + if(lastHdrEnabledVal != Settings.Vision.HDR_ENABLED.get()) { + //TODO: Offset if lastHdr was false, otherwise make them all the same + } - if (currentPipeline == Pipeline.NO_SUN) { - nextHdrPipeline = Pipeline.HIGH_SUN; - } + lastHdrEnabledVal = Settings.Vision.HDR_ENABLED.get(); - setPipeline(nextHdrPipeline); + if (Settings.Vision.HDR_ENABLED.get()) { + if(hdrTimer.hasElapsed(Settings.Vision.HDR_TIMEOUT_SEC)) { + for(Camera camera: Cameras.LimelightCameras) { + camera.performHDR(); + } hdrTimer.reset(); } } From e7d606c85126cf121324d24dc02c126afb9fccd7 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 11 Apr 2026 18:19:53 -0400 Subject: [PATCH 53/58] feat: pushdown current tele/auto switching, pushdown tele up to 75 --- .../com/stuypulse/robot/constants/Settings.java | 2 +- .../robot/subsystems/intake/IntakeImpl.java | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 1d1d00c5..1c8a8e91 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -74,7 +74,7 @@ public interface Intake { double HOMING_VOLTAGE = 3.0; double PUSHDOWN_VOLTAGE = -3.0; - double PUSHDOWN_CURRENT_TELEOP = -65.0;//new SmartNumber("Intake/Pushdown Current", -65.0); //TODO: GET ACTUAL TYTY + double PUSHDOWN_CURRENT_TELEOP = -75.0;//new SmartNumber("Intake/Pushdown Current", -65.0); //TODO: GET ACTUAL TYTY double PUSHDOWN_CURRENT_AUTON = -80.0; double GEAR_RATIO = 37.93; diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 724ea4f5..e231a31d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -184,7 +184,7 @@ public void periodicAfterScheduler() { // Gains.Intake.Pivot.kV, // Gains.Intake.Pivot.kA); - boolean applyingPushdownVoltage = false; + boolean applyingPushdownCurrent = false; if (EnabledSubsystems.INTAKE.get()) { if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); @@ -194,11 +194,11 @@ public void periodicAfterScheduler() { getPivotAngle().getDegrees() <= Settings.Intake.ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE.getDegrees() && rollerState != RollerState.STOP) { // pivot.setControl(new VoltageOut(Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts - // double pushdownCurrent = - // Robot.getMode() == RobotMode.AUTON ? - // Settings.Intake.PUSHDOWN_CURRENT_AUTON : Settings.Intake.PUSHDOWN_CURRENT_TELEOP; - pivot.setControl(torqueCurrentFOC.withOutput(Settings.Intake.PUSHDOWN_CURRENT_TELEOP)); - applyingPushdownVoltage = true; + double pushdownCurrent = + Robot.getMode() == RobotMode.AUTON ? + Settings.Intake.PUSHDOWN_CURRENT_AUTON : Settings.Intake.PUSHDOWN_CURRENT_TELEOP; + pivot.setControl(torqueCurrentFOC.withOutput(pushdownCurrent)); + applyingPushdownCurrent = true; } else if (pivotState == PivotState.HOMING) { pivot.setControl(voltageOut.withOutput(-Settings.Intake.HOMING_VOLTAGE)); } else { @@ -230,7 +230,7 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { // PIVOT - SmartDashboard.putBoolean("Intake/Pivot Pushdown Voltage Applied?", applyingPushdownVoltage); + SmartDashboard.putBoolean("Intake/Pivot Pushdown Voltage Applied?", applyingPushdownCurrent); SmartDashboard.putNumber("Intake/Pivot Closed Loop Error (deg)", pivot.getClosedLoopError().getValueAsDouble() * 360.0); From 33f5487ec73c74ae43aad6467acea5d64e96cb3c Mon Sep 17 00:00:00 2001 From: Brian Date: Sat, 11 Apr 2026 20:27:08 -0400 Subject: [PATCH 54/58] FIX: pivot supply current logging --- .../com/stuypulse/robot/subsystems/intake/IntakeImpl.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index e231a31d..15e217da 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -131,10 +131,10 @@ public IntakeImpl() { pivotMotorVoltage = pivot.getMotorVoltage(); rollerLeaderVoltage = rollerLeader.getMotorVoltage(); rollerFollowerVoltage = rollerFollower.getMotorVoltage(); - PhoenixUtil.registerToRio(pivotStatorCurrent, rollerLeaderSupplyCurrent, + PhoenixUtil.registerToRio(pivotSupplyCurrent, pivotStatorCurrent, pivotMotorPosition, rollerLeaderSupplyCurrent, rollerLeaderStatorCurrent, rollerFollowerSupplyCurrent, rollerFollowerStatorCurrent, rollerLeaderTemperature, rollerFollowerTemperature, pivotTemperature, pivotMotorVoltage, - rollerLeaderVoltage, rollerFollowerVoltage, pivotMotorPosition ); + rollerLeaderVoltage, rollerFollowerVoltage); pivotStalling = BStream.create( () -> Math.abs(pivotSupplyCurrent.getValueAsDouble()) > Settings.Intake.STALL_CURRENT_LIMIT) From 2c51735d36a3793c583bbe74a96d343469c1fdc3 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sun, 12 Apr 2026 08:52:40 -0400 Subject: [PATCH 55/58] feat: auton changes + log imu + robot accel --- .../pathplanner/paths/Depot To Score.path | 4 +- .../paths/Left Bump Score To Depot.path | 4 +- .../pathplanner/paths/Left Bump To NZ.path | 6 +-- .../paths/Left Middy To Bump Score.path | 4 +- .../pathplanner/paths/Left NZ To Score.path | 4 +- .../paths/Left Score To NZ (F).path | 4 +- .../paths/Left Score To Score.path | 2 +- .../pathplanner/paths/Left Trench To NZ.path | 10 ++--- .../paths/Right Bump Score To Depot.path | 4 +- .../pathplanner/paths/Right Bump To NZ.path | 6 +-- .../paths/Right Middy To Bump Score.path | 4 +- .../pathplanner/paths/Right NZ To Score.path | 4 +- .../paths/Right Score To NZ (F).path | 4 +- .../pathplanner/paths/Right Trench To NZ.path | 10 ++--- src/main/deploy/pathplanner/settings.json | 4 +- .../com/stuypulse/robot/constants/Gains.java | 2 +- .../swerve/CommandSwerveDrivetrain.java | 42 +++++++++++++------ 17 files changed, 68 insertions(+), 50 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Depot To Score.path b/src/main/deploy/pathplanner/paths/Depot To Score.path index 1dd5f402..d3c9f452 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Score.path +++ b/src/main/deploy/pathplanner/paths/Depot To Score.path @@ -52,8 +52,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path index 1344dbad..01589c7e 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path @@ -52,8 +52,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Left Bump To NZ.path b/src/main/deploy/pathplanner/paths/Left Bump To NZ.path index 097508b0..ea3c532e 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Bump To NZ.path @@ -55,7 +55,7 @@ "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false } } @@ -63,8 +63,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path b/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path index 9e7c56aa..7a194703 100644 --- a/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path +++ b/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index de38508d..52c62217 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -58,8 +58,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index 2e148f18..4bd6d40f 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index eaa8033c..8a3f8436 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -107,7 +107,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.5, - "maxAcceleration": 7.5, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 4fce49ab..3568773d 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -37,14 +37,14 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.65, + "minWaypointRelativePos": 0.55, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 1.5, - "maxAcceleration": 7.0, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false } } @@ -52,8 +52,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path index b11d1b8e..70b3af07 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path @@ -68,8 +68,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Right Bump To NZ.path b/src/main/deploy/pathplanner/paths/Right Bump To NZ.path index 4b0a9250..e249ebb2 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Bump To NZ.path @@ -55,7 +55,7 @@ "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false } } @@ -63,8 +63,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path b/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path index d461f0f7..b9e05c6d 100644 --- a/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path +++ b/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index b5d6b858..c569aff8 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -58,8 +58,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 82a73da8..71ca80d6 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index ad2922fd..13b73e23 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -37,14 +37,14 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.65, + "minWaypointRelativePos": 0.55, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 1.5, - "maxAcceleration": 7.0, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "nominalVoltage": 12.7, "unlimited": false } } @@ -52,8 +52,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index bcf7dddd..a693d1fb 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -8,8 +8,8 @@ "To Score" ], "autoFolders": [], - "defaultMaxVel": 4.0, - "defaultMaxAccel": 7.0, + "defaultMaxVel": 4.19, + "defaultMaxAccel": 10.0, "defaultMaxAngVel": 300.0, "defaultMaxAngAccel": 900.0, "defaultNominalVoltage": 12.7, diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index ac914d08..0c9bc260 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -115,7 +115,7 @@ public interface Turn { } public interface Alignment { - PIDConstants XY = new PIDConstants(5.0, 0, 0); // 4.0 P + PIDConstants XY = new PIDConstants(5.0, 0, 0); PIDConstants THETA = new PIDConstants(4.0, 0, 0.0); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index dbed2787..d711b968 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -6,6 +6,7 @@ package com.stuypulse.robot.subsystems.swerve; import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -32,6 +33,7 @@ import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.swerve.TunerConstants.TunerSwerveDrivetrain; +import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.math.Vector2D; @@ -47,6 +49,9 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.LinearAcceleration; + import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.wpilibj.Notifier; @@ -69,6 +74,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private StructPublisher leftBehindHubYPlublisher; private StructPublisher rightBehindHubYPlublisher; private StructPublisher vertexBehindHubPublisher; + private StatusSignal robotAccelerationX; + private StatusSignal robotAccelerationY; private StructPublisher robotPose = NetworkTableInstance.getDefault() .getStructTopic("Robot Pose", Pose2d.struct).publish(); @@ -78,18 +85,18 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su // instance.registerTelemetry(instance::telemeterize); } - public void telemeterize(SwerveDriveState state) { - /* Write drive state to the log file */ - SignalLogger.writeStruct("DriveState/Pose", Pose2d.struct, state.Pose); - SignalLogger.writeStruct("DriveState/Speeds", ChassisSpeeds.struct, state.Speeds); - SignalLogger.writeStructArray("DriveState/ModuleStates", SwerveModuleState.struct, state.ModuleStates); - SignalLogger.writeStructArray("DriveState/ModuleTargets", SwerveModuleState.struct, state.ModuleTargets); - SignalLogger.writeStructArray("DriveState/ModulePositions", SwerveModulePosition.struct, state.ModulePositions); - SignalLogger.writeStruct("DriveState/RawHeading", Rotation2d.struct, state.RawHeading); - SignalLogger.writeDouble("DriveState/Timestamp", state.Timestamp, "seconds"); - SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - SignalLogger.writeInteger("DriveState/FailedDaqs", state.FailedDaqs); - } + // public void telemeterize(SwerveDriveState state) { + // /* Write drive state to the log file */ + // SignalLogger.writeStruct("DriveState/Pose", Pose2d.struct, state.Pose); + // SignalLogger.writeStruct("DriveState/Speeds", ChassisSpeeds.struct, state.Speeds); + // SignalLogger.writeStructArray("DriveState/ModuleStates", SwerveModuleState.struct, state.ModuleStates); + // SignalLogger.writeStructArray("DriveState/ModuleTargets", SwerveModuleState.struct, state.ModuleTargets); + // SignalLogger.writeStructArray("DriveState/ModulePositions", SwerveModulePosition.struct, state.ModulePositions); + // SignalLogger.writeStruct("DriveState/RawHeading", Rotation2d.struct, state.RawHeading); + // SignalLogger.writeDouble("DriveState/Timestamp", state.Timestamp, "seconds"); + // SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); + // SignalLogger.writeInteger("DriveState/FailedDaqs", state.FailedDaqs); + // } public static CommandSwerveDrivetrain getInstance() { return instance; @@ -239,6 +246,14 @@ protected CommandSwerveDrivetrain( leftBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/LeftBehindHubY", Pose2d.struct).publish(); rightBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/RightBehindHubY", Pose2d.struct).publish(); vertexBehindHubPublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/VertexBehindHub", Pose2d.struct).publish(); + + robotAccelerationX = this.getPigeon2().getAccelerationX(); + robotAccelerationY = this.getPigeon2().getAccelerationY(); + + PhoenixUtil.registerToCanivore( + robotAccelerationX, + robotAccelerationY + ); } /** @@ -699,6 +714,9 @@ public void periodicAfterScheduler() { Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { + SmartDashboard.putNumber("Swerve/Robot Accel X", robotAccelerationX.getValueAsDouble()); + SmartDashboard.putNumber("Swerve/Robot Accel Y", robotAccelerationY.getValueAsDouble()); + SmartDashboard.putNumber("Swerve/Failed DAQ Count", this.getState().FailedDaqs); SmartDashboard.putNumber("Swerve/CANBus Utiliaztion", Ports.CANIVORE.getStatus().BusUtilization); // will confirm whether we are even getting data From 69e09371e6ba94669d4d4b1ec60c7665f1834e8a Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sun, 12 Apr 2026 11:43:33 -0400 Subject: [PATCH 56/58] fix: properly log robot accel --- .../deploy/pathplanner/paths/Right Score To Score.path | 8 ++++---- .../robot/subsystems/swerve/CommandSwerveDrivetrain.java | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 38bdf06e..d0f23ac2 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 5.928102710413694, + "x": 5.850470756062768, "y": 2.851112696148359 }, "prevControl": { - "x": 5.928102710413694, + "x": 5.850470756062768, "y": 0.3280741797432247 }, "nextControl": { - "x": 5.928102710413694, + "x": 5.850470756062768, "y": 4.150659142168011 }, "isLocked": false, @@ -52,7 +52,7 @@ "y": 0.5868473609129818 }, "prevControl": { - "x": 7.673089129599266, + "x": 7.673089129599265, "y": 0.5487960706826538 }, "nextControl": null, diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index d711b968..4453584c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -714,8 +714,8 @@ public void periodicAfterScheduler() { Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { - SmartDashboard.putNumber("Swerve/Robot Accel X", robotAccelerationX.getValueAsDouble()); - SmartDashboard.putNumber("Swerve/Robot Accel Y", robotAccelerationY.getValueAsDouble()); + SmartDashboard.putNumber("Swerve/Robot Accel X", robotAccelerationX.getValueAsDouble() * 9.81); + SmartDashboard.putNumber("Swerve/Robot Accel Y", robotAccelerationY.getValueAsDouble() * 9.81); SmartDashboard.putNumber("Swerve/Failed DAQ Count", this.getState().FailedDaqs); SmartDashboard.putNumber("Swerve/CANBus Utiliaztion", Ports.CANIVORE.getStatus().BusUtilization); From 270b798e15fab805cc0f97c71d4ddc9e5b66ecfd Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sun, 12 Apr 2026 11:55:16 -0400 Subject: [PATCH 57/58] feat: alternating pipelines for hdr --- src/main/java/com/stuypulse/robot/RobotContainer.java | 2 +- .../com/stuypulse/robot/commands/vision/SetPipeline.java | 3 +-- .../robot/subsystems/vision/LimelightVision.java | 8 ++++++-- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index d5a29c30..605ae076 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -55,6 +55,7 @@ import com.stuypulse.robot.commands.vision.WhitelistAllTagsForAllCameras; import com.stuypulse.robot.commands.vision.WhitelistOutpostTags; import com.stuypulse.robot.commands.vision.WhitelistTowerTags; +import com.stuypulse.robot.constants.Cameras.Camera.Pipeline; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -72,7 +73,6 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; -import com.stuypulse.robot.subsystems.vision.LimelightVision.Pipeline; import com.stuypulse.robot.util.PathUtil.AutonConfig; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetPipeline.java b/src/main/java/com/stuypulse/robot/commands/vision/SetPipeline.java index cf82ff98..e20f816d 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/SetPipeline.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetPipeline.java @@ -5,9 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.commands.vision; +import com.stuypulse.robot.constants.Cameras.Camera.Pipeline; import com.stuypulse.robot.subsystems.vision.LimelightVision; -import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; -import com.stuypulse.robot.subsystems.vision.LimelightVision.Pipeline; import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 7cbf728e..5e8e10cd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -332,8 +332,12 @@ public void periodicAfterScheduler() { } } - if(lastHdrEnabledVal != Settings.Vision.HDR_ENABLED.get()) { - //TODO: Offset if lastHdr was false, otherwise make them all the same + // Alternating pipelines for hdr + if (lastHdrEnabledVal != Settings.Vision.HDR_ENABLED.get()) { + setPipeline(Pipeline.NO_SUN); + if(!lastHdrEnabledVal) { + Cameras.LimelightCameras[2].setPipeline(Pipeline.HIGH_SUN); + } } lastHdrEnabledVal = Settings.Vision.HDR_ENABLED.get(); From a34dcac8f54a919f7178dc3681781c9245913f1d Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sun, 12 Apr 2026 14:33:18 -0400 Subject: [PATCH 58/58] fix: actually start hdr timer so that hdr works properly now --- .../stuypulse/robot/subsystems/vision/LimelightVision.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 5e8e10cd..b41a813e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -103,6 +103,7 @@ public LimelightVision() { setIMUMode(1); hdrTimer.reset(); + hdrTimer.start(); debouncedHasData = BStream.create( () -> hasData) @@ -336,13 +337,13 @@ public void periodicAfterScheduler() { if (lastHdrEnabledVal != Settings.Vision.HDR_ENABLED.get()) { setPipeline(Pipeline.NO_SUN); if(!lastHdrEnabledVal) { - Cameras.LimelightCameras[2].setPipeline(Pipeline.HIGH_SUN); + Cameras.LimelightCameras[2].setPipeline(Pipeline.HIGH_SUN); } } lastHdrEnabledVal = Settings.Vision.HDR_ENABLED.get(); - if (Settings.Vision.HDR_ENABLED.get()) { + if(Settings.Vision.HDR_ENABLED.get()) { if(hdrTimer.hasElapsed(Settings.Vision.HDR_TIMEOUT_SEC)) { for(Camera camera: Cameras.LimelightCameras) { camera.performHDR();