From 9e7f662ba5695790d3a51a3483071021011a4a9d Mon Sep 17 00:00:00 2001 From: Nolan Barker <107814443+TheRekrab@users.noreply.github.com> Date: Wed, 8 Apr 2026 18:19:49 -0400 Subject: [PATCH 01/26] dont look at this commit please, its a secret --- .../robot/binding/OperatorPS5Bindings.java | 7 +++++++ .../java/frc/robot/subsystems/led/Led.java | 1 + .../subsystems/led/ledStates/PartyMode.java | 20 +++++++++++++++++++ .../robot/superstructure/StateManager.java | 7 +++++++ .../java/frc/robot/util/TriggerSequence.java | 14 +++++++++---- 5 files changed, 45 insertions(+), 4 deletions(-) create mode 100644 CompBot/src/main/java/frc/robot/subsystems/led/ledStates/PartyMode.java diff --git a/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java b/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java index 9332bf7..1f78f92 100644 --- a/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java +++ b/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java @@ -1,5 +1,7 @@ package frc.robot.binding; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.binding.BindingConstants.Operator; @@ -9,6 +11,7 @@ import frc.robot.commands.RetractIntake; import frc.robot.commands.RunIndex; import frc.robot.superstructure.Superstructure; +import frc.robot.util.TriggerSequence; public class OperatorPS5Bindings implements Binder { private final CommandPS5Controller controller; @@ -31,5 +34,9 @@ public void bind(Superstructure superstructure) { agitate.whileTrue(superstructure.build(new AgitateIntake())); retract.onTrue(superstructure.build(new RetractIntake())); shootTillEmpty.onTrue(superstructure.build(new EmptyHopper())); + + // obfuscation is how we beat the llms + Trigger ianSecret = TriggerSequence.fromController(controller, 4, 4, 2, 2, 1, 3, 1, 3, 11, 12); + ianSecret.onTrue(superstructure.state.runPartyMode(5.0)); } } diff --git a/CompBot/src/main/java/frc/robot/subsystems/led/Led.java b/CompBot/src/main/java/frc/robot/subsystems/led/Led.java index f347bc6..7b259dc 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/led/Led.java +++ b/CompBot/src/main/java/frc/robot/subsystems/led/Led.java @@ -25,6 +25,7 @@ public Led(LedIO io) { private List hierarchy = List.of( new BadController(), + new PartyMode(), new TestRslEnabled(), new TestRsl(), new EndGameAlert(), diff --git a/CompBot/src/main/java/frc/robot/subsystems/led/ledStates/PartyMode.java b/CompBot/src/main/java/frc/robot/subsystems/led/ledStates/PartyMode.java new file mode 100644 index 0000000..6f5f295 --- /dev/null +++ b/CompBot/src/main/java/frc/robot/subsystems/led/ledStates/PartyMode.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.led.ledStates; + +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.signals.RGBWColor; + +import edu.wpi.first.wpilibj.util.Color; +import frc.robot.subsystems.led.LedIO; +import frc.robot.subsystems.led.LedState; +import frc.robot.subsystems.led.LedIO.AnimationType; +import frc.robot.superstructure.StateManager; + +public class PartyMode implements LedState { + public boolean check(StateManager state) { + return state.partying.getAsBoolean(); + } + + public ControlRequest apply(LedIO io) { + return io.createAnimation(new RGBWColor(Color.kWhite), AnimationType.Rainbow); + } +} diff --git a/CompBot/src/main/java/frc/robot/superstructure/StateManager.java b/CompBot/src/main/java/frc/robot/superstructure/StateManager.java index c2babbd..a6730a7 100644 --- a/CompBot/src/main/java/frc/robot/superstructure/StateManager.java +++ b/CompBot/src/main/java/frc/robot/superstructure/StateManager.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.AimConstants; import frc.robot.aiming.AimParams; @@ -41,6 +42,9 @@ public enum ShootMode { private SendableChooser autoChooser = new SendableChooser<>(); + private boolean partyModeEnabled = false; + public final Trigger partying = new Trigger(() -> partyModeEnabled); + public StateManager(Subsystems subsystems) { this.subsystems = subsystems; shootReady = initShootReady(); @@ -179,4 +183,7 @@ public void update() { wantedShootMode = calculateWantedShootMode(); } + public Command runPartyMode(double duration) { + return Commands.startEnd(() -> partyModeEnabled = true, () -> partyModeEnabled = false).withTimeout(duration).ignoringDisable(true); + } } diff --git a/CompBot/src/main/java/frc/robot/util/TriggerSequence.java b/CompBot/src/main/java/frc/robot/util/TriggerSequence.java index 107a388..f9b4183 100644 --- a/CompBot/src/main/java/frc/robot/util/TriggerSequence.java +++ b/CompBot/src/main/java/frc/robot/util/TriggerSequence.java @@ -1,6 +1,7 @@ package frc.robot.util; import java.util.ArrayList; +import java.util.Arrays; import java.util.HashMap; import java.util.List; import java.util.Map; @@ -20,7 +21,7 @@ private TriggerSequence(Trigger[] triggers, int[] sequence) { count = 0; for (int i = 0; i < triggers.length; i++) { - int j = i; + int j = i; // j is the index of the trigger! triggers[j].onTrue(Commands.runOnce(() -> { if (count == sequence.length) { count = 0; @@ -30,7 +31,7 @@ private TriggerSequence(Trigger[] triggers, int[] sequence) { } else { count = 0; } - })); + }).ignoringDisable(true)); } } @@ -65,10 +66,15 @@ public static Trigger fromController(CommandGenericHID controller, int... sequen // We already have an ID for this one. realSequence[i] = seenTriggers.get(triggerId); } else { + // We haven't seen this trigger before + // The index of the newest item in triggers is simply the size of the current seenTriggers (or triggers) list realSequence[i] = seenTriggers.size(); - seenTriggers.put(sequence[i], realSequence[i]); + seenTriggers.put(triggerId, realSequence[i]); + Trigger button = controller.button(triggerId); + triggers.add(button); } } - return create(triggers.toArray(new Trigger[] {}), realSequence); + Trigger[] triggerArray = triggers.toArray(new Trigger[] {}); + return create(triggerArray, realSequence); } } From 8cf0149d6f6e3bb61548e3829c137bffbd5bbda9 Mon Sep 17 00:00:00 2001 From: Nolan Barker <107814443+TheRekrab@users.noreply.github.com> Date: Wed, 8 Apr 2026 18:28:43 -0400 Subject: [PATCH 02/26] wrong code --- .../main/java/frc/robot/binding/OperatorPS5Bindings.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java b/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java index 1f78f92..5f16082 100644 --- a/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java +++ b/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java @@ -22,9 +22,9 @@ public OperatorPS5Bindings() { controller = new CommandPS5Controller(Operator.kOperatorControllerPort); index = controller.R1(); - eject = controller.cross(); - agitate = controller.square(); - retract = controller.triangle(); + eject = controller.pov(180); // down + agitate = controller.pov(270); // left + retract = controller.pov(0); // up shootTillEmpty = controller.L2(); } @@ -36,7 +36,7 @@ public void bind(Superstructure superstructure) { shootTillEmpty.onTrue(superstructure.build(new EmptyHopper())); // obfuscation is how we beat the llms - Trigger ianSecret = TriggerSequence.fromController(controller, 4, 4, 2, 2, 1, 3, 1, 3, 11, 12); + Trigger ianSecret = TriggerSequence.fromController(controller, 4, 4, 2, 2, 1, 3, 1, 3, 12, 11); ianSecret.onTrue(superstructure.state.runPartyMode(5.0)); } } From 1162bdc8518b700727e933a10e30e30f13db47f2 Mon Sep 17 00:00:00 2001 From: 3414 Linux Date: Wed, 8 Apr 2026 19:13:23 -0400 Subject: [PATCH 03/26] Autons updated and the new ones created --- CompBot/simgui-ds.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CompBot/simgui-ds.json b/CompBot/simgui-ds.json index 3140247..a230ebf 100644 --- a/CompBot/simgui-ds.json +++ b/CompBot/simgui-ds.json @@ -6,7 +6,7 @@ }, "System Joysticks": { "window": { - "visible": false + "enabled": false } }, "keyboardJoysticks": [ From fc0eedc872a0ba616a1e2bae69135472e0c4c422 Mon Sep 17 00:00:00 2001 From: 3414 Linux Date: Wed, 8 Apr 2026 19:13:34 -0400 Subject: [PATCH 04/26] Staged them this time --- .../{Alt Left Depot.auto => Alt Right.auto} | 0 .../autos/Copy of FLIP Double.auto | 44 ++++ .../deploy/pathplanner/autos/FLIP Crash.auto | 38 ++++ .../deploy/pathplanner/autos/FLIP Loopy .auto | 57 ++++++ .../{Loop-de-doo.auto => Loopy Depot.auto} | 2 +- .../deploy/pathplanner/autos/Right Depot.auto | 63 ++++++ .../main/deploy/pathplanner/paths/Alt.path | 9 +- ...op-de-doo.path => Center Loop-de-loo.path} | 24 +-- .../main/deploy/pathplanner/paths/Depot.path | 4 +- .../deploy/pathplanner/paths/FLIP Crash.path | 130 ++++++++++++ .../pathplanner/paths/Inverse Right.path | 163 +++++++++++++++ .../main/deploy/pathplanner/paths/Loopy.path | 188 ++++++++++++++++++ .../pathplanner/paths/Middle Depot.path | 8 +- .../deploy/pathplanner/paths/Right Depot.path | 68 +++++++ .../main/deploy/pathplanner/paths/Right.path | 2 +- 15 files changed, 779 insertions(+), 21 deletions(-) rename CompBot/src/main/deploy/pathplanner/autos/{Alt Left Depot.auto => Alt Right.auto} (100%) create mode 100644 CompBot/src/main/deploy/pathplanner/autos/Copy of FLIP Double.auto create mode 100644 CompBot/src/main/deploy/pathplanner/autos/FLIP Crash.auto create mode 100644 CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy .auto rename CompBot/src/main/deploy/pathplanner/autos/{Loop-de-doo.auto => Loopy Depot.auto} (96%) create mode 100644 CompBot/src/main/deploy/pathplanner/autos/Right Depot.auto rename CompBot/src/main/deploy/pathplanner/paths/{Center Loop-de-doo.path => Center Loop-de-loo.path} (85%) create mode 100644 CompBot/src/main/deploy/pathplanner/paths/FLIP Crash.path create mode 100644 CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path create mode 100644 CompBot/src/main/deploy/pathplanner/paths/Loopy.path create mode 100644 CompBot/src/main/deploy/pathplanner/paths/Right Depot.path diff --git a/CompBot/src/main/deploy/pathplanner/autos/Alt Left Depot.auto b/CompBot/src/main/deploy/pathplanner/autos/Alt Right.auto similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/Alt Left Depot.auto rename to CompBot/src/main/deploy/pathplanner/autos/Alt Right.auto diff --git a/CompBot/src/main/deploy/pathplanner/autos/Copy of FLIP Double.auto b/CompBot/src/main/deploy/pathplanner/autos/Copy of FLIP Double.auto new file mode 100644 index 0000000..348447a --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/Copy of FLIP Double.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Inverse Right" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "wait", + "data": { + "waitTime": 10.0 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Crash.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Crash.auto new file mode 100644 index 0000000..7d357d8 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Crash.auto @@ -0,0 +1,38 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "FLIP Crash" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy .auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy .auto new file mode 100644 index 0000000..9a8bf88 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy .auto @@ -0,0 +1,57 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Loopy" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 10.0 + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/Loop-de-doo.auto b/CompBot/src/main/deploy/pathplanner/autos/Loopy Depot.auto similarity index 96% rename from CompBot/src/main/deploy/pathplanner/autos/Loop-de-doo.auto rename to CompBot/src/main/deploy/pathplanner/autos/Loopy Depot.auto index d7649cd..07f1f93 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/Loop-de-doo.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/Loopy Depot.auto @@ -26,7 +26,7 @@ { "type": "path", "data": { - "pathName": "Center Loop-de-doo" + "pathName": "Loopy" } }, { diff --git a/CompBot/src/main/deploy/pathplanner/autos/Right Depot.auto b/CompBot/src/main/deploy/pathplanner/autos/Right Depot.auto new file mode 100644 index 0000000..d439020 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/Right Depot.auto @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "wait", + "data": { + "waitTime": 10.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Right Depot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Alt.path index 692e33c..33d0abe 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Alt.path @@ -31,7 +31,14 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Aim", + "waypointRelativePos": 0, + "endWaypointRelativePos": 0, + "command": null + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-doo.path b/CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-loo.path similarity index 85% rename from CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-doo.path rename to CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-loo.path index 5df84c2..42273a9 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-doo.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-loo.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 3.58620542082739, - "y": 2.6052781740370894 + "x": 0.4129759036144578, + "y": 4.155204819277109 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.0057346647646215, - "y": 2.4888302425106996 + "x": 7.668975903614458, + "y": 2.3193493975903605 }, "prevControl": { - "x": 4.0637691044309765, - "y": 2.0343276645602733 + "x": 3.0137710843373497, + "y": 1.3467831325301218 }, "nextControl": { - "x": 6.613851640513552, - "y": 2.631155492154066 + "x": 8.280326416522545, + "y": 2.4470728615547257 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 5.594108416547789 }, "prevControl": { - "x": 7.259038516478155, - "y": 4.6988913796095435 + "x": 4.019120481927711, + "y": 4.8108674698795175 }, "nextControl": { - "x": 5.7340228245363765, - "y": 5.788188302425108 + "x": 6.316371263922148, + "y": 5.716579758037213 }, "isLocked": false, "linkedName": null diff --git a/CompBot/src/main/deploy/pathplanner/paths/Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Depot.path index 861c3db..bb211ef 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Depot.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.8974401712548474, + "x": 1.8974401712548472, "y": 5.639258950356227 }, "isLocked": false, @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Depot end" } ], "rotationTargets": [], diff --git a/CompBot/src/main/deploy/pathplanner/paths/FLIP Crash.path b/CompBot/src/main/deploy/pathplanner/paths/FLIP Crash.path new file mode 100644 index 0000000..d04fcdb --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/FLIP Crash.path @@ -0,0 +1,130 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4361022727272723, + "y": 2.4168295454545454 + }, + "prevControl": null, + "nextControl": { + "x": 4.521795180722892, + "y": 3.226349397590361 + }, + "isLocked": false, + "linkedName": "Right Start" + }, + { + "anchor": { + "x": 9.089578313253012, + "y": 1.128228915662652 + }, + "prevControl": { + "x": 8.01866265060241, + "y": 1.5434819277108467 + }, + "nextControl": { + "x": 10.021939622203154, + "y": 0.7667010611717786 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.089578313253012, + "y": 6.417240963855421 + }, + "prevControl": { + "x": 9.876373493975905, + "y": 6.7778554216867475 + }, + "nextControl": { + "x": 7.901769602229587, + "y": 5.872828637969683 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6422289156626504, + "y": 2.5816144578313254 + }, + "prevControl": { + "x": 5.636421686746985, + "y": 1.4560602409638557 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26141078838174225, + "rotationDegrees": 30.243274429662616 + }, + { + "waypointRelativePos": 1.4265560165975106, + "rotationDegrees": 110.54278821473616 + }, + { + "waypointRelativePos": 2.3427385892116224, + "rotationDegrees": -131.53019707072244 + }, + { + "waypointRelativePos": 2.765975103734439, + "rotationDegrees": -140.00418907423557 + } + ], + "constraintZones": [ + { + "name": "Bump end", + "minWaypointRelativePos": 2.6590570719602775, + "maxWaypointRelativePos": 2.855583126550863, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.8307692307692283, + "endWaypointRelativePos": 2.4327543424317684, + "command": null + }, + { + "name": "Aim", + "waypointRelativePos": 2.8496277915632646, + "endWaypointRelativePos": 3.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -178.9187647670302 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 40.23685706767951 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path b/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path new file mode 100644 index 0000000..ccab759 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path @@ -0,0 +1,163 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4361022727272723, + "y": 2.4168295454545454 + }, + "prevControl": null, + "nextControl": { + "x": 4.436102272727274, + "y": 2.4168295454545454 + }, + "isLocked": false, + "linkedName": "Right Start" + }, + { + "anchor": { + "x": 5.6364216867469885, + "y": 2.701819277108433 + }, + "prevControl": { + "x": 4.647963248912461, + "y": 2.1280691895482318 + }, + "nextControl": { + "x": 6.598060240963855, + "y": 3.2600017798466583 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.614337349397591, + "y": 4.668807228915662 + }, + "prevControl": { + "x": 5.481702751455275, + "y": 5.263961070201889 + }, + "nextControl": { + "x": 9.024012048192771, + "y": 4.275409638554216 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.062373493975903, + "y": 2.4168295454545454 + }, + "prevControl": { + "x": 8.750410708249534, + "y": 2.4738619231276058 + }, + "nextControl": { + "x": 6.095385542168674, + "y": 2.2537831325301205 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.806144578313253, + "y": 2.4168295454545454 + }, + "prevControl": { + "x": 3.055855915741729, + "y": 2.4048192182449206 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5742738589211605, + "rotationDegrees": 45.31009981067239 + }, + { + "waypointRelativePos": 1.311203319502061, + "rotationDegrees": 60.53569897702715 + }, + { + "waypointRelativePos": 1.961825726141073, + "rotationDegrees": -23.228432521120364 + }, + { + "waypointRelativePos": 2.7319502074688704, + "rotationDegrees": -140.30640603093977 + }, + { + "waypointRelativePos": 3.5087136929460336, + "rotationDegrees": 133.41473277836215 + } + ], + "constraintZones": [ + { + "name": "Bump End", + "minWaypointRelativePos": 3.513647642679896, + "maxWaypointRelativePos": 3.6406947890818926, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Intake Zone", + "minWaypointRelativePos": 1.2188585607940443, + "maxWaypointRelativePos": 3.2992555831265475, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 1.2665012406947882, + "endWaypointRelativePos": 3.386600496277901, + "command": null + }, + { + "name": "Aim", + "waypointRelativePos": 3.6803970223325035, + "endWaypointRelativePos": 4.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 40.23685706767951 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path new file mode 100644 index 0000000..8db828d --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path @@ -0,0 +1,188 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.54922891566265, + "y": 4.155204819277109 + }, + "prevControl": null, + "nextControl": { + "x": 2.4236746987951805, + "y": 4.461180722891566 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.379963855421687, + "y": 3.3246987951807228 + }, + "prevControl": { + "x": 2.1832650602409642, + "y": 3.63067469879518 + }, + "nextControl": { + "x": 3.4852291968414937, + "y": 1.6053971529721363 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.297433734939758, + "y": 2.7783132530120485 + }, + "prevControl": { + "x": 6.303012048192771, + "y": 2.461409638554217 + }, + "nextControl": { + "x": 8.583996122143095, + "y": 3.188316651131794 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.062373493975903, + "y": 4.450253012048193 + }, + "prevControl": { + "x": 8.160722891566266, + "y": 3.860156626506024 + }, + "nextControl": { + "x": 7.903547856180251, + "y": 5.403206838822087 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.029819277108434, + "y": 4.450253012048193 + }, + "prevControl": { + "x": 6.582304041411692, + "y": 3.0258782290788577 + }, + "nextControl": { + "x": 5.33044578313253, + "y": 6.2533253012048196 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.286711643768041, + "y": 5.317665405269765 + }, + "prevControl": { + "x": 4.401338149792139, + "y": 5.17560516430591 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left 2 Start" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.7800829875518442, + "rotationDegrees": 32.762556552753445 + }, + { + "waypointRelativePos": 3.6556016597510514, + "rotationDegrees": -150.84535599436526 + }, + { + "waypointRelativePos": 4.54356846473027, + "rotationDegrees": 145.60146017476993 + } + ], + "constraintZones": [ + { + "name": "Preload Shooting", + "minWaypointRelativePos": 0.45, + "maxWaypointRelativePos": 1.3, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Bump End ", + "minWaypointRelativePos": 4.183622828784128, + "maxWaypointRelativePos": 4.799007444168695, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.672456575682393, + "maxWaypointRelativePos": 4.084367245657575, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Aim", + "waypointRelativePos": 0.4516129032257933, + "endWaypointRelativePos": 1.2952853598014844, + "command": null + }, + { + "name": "Intake", + "waypointRelativePos": 1.6228287841191003, + "endWaypointRelativePos": 4.094292803970218, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -178.9187647670302 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Middle Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Middle Depot.path index d4ef384..7a730f3 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Middle Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Middle Depot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.6643132530120479, - "y": 5.892710843373494 + "x": 2.9919156626506025, + "y": 5.870855421686747 }, "prevControl": { - "x": 2.4564578313253, - "y": 5.914566265060241 + "x": 4.784060240963855, + "y": 5.892710843373494 }, "nextControl": null, "isLocked": false, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path new file mode 100644 index 0000000..3fb9ff6 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.7187228915662653, + "y": 2.4168295454545454 + }, + "prevControl": null, + "nextControl": { + "x": 2.1067710843373493, + "y": 3.182638554216868 + }, + "isLocked": false, + "linkedName": "Right 2 Start" + }, + { + "anchor": { + "x": 0.5878192771084336, + "y": 5.969204819277109 + }, + "prevControl": { + "x": 2.0302771084337348, + "y": 6.286108433734939 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot end" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.04515874612784 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -41.30950981274552 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right.path b/CompBot/src/main/deploy/pathplanner/paths/Right.path index a106981..a2b49b6 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right.path @@ -63,7 +63,7 @@ "rotationTargets": [ { "waypointRelativePos": 1.3904619970193783, - "rotationDegrees": 90.0 + "rotationDegrees": 49.27300832426523 }, { "waypointRelativePos": 1.6207468879668165, From 5deb6cf1bcd103bd67994e170d1156c7bc88f953 Mon Sep 17 00:00:00 2001 From: Nolan Barker <107814443+TheRekrab@users.noreply.github.com> Date: Fri, 10 Apr 2026 20:16:35 -0400 Subject: [PATCH 05/26] fixed a few autons --- .../main/deploy/pathplanner/paths/Depot.path | 45 ++++++++++++++----- .../main/deploy/pathplanner/paths/Left.path | 36 ++++++++------- .../subsystems/drivetrain/Drivetrain.java | 2 +- .../frc/robot/subsystems/indexer/Indexer.java | 4 +- .../frc/robot/util/BetterAutoChooser.java | 6 +-- 5 files changed, 59 insertions(+), 34 deletions(-) diff --git a/CompBot/src/main/deploy/pathplanner/paths/Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Depot.path index 861c3db..8fff921 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Depot.path @@ -8,27 +8,48 @@ }, "prevControl": null, "nextControl": { - "x": 1.8974401712548474, - "y": 5.639258950356227 + "x": 2.6094457831325304, + "y": 6.144048192771083 }, "isLocked": false, "linkedName": "Left 2 Start" }, { "anchor": { - "x": 0.5878192771084336, - "y": 5.969204819277109 + "x": 1.3309036144578312, + "y": 7.466301204819277 }, "prevControl": { - "x": 1.894745611013466, - "y": 5.719045481309168 + "x": 2.637829948362863, + "y": 7.216141866851336 + }, + "nextControl": { + "x": 0.023977280552799574, + "y": 7.7164605427872175 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.45668674698795175, + "y": 5.051277108433735 + }, + "prevControl": { + "x": 0.5441084337349398, + "y": 5.706939759036145 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.6456431535269674, + "rotationDegrees": -90.0 + } + ], "constraintZones": [ { "name": "Constraints Zone", @@ -49,18 +70,18 @@ { "name": "Aim", "waypointRelativePos": 0, - "endWaypointRelativePos": 1.0, + "endWaypointRelativePos": 2.0, "command": null }, { "name": "Intake", - "waypointRelativePos": 0.2289763289036545, - "endWaypointRelativePos": 1.0, + "waypointRelativePos": 0.9985111662531012, + "endWaypointRelativePos": 2.0, "command": null } ], "globalConstraints": { - "maxVelocity": 0.3414, + "maxVelocity": 1.0, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -69,7 +90,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 179.04515874612784 + "rotation": -90.74405920288872 }, "reversed": false, "folder": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Left.path b/CompBot/src/main/deploy/pathplanner/paths/Left.path index a65b058..e74cd06 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Left.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Left.path @@ -16,32 +16,32 @@ }, { "anchor": { - "x": 7.8875301204751285, - "y": 4.898289156624749 + "x": 7.668975903614458, + "y": 4.9638554216867465 }, "prevControl": { - "x": 7.942288744908979, - "y": 9.881323983196895 + "x": 7.853417432344418, + "y": 9.943776697395887 }, "nextControl": { - "x": 7.876602409638554, - "y": 3.9038674698795175 + "x": 7.647120481927711, + "y": 4.373759036144579 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.204662650602408, - "y": 5.1168433734939756 + "x": 6.2265180722891555, + "y": 4.799939759036144 }, "prevControl": { - "x": 7.027837406324716, - "y": 4.20220475602474 + "x": 7.374171651394384, + "y": 4.380066498387889 }, "nextControl": { - "x": 6.0079638554216865, - "y": 5.3353975903614455 + "x": 5.7784819277108435, + "y": 4.9638554216867465 }, "isLocked": false, "linkedName": null @@ -61,20 +61,24 @@ } ], "rotationTargets": [ + { + "waypointRelativePos": 0.6099585062240611, + "rotationDegrees": -91.09161788930598 + }, { "waypointRelativePos": 0.968464730290456, - "rotationDegrees": -89.74471187944988 + "rotationDegrees": -85.48797342569422 }, { - "waypointRelativePos": 1.595850622406647, + "waypointRelativePos": 1.9394190871369252, "rotationDegrees": 126.4922832788486 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.6342431761786578, - "maxWaypointRelativePos": 1.7121588089330073, + "minWaypointRelativePos": 0.6223325062034732, + "maxWaypointRelativePos": 2.3791563275434195, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 5.0, diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 1b5970b..6392ae8 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -617,7 +617,7 @@ private void configurePathplanner() { new PPHolonomicDriveController( // PPHolonomicController is the built in path following // controller for holonomic // drive trains - new PIDConstants(1.7, 0.0, 0.0), // Translation PID constants + new PIDConstants(3.0, 0.0, 0.0), // Translation PID constants new PIDConstants(3.0, 0.0, 0.0) // Rotation PID constants ), config, // The robot configuration diff --git a/CompBot/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/CompBot/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 9161345..57c66cf 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/CompBot/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -38,9 +38,9 @@ public Command index() { } public Command indexWithVariation() { - final double MIN_SPIN_VOLTAGE = 9.0; + final double MIN_SPIN_VOLTAGE = 6.0; final double MAX_SPIN_VOLTAGE = 12.0; - final double OMEGA = Math.PI; // Make this faster for more variation + final double OMEGA = 4 * Math.PI; // Make this faster for more variation return this.runEnd( () -> { io.setFeedVoltage(FeederConstants.kFeedVoltage); diff --git a/CompBot/src/main/java/frc/robot/util/BetterAutoChooser.java b/CompBot/src/main/java/frc/robot/util/BetterAutoChooser.java index 7e9d51d..1023e93 100644 --- a/CompBot/src/main/java/frc/robot/util/BetterAutoChooser.java +++ b/CompBot/src/main/java/frc/robot/util/BetterAutoChooser.java @@ -16,7 +16,7 @@ public class BetterAutoChooser { private static final boolean RIGHT_IS_FLIPPED = false; private static final String DEFAULT_NAME = "None"; - private static final String PREFIX = "FLIP "; + private static final String FLIP_PREFIX = "FLIP "; private static final double TRANSLATION_ERROR = 0.5; // meters private static final double ROTATION_ERROR = 0.1; // rotations @@ -34,9 +34,9 @@ public static SendableChooser buildAutoChooser() { } for (String name : AutoBuilder.getAllAutoNames()) { - if (name.startsWith(PREFIX)) { + if (name.startsWith(FLIP_PREFIX)) { // Make the flipped paths - String remainder = name.substring(PREFIX.length()); + String remainder = name.substring(FLIP_PREFIX.length()); registerAuto("Right " + remainder + " (Generated)", new PathPlannerAuto(name, RIGHT_IS_FLIPPED)); registerAuto("Left " + remainder + " (Generated)", new PathPlannerAuto(name, !RIGHT_IS_FLIPPED)); } else { From 29e991f144bb8e3951ee1deb85c23fda5c3ed01c Mon Sep 17 00:00:00 2001 From: Nolan Barker <107814443+TheRekrab@users.noreply.github.com> Date: Fri, 10 Apr 2026 20:57:43 -0400 Subject: [PATCH 06/26] Changed Depot Path and made it little better --- .../main/deploy/pathplanner/paths/Alt.path | 6 ++-- .../main/deploy/pathplanner/paths/Depot.path | 34 +++++++++++++++---- .../deploy/pathplanner/paths/Left Alt.path | 6 ++-- .../main/deploy/pathplanner/paths/Left.path | 16 ++++----- .../localization/LocalizationConstants.java | 2 +- 5 files changed, 42 insertions(+), 22 deletions(-) diff --git a/CompBot/src/main/deploy/pathplanner/paths/Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Alt.path index 692e33c..b28567a 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Alt.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.4290240963855423, + "x": 3.5929397590361445, "y": 5.317665405269765 }, "prevControl": null, "nextControl": { - "x": 2.693437945791726, + "x": 2.8573536084423283, "y": 4.85660485021398 }, "isLocked": false, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -62.58308037431244 + "rotation": 3.9102257507213043 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Depot.path index 8fff921..5a66d9f 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Depot.path @@ -32,12 +32,28 @@ }, { "anchor": { - "x": 0.45668674698795175, - "y": 5.051277108433735 + "x": 0.5003975903614455, + "y": 5.317665405269765 }, "prevControl": { - "x": 0.5441084337349398, - "y": 5.706939759036145 + "x": 0.5331807228915659, + "y": 6.078481927710843 + }, + "nextControl": { + "x": 0.48963522528803627, + "y": 5.067897169703216 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9100722891566262, + "y": 5.317665405269765 + }, + "prevControl": { + "x": 1.2653373493975903, + "y": 5.258903614457831 }, "nextControl": null, "isLocked": false, @@ -47,6 +63,10 @@ "rotationTargets": [ { "waypointRelativePos": 0.6456431535269674, + "rotationDegrees": -131.08312006631627 + }, + { + "waypointRelativePos": 1.7302904564315178, "rotationDegrees": -90.0 } ], @@ -54,7 +74,7 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0.6095783774685815, - "maxWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.3607940446650069, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 5.0, @@ -70,7 +90,7 @@ { "name": "Aim", "waypointRelativePos": 0, - "endWaypointRelativePos": 2.0, + "endWaypointRelativePos": 3.0, "command": null }, { @@ -90,7 +110,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -90.74405920288872 + "rotation": -1.8476102659945397 }, "reversed": false, "folder": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path index a9c5a1f..5651baa 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.4290240963855423, + "x": 3.5929397590361445, "y": 5.317665405269765 }, "prevControl": { - "x": 2.4217261055634807, + "x": 2.585641768214083, "y": 5.10243937232525 }, "nextControl": null, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -62.58308037431244 + "rotation": 3.9102257507213043 }, "reversed": false, "folder": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Left.path b/CompBot/src/main/deploy/pathplanner/paths/Left.path index e74cd06..3a8cc6a 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Left.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Left.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.4290240963855423, + "x": 3.5929397590361445, "y": 5.317665405269765 }, "prevControl": null, "nextControl": { - "x": 6.696409638554217, + "x": 6.860325301204819, "y": 5.073132530120482 }, "isLocked": false, @@ -36,12 +36,12 @@ "y": 4.799939759036144 }, "prevControl": { - "x": 7.374171651394384, - "y": 4.380066498387889 + "x": 6.989925655819335, + "y": 3.8456802796234166 }, "nextControl": { - "x": 5.7784819277108435, - "y": 4.9638554216867465 + "x": 5.745698795180723, + "y": 5.400963855421687 }, "isLocked": false, "linkedName": null @@ -70,7 +70,7 @@ "rotationDegrees": -85.48797342569422 }, { - "waypointRelativePos": 1.9394190871369252, + "waypointRelativePos": 2.686307053941867, "rotationDegrees": 126.4922832788486 } ], @@ -114,7 +114,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -62.58308037431244 + "rotation": 3.9102257507213043 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/java/frc/robot/vision/localization/LocalizationConstants.java b/CompBot/src/main/java/frc/robot/vision/localization/LocalizationConstants.java index ec61914..b2d6ce3 100644 --- a/CompBot/src/main/java/frc/robot/vision/localization/LocalizationConstants.java +++ b/CompBot/src/main/java/frc/robot/vision/localization/LocalizationConstants.java @@ -24,7 +24,7 @@ public class LocalizationConstants { Degrees.of(77.4), // Horizontal FOV () -> Transform3d.kZero, // Pose supplier new CameraTrustConfig( - VecBuilder.fill(0.3, 0.3, 0.3), // Base std devs + VecBuilder.fill(0.1, 0.1, 0.1), // Base std devs 0.75, // Latency threshold 1.3, // Latency multiplier 2.5, // Field XY margin From ff18dcff12bcb023c1d260639e2ce4e185e0a977 Mon Sep 17 00:00:00 2001 From: 3414 Linux Date: Sat, 11 Apr 2026 14:40:12 -0400 Subject: [PATCH 07/26] the autons --- CompBot/simgui-ds.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CompBot/simgui-ds.json b/CompBot/simgui-ds.json index a230ebf..3140247 100644 --- a/CompBot/simgui-ds.json +++ b/CompBot/simgui-ds.json @@ -6,7 +6,7 @@ }, "System Joysticks": { "window": { - "enabled": false + "visible": false } }, "keyboardJoysticks": [ From 1cc2e7c6cb76b4c0f6039b81213a5feebef05c50 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Sat, 11 Apr 2026 15:59:08 -0400 Subject: [PATCH 08/26] Add slew rate limiter and improve things --- CompBot/simgui-ds.json | 8 ++- .../frc/robot/binding/DriverXboxBindings.java | 8 ++- .../frc/robot/binding/KeyboardBindings.java | 49 +++++++++-------- .../robot/binding/OperatorPS5Bindings.java | 2 - .../subsystems/drivetrain/Drivetrain.java | 53 +++++++++++++------ .../robot/superstructure/Superstructure.java | 24 ++++++--- .../robot/util/ChassisSpeedRateLimiter.java | 48 +++++++++++++++++ .../java/frc/robot/util/TriggerSequence.java | 1 - 8 files changed, 130 insertions(+), 63 deletions(-) create mode 100644 CompBot/src/main/java/frc/robot/util/ChassisSpeedRateLimiter.java diff --git a/CompBot/simgui-ds.json b/CompBot/simgui-ds.json index 3140247..2ef491b 100644 --- a/CompBot/simgui-ds.json +++ b/CompBot/simgui-ds.json @@ -1,9 +1,4 @@ { - "Joysticks": { - "window": { - "visible": false - } - }, "System Joysticks": { "window": { "visible": false @@ -14,14 +9,17 @@ "axisConfig": [ { "decKey": 83, + "decayRate": 1.0, "incKey": 87 }, { "decKey": 68, + "decayRate": 1.0, "incKey": 65 }, { "decKey": 93, + "decayRate": 1.0, "incKey": 91 }, { diff --git a/CompBot/src/main/java/frc/robot/binding/DriverXboxBindings.java b/CompBot/src/main/java/frc/robot/binding/DriverXboxBindings.java index 6a40945..1b4e38c 100644 --- a/CompBot/src/main/java/frc/robot/binding/DriverXboxBindings.java +++ b/CompBot/src/main/java/frc/robot/binding/DriverXboxBindings.java @@ -1,7 +1,6 @@ package frc.robot.binding; import java.util.function.DoubleSupplier; - import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -10,7 +9,6 @@ import frc.robot.commands.ResetForwards; import frc.robot.commands.RetractIntake; import frc.robot.commands.RunIntake; -import frc.robot.subsystems.drivetrain.Drivetrain.TeleopDriveMode; import frc.robot.superstructure.Superstructure; import frc.robot.util.RumbleUtil; import frc.robot.util.RumbleUtil.RumbleStrength; @@ -20,7 +18,7 @@ public class DriverXboxBindings implements Binder { private final DoubleSupplier vx, vy, vrot; - private final Trigger shoot, intake, resetPerspective, robotRelativeDrive, retract; + private final Trigger shoot, intake, resetPerspective, slowbot, retract; public DriverXboxBindings() { controller = new CommandXboxController(Driver.kDriveControllerPort); @@ -32,12 +30,12 @@ public DriverXboxBindings() { shoot = controller.rightBumper(); intake = controller.rightTrigger(); resetPerspective = controller.leftBumper(); - robotRelativeDrive = controller.leftTrigger(); + slowbot = controller.leftTrigger(); retract = controller.x(); } public void bind(Superstructure superstructure) { - superstructure.bindDrive(vx, vy, vrot, () -> robotRelativeDrive.getAsBoolean() ? TeleopDriveMode.RobotRelative : TeleopDriveMode.FieldRelativeSpin); + superstructure.bindDrive(vx, vy, vrot, slowbot); shoot.toggleOnTrue(superstructure.build(new AimPrep())); intake.toggleOnTrue(superstructure.build(new RunIntake())); diff --git a/CompBot/src/main/java/frc/robot/binding/KeyboardBindings.java b/CompBot/src/main/java/frc/robot/binding/KeyboardBindings.java index 10c9da2..668aeb0 100644 --- a/CompBot/src/main/java/frc/robot/binding/KeyboardBindings.java +++ b/CompBot/src/main/java/frc/robot/binding/KeyboardBindings.java @@ -9,40 +9,39 @@ import frc.robot.commands.RetractIntake; import frc.robot.commands.RunIndex; import frc.robot.commands.RunIntake; -import frc.robot.subsystems.drivetrain.Drivetrain.TeleopDriveMode; import frc.robot.superstructure.Superstructure; public class KeyboardBindings implements Binder { - private final CommandGenericHID controller; + private final CommandGenericHID controller; - private final Trigger shoot, intake, robotCentricDrive, resetForwards, index, retractIntake; + private final Trigger shoot, intake, slowbot, resetForwards, index, retractIntake; - private final DoubleSupplier vx, vy, vrot; + private final DoubleSupplier vx, vy, vrot; - public KeyboardBindings() { - controller = new CommandGenericHID(0); + public KeyboardBindings() { + controller = new CommandGenericHID(0); - shoot = controller.button(1); - intake = controller.button(2); - robotCentricDrive = controller.button(3); - resetForwards = controller.button(4); - index = controller.button(5); - retractIntake = controller.button(6); + shoot = controller.button(1); + intake = controller.button(2); + slowbot = controller.button(3); + resetForwards = controller.button(4); + index = controller.button(5); + retractIntake = controller.button(6); - vx = () -> controller.getRawAxis(0); - vy = () -> controller.getRawAxis(1); - vrot = () -> controller.getRawAxis(2); - } + vx = () -> controller.getRawAxis(0); + vy = () -> controller.getRawAxis(1); + vrot = () -> controller.getRawAxis(2); + } - public void bind(Superstructure superstructure) { - superstructure.bindDrive(vx, vy, vrot, () -> robotCentricDrive.getAsBoolean() ? TeleopDriveMode.RobotRelative : TeleopDriveMode.FieldRelativeSpin); + public void bind(Superstructure superstructure) { + superstructure.bindDrive(vx, vy, vrot, slowbot); - shoot.toggleOnTrue(superstructure.build(new AimPrep())); - intake.whileTrue(superstructure.build(new RunIntake())); - resetForwards.onTrue(superstructure.build(new ResetForwards())); - index.whileTrue(superstructure.build(new RunIndex())); - retractIntake.onTrue(superstructure.build(new RetractIntake())); + shoot.toggleOnTrue(superstructure.build(new AimPrep())); + intake.whileTrue(superstructure.build(new RunIntake())); + resetForwards.onTrue(superstructure.build(new ResetForwards())); + index.whileTrue(superstructure.build(new RunIndex())); + retractIntake.onTrue(superstructure.build(new RetractIntake())); + + } - } - } diff --git a/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java b/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java index 5f16082..eef901b 100644 --- a/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java +++ b/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java @@ -1,7 +1,5 @@ package frc.robot.binding; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.binding.BindingConstants.Operator; diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 6392ae8..0ee6af0 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -60,6 +60,7 @@ import frc.robot.subsystems.turret.TurretConstants; import frc.robot.util.FieldUtils; import frc.robot.util.OnboardLogger; +import frc.robot.util.ChassisSpeedRateLimiter; import frc.robot.util.StatusSignalUtil; import frc.robot.vision.localization.LocalizationConstants; import frc.robot.vision.localization.TimestampedPoseEstimate; @@ -87,11 +88,13 @@ public enum TeleopDriveMode { * Drive the robot with a field-relative control for translation and spin control (i.e. control * over how fast we rotate) */ - FieldRelativeSpin, + FieldRelative, /** Drive the robot slower than FieldRelativeSpin */ - SlowFieldRelativeSpin, + SlowFieldRelative, /** Drive robot relative. */ RobotRelative, + /** Drive with limited acceleration */ + AccelerationLimitedFieldRelative } private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() @@ -117,6 +120,8 @@ public enum TeleopDriveMode { .withHeadingPID(HeadingGains.kP, HeadingGains.kI, HeadingGains.kD) .withCenterOfRotation(TurretConstants.kOffset.getTranslation().toTranslation2d()); + private ChassisSpeedRateLimiter rateLimiter = new ChassisSpeedRateLimiter(1.0, 3.0); + private SwerveDriveState state; private double lastOkayVisionUpdateTime; @@ -408,40 +413,49 @@ public Command teleopDrive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier return this.applyRequest(() -> { TeleopDriveMode mode = modeSupplier.get(); // Recalculate the *real* vx and vy to be operator-dependent - Translation2d operatorRelative = - new Translation2d(vx.getAsDouble() * maxSpeed, vy.getAsDouble() * maxSpeed); + ChassisSpeeds operatorRelative = + new ChassisSpeeds( + vx.getAsDouble() * maxSpeed, + vy.getAsDouble() * maxSpeed, + vrot.getAsDouble() * maxRotationalSpeed); - Translation2d fieldRelative = operatorRelative.rotateBy(getOperatorForwardDirection()); - double spin = vrot.getAsDouble() * maxRotationalSpeed; + ChassisSpeeds fieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(operatorRelative, getOperatorForwardDirection()); if (mode == TeleopDriveMode.RobotRelative) { return robotCentricDrive - .withVelocityX(operatorRelative.getX()) - .withVelocityY(operatorRelative.getY()) - .withRotationalRate(spin); + .withVelocityX(operatorRelative.vxMetersPerSecond) + .withVelocityY(operatorRelative.vyMetersPerSecond) + .withRotationalRate(operatorRelative.omegaRadiansPerSecond); } - if (mode == TeleopDriveMode.SlowFieldRelativeSpin) { + if (mode == TeleopDriveMode.SlowFieldRelative + || mode == TeleopDriveMode.AccelerationLimitedFieldRelative) { fieldRelative = fieldRelative.times(0.3); - spin *= 0.5; + } + + if (mode != TeleopDriveMode.AccelerationLimitedFieldRelative) { + rateLimiter.reset(fieldRelative); + } else { + fieldRelative = rateLimiter.calculate(fieldRelative); } if (override.isPresent()) { - return drivetrainAim.withVelocityX(fieldRelative.getX()) - .withVelocityY(fieldRelative.getY()) + return drivetrainAim + .withVelocityX(fieldRelative.vxMetersPerSecond) + .withVelocityY(fieldRelative.vyMetersPerSecond) .withTargetDirection(override.get().yaw); } return drive - .withVelocityX(fieldRelative.getX()) - .withVelocityY(fieldRelative.getY()) - .withRotationalRate(spin); + .withVelocityX(fieldRelative.vxMetersPerSecond) + .withVelocityY(fieldRelative.vyMetersPerSecond) + .withRotationalRate(fieldRelative.omegaRadiansPerSecond); }) .withName("Teleop Drive"); } public Command teleopDrive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier vrot) { - return teleopDrive(vx, vy, vrot, () -> TeleopDriveMode.FieldRelativeSpin); + return teleopDrive(vx, vy, vrot, () -> TeleopDriveMode.FieldRelative); } /** @@ -636,4 +650,9 @@ private void configurePathplanner() { this // Reference to this subsystem to set requirements ); } + + @Override + public void simulationPeriodic() { + SmartDashboard.putNumber("Drivetrain/Speed", robotVelocity().getTranslation().getNorm()); + } } diff --git a/CompBot/src/main/java/frc/robot/superstructure/Superstructure.java b/CompBot/src/main/java/frc/robot/superstructure/Superstructure.java index 8ebbc2a..282367b 100644 --- a/CompBot/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/CompBot/src/main/java/frc/robot/superstructure/Superstructure.java @@ -1,9 +1,8 @@ package frc.robot.superstructure; import java.util.List; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.util.Units; @@ -54,7 +53,7 @@ public record Subsystems( public final StateManager state; public Superstructure() { - subsystems = switch(RobotIdentifier.id()) { + subsystems = switch (RobotIdentifier.id()) { case CompBot -> createCompBotSubsystems(); case SimBot -> createSimBotSubsystems(); case TestBot -> createTestBotSubsystems(); @@ -99,8 +98,16 @@ private Subsystems createTestBotSubsystems() { return new Subsystems(drivetrain, turret, shooter, indexer, intake, climber, led); } - public void bindDrive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier vrot, Supplier mode) { - subsystems.drivetrain.setDefaultCommand(subsystems.drivetrain.teleopDrive(vx, vy, vrot, () -> state.shooting(ShootMode.Scoring).getAsBoolean() ? TeleopDriveMode.SlowFieldRelativeSpin : mode.get())); + public void bindDrive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier vrot, BooleanSupplier slowbot) { + subsystems.drivetrain.setDefaultCommand(subsystems.drivetrain.teleopDrive(vx, vy, vrot, () -> { + if (slowbot.getAsBoolean()) { + return TeleopDriveMode.AccelerationLimitedFieldRelative; + } + if (state.shooting(ShootMode.Scoring).getAsBoolean()) { + return TeleopDriveMode.SlowFieldRelative; + } + return TeleopDriveMode.FieldRelative; + })); } /** @@ -127,7 +134,8 @@ public AprilTagVisionHandler createAprilTagVisionHandler() { LocalizationConstants.kRegularBaseCameraConfig.cameraCopy( "cam1", () -> new Transform3d(-0.207, -0.318, 0.473, - new Rotation3d(Units.degreesToRadians(0.7), Units.degreesToRadians(-28.578), Units.degreesToRadians(-67.63)))), + new Rotation3d(Units.degreesToRadians(0.7), Units.degreesToRadians(-28.578), + Units.degreesToRadians(-67.63)))), LocalizationConstants.kRegularBaseCameraConfig.cameraCopy( "cam2", () -> new Transform3d(0.221, -0.262, 0.724, @@ -139,8 +147,8 @@ public AprilTagVisionHandler createAprilTagVisionHandler() { LocalizationConstants.kRegularBaseCameraConfig.cameraCopy( "cam4", () -> new Transform3d(-0.315, 0.138, 0.438, - new Rotation3d(Units.degreesToRadians(-6.5), Units.degreesToRadians(-29.9), Units.degreesToRadians(-169)))) - ); + new Rotation3d(Units.degreesToRadians(-6.5), Units.degreesToRadians(-29.9), + Units.degreesToRadians(-169))))); return new AprilTagVisionHandler(this, configs); } diff --git a/CompBot/src/main/java/frc/robot/util/ChassisSpeedRateLimiter.java b/CompBot/src/main/java/frc/robot/util/ChassisSpeedRateLimiter.java new file mode 100644 index 0000000..fd93075 --- /dev/null +++ b/CompBot/src/main/java/frc/robot/util/ChassisSpeedRateLimiter.java @@ -0,0 +1,48 @@ +package frc.robot.util; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; + +public class ChassisSpeedRateLimiter { + private ChassisSpeeds last; + private double lastTime; + + private final double accelerationLimit; + private final double angularAccelerationLimit; + + public ChassisSpeedRateLimiter(double accelerationLimit, double angularAccelerationLimit) { + this.accelerationLimit = accelerationLimit; + this.angularAccelerationLimit = angularAccelerationLimit; + } + + public ChassisSpeeds calculate(ChassisSpeeds input) { + // Time differences + double currentTime = Timer.getTimestamp(); + double dt = currentTime - lastTime; + // Cap 2d acceleration + double dx = input.vxMetersPerSecond - last.vxMetersPerSecond; + double dy = input.vyMetersPerSecond - last.vyMetersPerSecond; + double domega = MathUtil.angleModulus(input.omegaRadiansPerSecond - last.omegaRadiansPerSecond); + double hypot = Math.hypot(dx, dy); + if (hypot > accelerationLimit * dt) { + // divide by hypotenuse to normalize, then multiply by new length + dx *= accelerationLimit * dt / hypot; + dy *= accelerationLimit * dt / hypot; + } + domega = MathUtil.clamp( + domega, + -angularAccelerationLimit * dt, + angularAccelerationLimit * dt); + last = new ChassisSpeeds( + last.vxMetersPerSecond + dx, + last.vyMetersPerSecond + dy, + last.omegaRadiansPerSecond + domega); + lastTime = currentTime; + return last; + } + + public void reset(ChassisSpeeds seed) { + last = seed; + } +} diff --git a/CompBot/src/main/java/frc/robot/util/TriggerSequence.java b/CompBot/src/main/java/frc/robot/util/TriggerSequence.java index f9b4183..8732c37 100644 --- a/CompBot/src/main/java/frc/robot/util/TriggerSequence.java +++ b/CompBot/src/main/java/frc/robot/util/TriggerSequence.java @@ -1,7 +1,6 @@ package frc.robot.util; import java.util.ArrayList; -import java.util.Arrays; import java.util.HashMap; import java.util.List; import java.util.Map; From 6d3ea23e8b1602d245367428421a58611c20f41a Mon Sep 17 00:00:00 2001 From: Nolan Barker <107814443+TheRekrab@users.noreply.github.com> Date: Sat, 11 Apr 2026 15:59:47 -0400 Subject: [PATCH 09/26] auto works sometimes --- .../autos/{FLIP Crash.auto => FLIP Far.auto} | 0 .../src/main/deploy/pathplanner/paths/Right Depot.path | 10 +++++----- .../frc/robot/subsystems/drivetrain/Drivetrain.java | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) rename CompBot/src/main/deploy/pathplanner/autos/{FLIP Crash.auto => FLIP Far.auto} (100%) diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Crash.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Far.auto similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/FLIP Crash.auto rename to CompBot/src/main/deploy/pathplanner/autos/FLIP Far.auto diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path index 3fb9ff6..d7a4008 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.5878192771084336, - "y": 5.969204819277109 + "x": 1.9100722891566262, + "y": 5.317665405269765 }, "prevControl": { - "x": 2.0302771084337348, - "y": 6.286108433734939 + "x": 3.3525301204819273, + "y": 5.6345690197275955 }, "nextControl": null, "isLocked": false, @@ -56,7 +56,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 179.04515874612784 + "rotation": -1.8476102659945397 }, "reversed": false, "folder": null, diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 6392ae8..4b5603c 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -274,8 +274,8 @@ public Drivetrain( steer.getPosition(false)::getValue, Rotations); } sysIDCommands(); - // SmartDashboard.putData("Drivetrain/Set Home", setMemorySpot()); - // SmartDashboard.putData("Drivetrain/Go Home", goHome()); + SmartDashboard.putData("Drivetrain/Set Home", setMemorySpot()); + SmartDashboard.putData("Drivetrain/Go Home", goHome()); SmartDashboard.putData("Drivetrain/Shake", shake()); configurePathplanner(); } From 99a1327eea82bc43ceb13d07577af5c76015d24e Mon Sep 17 00:00:00 2001 From: Nolan Barker <107814443+TheRekrab@users.noreply.github.com> Date: Sat, 11 Apr 2026 17:04:36 -0400 Subject: [PATCH 10/26] stuffs --- .../{FLIP Double.auto => FLIP Single.auto} | 31 ++++++------- ... of FLIP Double.auto => Flip Inverse.auto} | 24 +++++----- .../main/deploy/pathplanner/paths/Alt.path | 6 +-- .../main/deploy/pathplanner/paths/Depot.path | 2 +- .../deploy/pathplanner/paths/FLIP Crash.path | 8 ++-- .../pathplanner/paths/Inverse Right.path | 12 ++--- .../main/deploy/pathplanner/paths/Left.path | 2 +- .../deploy/pathplanner/paths/Right Depot.path | 12 ++--- .../main/deploy/pathplanner/paths/Right.path | 45 +++++++------------ .../robot/binding/NamedCommandBindings.java | 2 + .../subsystems/drivetrain/Drivetrain.java | 2 +- .../subsystems/intake/IntakeConstants.java | 2 +- 12 files changed, 69 insertions(+), 79 deletions(-) rename CompBot/src/main/deploy/pathplanner/autos/{FLIP Double.auto => FLIP Single.auto} (55%) rename CompBot/src/main/deploy/pathplanner/autos/{Copy of FLIP Double.auto => Flip Inverse.auto} (89%) diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Double.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Single.auto similarity index 55% rename from CompBot/src/main/deploy/pathplanner/autos/FLIP Double.auto rename to CompBot/src/main/deploy/pathplanner/autos/FLIP Single.auto index 8714de4..9c8b55c 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/FLIP Double.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Single.auto @@ -11,7 +11,7 @@ } }, { - "type": "race", + "type": "parallel", "data": { "commands": [ { @@ -21,25 +21,26 @@ } }, { - "type": "wait", + "type": "sequential", "data": { - "waitTime": 10.0 + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 7.0 + } + }, + { + "type": "named", + "data": { + "name": "Retract" + } + } + ] } } ] } - }, - { - "type": "path", - "data": { - "pathName": "Right Cleanup" - } - }, - { - "type": "named", - "data": { - "name": "Aim" - } } ] } diff --git a/CompBot/src/main/deploy/pathplanner/autos/Copy of FLIP Double.auto b/CompBot/src/main/deploy/pathplanner/autos/Flip Inverse.auto similarity index 89% rename from CompBot/src/main/deploy/pathplanner/autos/Copy of FLIP Double.auto rename to CompBot/src/main/deploy/pathplanner/autos/Flip Inverse.auto index 348447a..fec7ea7 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/Copy of FLIP Double.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/Flip Inverse.auto @@ -5,30 +5,30 @@ "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "Inverse Right" - } - }, - { - "type": "race", + "type": "deadline", "data": { "commands": [ { - "type": "named", + "type": "wait", "data": { - "name": "Aim" + "waitTime": 3.0 } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 10.0 + "name": "Aim" } } ] } }, + { + "type": "path", + "data": { + "pathName": "Inverse Right" + } + }, { "type": "named", "data": { @@ -38,7 +38,7 @@ ] } }, - "resetOdom": false, + "resetOdom": true, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Alt.path index dc8370e..7ed88ab 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Alt.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.4361022727272723, + "x": 3.5929397590361445, "y": 2.4168295454545454 }, "prevControl": { - "x": 2.7969472182596284, + "x": 2.9537847045685006, "y": 3.1616405135520687 }, "nextControl": null, @@ -49,7 +49,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 40.23685706767951 + "rotation": 0.0 }, "reversed": false, "folder": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Depot.path index 4c864e9..cc3f10e 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Depot.path @@ -89,7 +89,7 @@ "eventMarkers": [ { "name": "Aim", - "waypointRelativePos": 0, + "waypointRelativePos": 0.14590570719603252, "endWaypointRelativePos": 3.0, "command": null }, diff --git a/CompBot/src/main/deploy/pathplanner/paths/FLIP Crash.path b/CompBot/src/main/deploy/pathplanner/paths/FLIP Crash.path index d04fcdb..28d1c23 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/FLIP Crash.path +++ b/CompBot/src/main/deploy/pathplanner/paths/FLIP Crash.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.4361022727272723, + "x": 3.5929397590361445, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 4.521795180722892, + "x": 4.6786326670317635, "y": 3.226349397590361 }, "isLocked": false, @@ -53,7 +53,7 @@ }, "prevControl": { "x": 5.636421686746985, - "y": 1.4560602409638557 + "y": 1.4560602409638554 }, "nextControl": null, "isLocked": false, @@ -124,7 +124,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 40.23685706767951 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path b/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path index ccab759..fc09eb2 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.4361022727272723, + "x": 3.5929397590361445, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 4.436102272727274, + "x": 4.5929397590361445, "y": 2.4168295454545454 }, "isLocked": false, @@ -79,7 +79,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5742738589211605, - "rotationDegrees": 45.31009981067239 + "rotationDegrees": 45.3100998106724 }, { "waypointRelativePos": 1.311203319502061, @@ -95,7 +95,7 @@ }, { "waypointRelativePos": 3.5087136929460336, - "rotationDegrees": 133.41473277836215 + "rotationDegrees": 133.41473277836218 } ], "constraintZones": [ @@ -130,7 +130,7 @@ "eventMarkers": [ { "name": "Intake", - "waypointRelativePos": 1.2665012406947882, + "waypointRelativePos": 1.0759305210918124, "endWaypointRelativePos": 3.386600496277901, "command": null }, @@ -157,7 +157,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 40.23685706767951 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Left.path b/CompBot/src/main/deploy/pathplanner/paths/Left.path index 3a8cc6a..3e15706 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Left.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Left.path @@ -93,7 +93,7 @@ "eventMarkers": [ { "name": "Intake", - "waypointRelativePos": 0.6342431761786518, + "waypointRelativePos": 0.39602977667494543, "endWaypointRelativePos": 2.128218438538206, "command": null } diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path index d7a4008..97e18e3 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 1.9100722891566262, - "y": 5.317665405269765 + "x": 2.9919156626506025, + "y": 5.870855421686747 }, "prevControl": { - "x": 3.3525301204819273, - "y": 5.6345690197275955 + "x": 4.434373493975904, + "y": 6.187759036144577 }, "nextControl": null, "isLocked": false, - "linkedName": "Depot end" + "linkedName": null } ], "rotationTargets": [], @@ -56,7 +56,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -1.8476102659945397 + "rotation": 178.89829388479365 }, "reversed": false, "folder": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right.path b/CompBot/src/main/deploy/pathplanner/paths/Right.path index a2b49b6..5e89a77 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right.path @@ -3,45 +3,45 @@ "waypoints": [ { "anchor": { - "x": 3.4361022727272723, + "x": 3.5929397590361445, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 6.128168674698795, - "y": 2.822024096385543 + "x": 6.171879518072289, + "y": 3.2919156626506028 }, "isLocked": false, "linkedName": "Right Start" }, { "anchor": { - "x": 6.728808593750001, - "y": 1.2268230083626759 + "x": 7.046096385542168, + "y": 0.7457590361445796 }, "prevControl": { - "x": 6.065178019973029, - "y": 2.5536917163810404 + "x": 5.636421686746989, + "y": 1.5981204819277113 }, "nextControl": { - "x": 7.614337349397591, - "y": -0.5437108433734943 + "x": 7.86954041464832, + "y": 0.24786264645248768 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.919806818181818, - "y": 3.9525454545454544 + "x": 6.936819277108433, + "y": 3.3684096385542173 }, "prevControl": { - "x": 8.68011542976235, - "y": 5.29958307018095 + "x": 8.80882222557424, + "y": 4.555339167581473 }, "nextControl": { - "x": 4.499939759036145, - "y": 2.100795180722891 + "x": 4.368807228915662, + "y": 1.740180722891567 }, "isLocked": false, "linkedName": null @@ -92,19 +92,6 @@ "unlimited": false } }, - { - "name": "BUMP", - "minWaypointRelativePos": 0.06253101736972864, - "maxWaypointRelativePos": 0.6342431761786589, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, { "name": "MORE BUMPING", "minWaypointRelativePos": 2.158808933002488, @@ -144,7 +131,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 40.23685706767951 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java b/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java index 2a00f78..7fc131c 100644 --- a/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -4,6 +4,7 @@ import com.pathplanner.lib.events.EventTrigger; import frc.robot.commands.AimPrep; +import frc.robot.commands.RetractIntake; import frc.robot.commands.RunIntake; import frc.robot.superstructure.Superstructure; @@ -11,6 +12,7 @@ public class NamedCommandBindings implements Binder { public void bind(Superstructure superstructure) { NamedCommands.registerCommand("Aim", superstructure.build(new AimPrep())); NamedCommands.registerCommand("Intake", superstructure.build(new RunIntake())); + NamedCommands.registerCommand("Retract", superstructure.build(new RetractIntake())) ; new EventTrigger("Aim").whileTrue(superstructure.build(new AimPrep())); new EventTrigger("Intake").whileTrue(superstructure.build(new RunIntake())); diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index a0115b9..65c2ff5 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -120,7 +120,7 @@ public enum TeleopDriveMode { .withHeadingPID(HeadingGains.kP, HeadingGains.kI, HeadingGains.kD) .withCenterOfRotation(TurretConstants.kOffset.getTranslation().toTranslation2d()); - private ChassisSpeedRateLimiter rateLimiter = new ChassisSpeedRateLimiter(1.0, 3.0); + private ChassisSpeedRateLimiter rateLimiter = new ChassisSpeedRateLimiter(2.0, 3.0); private SwerveDriveState state; diff --git a/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 29afc1e..66bc760 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -39,7 +39,7 @@ public class IntakeConstants { .withCurrentLimits(new CurrentLimitsConfigs() .withSupplyCurrentLimitEnable(true) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(40) + .withSupplyCurrentLimit(50) .withStatorCurrentLimit(120)); protected static final Current kJamStatorThreshold = Amps.of(70); From 83da0399247ce912d8ce165b2744840fbb3d59f1 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Sat, 11 Apr 2026 17:04:54 -0400 Subject: [PATCH 11/26] Log memory spot --- .../main/java/frc/robot/subsystems/drivetrain/Drivetrain.java | 1 + 1 file changed, 1 insertion(+) diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index a0115b9..a5275c4 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -347,6 +347,7 @@ public void periodic() { state = getState(); FieldManager.getInstance().getField().setRobotPose(robotPose()); + FieldManager.getInstance().getField().getObject("Memory Spot").setPose(memorySpot); } private void startSimThread() { From f5c6eb974ec3f92ee97c12c73a151d16eb16bde4 Mon Sep 17 00:00:00 2001 From: Nolan Barker <107814443+TheRekrab@users.noreply.github.com> Date: Sat, 11 Apr 2026 20:00:44 -0400 Subject: [PATCH 12/26] Inverse Auton locked and everything locked --- .../deploy/pathplanner/autos/FLIP Single.auto | 30 +------------- .../pathplanner/autos/Flip Inverse.auto | 2 +- .../pathplanner/paths/Inverse Right.path | 41 ++++++++++++------- .../main/deploy/pathplanner/paths/Right.path | 24 +++++------ .../robot/binding/NamedCommandBindings.java | 2 +- .../subsystems/drivetrain/Drivetrain.java | 1 + .../frc/robot/subsystems/intake/Intake.java | 1 + .../subsystems/intake/IntakeConstants.java | 6 ++- .../subsystems/intake/IntakeIOHardware.java | 8 ++++ 9 files changed, 58 insertions(+), 57 deletions(-) diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Single.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Single.auto index 9c8b55c..feb216f 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/FLIP Single.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Single.auto @@ -11,35 +11,9 @@ } }, { - "type": "parallel", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Aim" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 7.0 - } - }, - { - "type": "named", - "data": { - "name": "Retract" - } - } - ] - } - } - ] + "name": "Aim" } } ] diff --git a/CompBot/src/main/deploy/pathplanner/autos/Flip Inverse.auto b/CompBot/src/main/deploy/pathplanner/autos/Flip Inverse.auto index fec7ea7..aa17f33 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/Flip Inverse.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/Flip Inverse.auto @@ -11,7 +11,7 @@ { "type": "wait", "data": { - "waitTime": 3.0 + "waitTime": 2.0 } }, { diff --git a/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path b/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path index fc09eb2..77f67a3 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 5.6364216867469885, - "y": 2.701819277108433 + "x": 5.712915662650602, + "y": 2.4168295454545454 }, "prevControl": { - "x": 4.647963248912461, - "y": 2.1280691895482318 + "x": 4.685710843373494, + "y": 2.461409638554217 }, "nextControl": { - "x": 6.598060240963855, - "y": 3.2600017798466583 + "x": 6.823767668618597, + "y": 2.368619212990509 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 4.668807228915662 }, "prevControl": { - "x": 5.481702751455275, - "y": 5.263961070201889 + "x": 5.428795180722891, + "y": 5.3463253012048195 }, "nextControl": { - "x": 9.024012048192771, - "y": 4.275409638554216 + "x": 9.012246699976645, + "y": 4.235455330236155 }, "isLocked": false, "linkedName": null @@ -78,12 +78,12 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.5742738589211605, - "rotationDegrees": 45.3100998106724 + "waypointRelativePos": 0.972614107883791, + "rotationDegrees": 0.0 }, { - "waypointRelativePos": 1.311203319502061, - "rotationDegrees": 60.53569897702715 + "waypointRelativePos": 1.4174273858921158, + "rotationDegrees": 79.22193857481086 }, { "waypointRelativePos": 1.961825726141073, @@ -124,6 +124,19 @@ "nominalVoltage": 12.0, "unlimited": false } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } } ], "pointTowardsZones": [], diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right.path b/CompBot/src/main/deploy/pathplanner/paths/Right.path index 5e89a77..2266640 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right.path @@ -20,28 +20,28 @@ "y": 0.7457590361445796 }, "prevControl": { - "x": 5.636421686746989, - "y": 1.5981204819277113 + "x": 6.180447232697349, + "y": 0.9501484194551628 }, "nextControl": { - "x": 7.86954041464832, - "y": 0.24786264645248768 + "x": 7.83289156626506, + "y": 0.5599879518072293 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.936819277108433, - "y": 3.3684096385542173 + "x": 6.783831325301205, + "y": 3.586963855421687 }, "prevControl": { - "x": 8.80882222557424, - "y": 4.555339167581473 + "x": 8.65583427376701, + "y": 4.773893384448944 }, "nextControl": { - "x": 4.368807228915662, - "y": 1.740180722891567 + "x": 4.215819277108434, + "y": 1.958734939759037 }, "isLocked": false, "linkedName": null @@ -74,14 +74,14 @@ "rotationDegrees": -141.73750857193497 }, { - "waypointRelativePos": 2.187633262260129, + "waypointRelativePos": 2.113692946058101, "rotationDegrees": -35.84339182244795 } ], "constraintZones": [ { "name": "slowdown intake", - "minWaypointRelativePos": 1.2714640198511127, + "minWaypointRelativePos": 0.9439205955335208, "maxWaypointRelativePos": 2.1230769230769058, "constraints": { "maxVelocity": 1.0, diff --git a/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java b/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java index 7fc131c..9fa48c3 100644 --- a/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -12,7 +12,7 @@ public class NamedCommandBindings implements Binder { public void bind(Superstructure superstructure) { NamedCommands.registerCommand("Aim", superstructure.build(new AimPrep())); NamedCommands.registerCommand("Intake", superstructure.build(new RunIntake())); - NamedCommands.registerCommand("Retract", superstructure.build(new RetractIntake())) ; + NamedCommands.registerCommand("Retract", superstructure.build(new RetractIntake())); new EventTrigger("Aim").whileTrue(superstructure.build(new AimPrep())); new EventTrigger("Intake").whileTrue(superstructure.build(new RunIntake())); diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 26e6c42..f7d9230 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -49,6 +49,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.FieldManager; diff --git a/CompBot/src/main/java/frc/robot/subsystems/intake/Intake.java b/CompBot/src/main/java/frc/robot/subsystems/intake/Intake.java index f387df9..a2a9036 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/CompBot/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.intake.IntakeConstants.DeployConstants; import frc.robot.subsystems.intake.IntakeConstants.DeployConstants.DeployPosition; diff --git a/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 66bc760..6ab5c8b 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -39,9 +39,13 @@ public class IntakeConstants { .withCurrentLimits(new CurrentLimitsConfigs() .withSupplyCurrentLimitEnable(true) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(50) + .withSupplyCurrentLimit(40) .withStatorCurrentLimit(120)); + protected static final TalonFXConfiguration kIntakeMotorConfigDuringAuto = kIntakeMotorConfig.clone().withCurrentLimits( + kIntakeMotorConfig.CurrentLimits.clone().withSupplyCurrentLimit(80) + ); + protected static final Current kJamStatorThreshold = Amps.of(70); protected static final AngularVelocity kJamVelocityThreshold = RotationsPerSecond.of(0.3); diff --git a/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java index 56b34c0..a7b72a8 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java +++ b/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java @@ -10,6 +10,8 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.subsystems.intake.IntakeConstants.DeployConstants; import frc.robot.util.StatusSignalUtil; @@ -34,6 +36,12 @@ public IntakeIOHardware() { deployMotor = new TalonFX(DeployConstants.kDeployMotorId, StatusSignalUtil.canivore); deployMotor.getConfigurator().apply(DeployConstants.kDeployMotorConfig); + RobotModeTriggers.autonomous().onTrue(Commands.startEnd(() -> { + intakeMotor.getConfigurator().apply(IntakeConstants.kIntakeMotorConfigDuringAuto); + }, () -> { + intakeMotor.getConfigurator().apply(IntakeConstants.kIntakeMotorConfig); + })); + StatusSignalUtil.registerRioSignals( intakeMotor.getSupplyCurrent(false), From 89c2fe2ee4685ca602a65e9ab6aefb38ec2a71e9 Mon Sep 17 00:00:00 2001 From: Nolan Date: Tue, 14 Apr 2026 20:56:43 -0400 Subject: [PATCH 13/26] Create branch dcmp From ec2b7cb06c43088444dc9f22acacfeec9441bb9f Mon Sep 17 00:00:00 2001 From: Nolan Barker <107814443+TheRekrab@users.noreply.github.com> Date: Thu, 16 Apr 2026 12:15:32 -0400 Subject: [PATCH 14/26] path changes --- CompBot/src/main/deploy/pathplanner/paths/Alt.path | 2 +- .../src/main/deploy/pathplanner/paths/G211 who.path | 12 ++++++------ .../src/main/deploy/pathplanner/paths/Left Alt.path | 12 ++++++------ CompBot/src/main/deploy/pathplanner/paths/Left.path | 6 +++--- CompBot/src/main/deploy/pathplanner/paths/Loopy.path | 4 ++++ .../main/deploy/pathplanner/paths/Right Depot.path | 8 ++++---- 6 files changed, 24 insertions(+), 20 deletions(-) diff --git a/CompBot/src/main/deploy/pathplanner/paths/Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Alt.path index 7ed88ab..5c9f812 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Alt.path @@ -55,7 +55,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 3.9102257507213043 + "rotation": 0.8493895342646307 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/G211 who.path b/CompBot/src/main/deploy/pathplanner/paths/G211 who.path index 594a1ed..f367205 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/G211 who.path +++ b/CompBot/src/main/deploy/pathplanner/paths/G211 who.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.621625, - "y": 2.39621590909091 + "x": 3.5929397590361445, + "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 6.6894642421177455, - "y": 2.7547862025669643 + "x": 6.660779001153889, + "y": 2.7753998389306 }, "isLocked": false, - "linkedName": null + "linkedName": "Right Start" }, { "anchor": { @@ -104,7 +104,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 32.74683427363852 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path index 5651baa..2761ee8 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.4361022727272723, + "x": 3.5929397590361445, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 2.4217261055634807, - "y": 3.2004564907275324 + "x": 2.578565757324572, + "y": 3.200459293817526 }, "isLocked": false, - "linkedName": null + "linkedName": "Right Start" }, { "anchor": { @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 3.9102257507213043 + "rotation": 0.8493895342646307 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -41.98140661033514 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Left.path b/CompBot/src/main/deploy/pathplanner/paths/Left.path index 3e15706..b4780bb 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Left.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Left.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.860325301204819, - "y": 5.073132530120482 + "x": 6.869463009098878, + "y": 5.317665405269765 }, "isLocked": false, "linkedName": "Left Start" @@ -114,7 +114,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 3.9102257507213043 + "rotation": 0.8493895342646307 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path index 8db828d..334ad61 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Loopy.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path @@ -97,6 +97,10 @@ "waypointRelativePos": 0.5, "rotationDegrees": 0.0 }, + { + "waypointRelativePos": 1.4730290456431554, + "rotationDegrees": 0.0 + }, { "waypointRelativePos": 1.7800829875518442, "rotationDegrees": 32.762556552753445 diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path index 97e18e3..a3ef875 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.9919156626506025, - "y": 5.870855421686747 + "x": 0.412975903614458, + "y": 5.991060240963854 }, "prevControl": { - "x": 4.434373493975904, - "y": 6.187759036144577 + "x": 1.8554337349397616, + "y": 6.307963855421685 }, "nextControl": null, "isLocked": false, From cb629f2f7cd2a6fa11c38274d8dbe5f028901c0a Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Thu, 16 Apr 2026 14:32:33 -0400 Subject: [PATCH 15/26] Revert "path changes" This reverts commit ec2b7cb06c43088444dc9f22acacfeec9441bb9f. --- CompBot/src/main/deploy/pathplanner/paths/Alt.path | 2 +- .../src/main/deploy/pathplanner/paths/G211 who.path | 12 ++++++------ .../src/main/deploy/pathplanner/paths/Left Alt.path | 12 ++++++------ CompBot/src/main/deploy/pathplanner/paths/Left.path | 6 +++--- CompBot/src/main/deploy/pathplanner/paths/Loopy.path | 4 ---- .../main/deploy/pathplanner/paths/Right Depot.path | 8 ++++---- 6 files changed, 20 insertions(+), 24 deletions(-) diff --git a/CompBot/src/main/deploy/pathplanner/paths/Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Alt.path index 5c9f812..7ed88ab 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Alt.path @@ -55,7 +55,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.8493895342646307 + "rotation": 3.9102257507213043 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/G211 who.path b/CompBot/src/main/deploy/pathplanner/paths/G211 who.path index f367205..594a1ed 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/G211 who.path +++ b/CompBot/src/main/deploy/pathplanner/paths/G211 who.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.5929397590361445, - "y": 2.4168295454545454 + "x": 3.621625, + "y": 2.39621590909091 }, "prevControl": null, "nextControl": { - "x": 6.660779001153889, - "y": 2.7753998389306 + "x": 6.6894642421177455, + "y": 2.7547862025669643 }, "isLocked": false, - "linkedName": "Right Start" + "linkedName": null }, { "anchor": { @@ -104,7 +104,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 32.74683427363852 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path index 2761ee8..5651baa 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.5929397590361445, + "x": 3.4361022727272723, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 2.578565757324572, - "y": 3.200459293817526 + "x": 2.4217261055634807, + "y": 3.2004564907275324 }, "isLocked": false, - "linkedName": "Right Start" + "linkedName": null }, { "anchor": { @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.8493895342646307 + "rotation": 3.9102257507213043 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -41.98140661033514 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Left.path b/CompBot/src/main/deploy/pathplanner/paths/Left.path index b4780bb..3e15706 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Left.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Left.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.869463009098878, - "y": 5.317665405269765 + "x": 6.860325301204819, + "y": 5.073132530120482 }, "isLocked": false, "linkedName": "Left Start" @@ -114,7 +114,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.8493895342646307 + "rotation": 3.9102257507213043 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path index 334ad61..8db828d 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Loopy.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path @@ -97,10 +97,6 @@ "waypointRelativePos": 0.5, "rotationDegrees": 0.0 }, - { - "waypointRelativePos": 1.4730290456431554, - "rotationDegrees": 0.0 - }, { "waypointRelativePos": 1.7800829875518442, "rotationDegrees": 32.762556552753445 diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path index a3ef875..97e18e3 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.412975903614458, - "y": 5.991060240963854 + "x": 2.9919156626506025, + "y": 5.870855421686747 }, "prevControl": { - "x": 1.8554337349397616, - "y": 6.307963855421685 + "x": 4.434373493975904, + "y": 6.187759036144577 }, "nextControl": null, "isLocked": false, From 770e0d1b4a6a41d2027594403b73e8b72440586e Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Thu, 16 Apr 2026 15:27:12 -0400 Subject: [PATCH 16/26] Reapply "path changes" This reverts commit cb629f2f7cd2a6fa11c38274d8dbe5f028901c0a. --- CompBot/src/main/deploy/pathplanner/paths/Alt.path | 2 +- .../src/main/deploy/pathplanner/paths/G211 who.path | 12 ++++++------ .../src/main/deploy/pathplanner/paths/Left Alt.path | 12 ++++++------ CompBot/src/main/deploy/pathplanner/paths/Left.path | 6 +++--- CompBot/src/main/deploy/pathplanner/paths/Loopy.path | 4 ++++ .../main/deploy/pathplanner/paths/Right Depot.path | 8 ++++---- 6 files changed, 24 insertions(+), 20 deletions(-) diff --git a/CompBot/src/main/deploy/pathplanner/paths/Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Alt.path index 7ed88ab..5c9f812 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Alt.path @@ -55,7 +55,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 3.9102257507213043 + "rotation": 0.8493895342646307 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/G211 who.path b/CompBot/src/main/deploy/pathplanner/paths/G211 who.path index 594a1ed..f367205 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/G211 who.path +++ b/CompBot/src/main/deploy/pathplanner/paths/G211 who.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.621625, - "y": 2.39621590909091 + "x": 3.5929397590361445, + "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 6.6894642421177455, - "y": 2.7547862025669643 + "x": 6.660779001153889, + "y": 2.7753998389306 }, "isLocked": false, - "linkedName": null + "linkedName": "Right Start" }, { "anchor": { @@ -104,7 +104,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 32.74683427363852 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path index 5651baa..2761ee8 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.4361022727272723, + "x": 3.5929397590361445, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 2.4217261055634807, - "y": 3.2004564907275324 + "x": 2.578565757324572, + "y": 3.200459293817526 }, "isLocked": false, - "linkedName": null + "linkedName": "Right Start" }, { "anchor": { @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 3.9102257507213043 + "rotation": 0.8493895342646307 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -41.98140661033514 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Left.path b/CompBot/src/main/deploy/pathplanner/paths/Left.path index 3e15706..b4780bb 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Left.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Left.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.860325301204819, - "y": 5.073132530120482 + "x": 6.869463009098878, + "y": 5.317665405269765 }, "isLocked": false, "linkedName": "Left Start" @@ -114,7 +114,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 3.9102257507213043 + "rotation": 0.8493895342646307 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path index 8db828d..334ad61 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Loopy.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path @@ -97,6 +97,10 @@ "waypointRelativePos": 0.5, "rotationDegrees": 0.0 }, + { + "waypointRelativePos": 1.4730290456431554, + "rotationDegrees": 0.0 + }, { "waypointRelativePos": 1.7800829875518442, "rotationDegrees": 32.762556552753445 diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path index 97e18e3..a3ef875 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.9919156626506025, - "y": 5.870855421686747 + "x": 0.412975903614458, + "y": 5.991060240963854 }, "prevControl": { - "x": 4.434373493975904, - "y": 6.187759036144577 + "x": 1.8554337349397616, + "y": 6.307963855421685 }, "nextControl": null, "isLocked": false, From 3555f2f00c37f6fc0dc0c25d86bedbd797b074d3 Mon Sep 17 00:00:00 2001 From: Ben Date: Thu, 16 Apr 2026 19:28:04 -0400 Subject: [PATCH 17/26] Update main.yml for dump branch in use at States --- .github/workflows/main.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 5045b50..73f69d9 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -4,9 +4,9 @@ name: CI # events but only for the main branch. on: push: - branches: [ main, District-1, District-2, States, Worlds, iri ] + branches: [ main, District-1, District-2, dcmp, States, Worlds, iri ] pull_request: - branches: [ main, District-1, District-2, States, Worlds, iri ] + branches: [ main, District-1, District-2, dcmp, States, Worlds, iri ] # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: # This workflow contains a single job called "build" From fef6b75e946bde72683429ed78f4f6f7814e1431 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Fri, 17 Apr 2026 11:35:06 -0400 Subject: [PATCH 18/26] idek --- .../main/java/frc/robot/subsystems/intake/IntakeConstants.java | 2 +- .../main/java/frc/robot/subsystems/turret/TurretConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 6ab5c8b..41d735c 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -87,7 +87,7 @@ public static final class DeployConstants { .withMagnetSensor(new MagnetSensorConfigs() .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) .withAbsoluteSensorDiscontinuityPoint(0.5) - .withMagnetOffset(-0.036376953125)); + .withMagnetOffset(-0.37939453125)); protected static final AngularVelocity kMaxVelocity = RotationsPerSecond.of(0.4); protected static final AngularAcceleration kMaxAcceleration = RotationsPerSecondPerSecond.of(4); diff --git a/CompBot/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/CompBot/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 2255449..e8fa401 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -83,7 +83,7 @@ public class TurretConstants { .withKI(0) .withKD(0.1) .withKS(0.6) - .withKV(2.5) + .withKV(4.0) .withKA(0)) .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs() From b92d2f668e4734e9e5d6013ba0e40e1c1ac4440a Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Sat, 18 Apr 2026 08:25:27 -0400 Subject: [PATCH 19/26] many many changes --- .../{Alt Right.auto => Alt Right.auto.bak} | 0 ...P Alt Double.auto => FLIP Alt Single.auto} | 0 .../{FLIP Far.auto => FLIP Far.auto.bak} | 0 .../{FLIP Loopy .auto => FLIP Loopy.auto} | 0 .../autos/FLIP Single + Auto-beach.auto | 44 +++++++++++++++ .../{Left Depot.auto => Left Depot.auto.bak} | 0 ...{Loopy Depot.auto => Loopy Depot.auto.bak} | 0 ...{Right Depot.auto => Right Depot.auto.bak} | 0 .../deploy/pathplanner/paths/Auto-beach.path | 54 +++++++++++++++++++ .../deploy/pathplanner/paths/Far Right.path | 2 +- .../pathplanner/paths/Inverse Right.path | 6 +-- .../deploy/pathplanner/paths/Outpost.path | 2 +- .../pathplanner/paths/Right Cleanup.path | 2 +- .../deploy/pathplanner/paths/Right Depot.path | 2 +- .../robot/binding/NamedCommandBindings.java | 2 + .../frc/robot/commands/DrivetrainShake.java | 11 ++++ .../drivetrain/AutopilotConstants.java | 4 +- .../subsystems/drivetrain/Drivetrain.java | 2 +- 18 files changed, 121 insertions(+), 10 deletions(-) rename CompBot/src/main/deploy/pathplanner/autos/{Alt Right.auto => Alt Right.auto.bak} (100%) rename CompBot/src/main/deploy/pathplanner/autos/{FLIP Alt Double.auto => FLIP Alt Single.auto} (100%) rename CompBot/src/main/deploy/pathplanner/autos/{FLIP Far.auto => FLIP Far.auto.bak} (100%) rename CompBot/src/main/deploy/pathplanner/autos/{FLIP Loopy .auto => FLIP Loopy.auto} (100%) create mode 100644 CompBot/src/main/deploy/pathplanner/autos/FLIP Single + Auto-beach.auto rename CompBot/src/main/deploy/pathplanner/autos/{Left Depot.auto => Left Depot.auto.bak} (100%) rename CompBot/src/main/deploy/pathplanner/autos/{Loopy Depot.auto => Loopy Depot.auto.bak} (100%) rename CompBot/src/main/deploy/pathplanner/autos/{Right Depot.auto => Right Depot.auto.bak} (100%) create mode 100644 CompBot/src/main/deploy/pathplanner/paths/Auto-beach.path create mode 100644 CompBot/src/main/java/frc/robot/commands/DrivetrainShake.java diff --git a/CompBot/src/main/deploy/pathplanner/autos/Alt Right.auto b/CompBot/src/main/deploy/pathplanner/autos/Alt Right.auto.bak similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/Alt Right.auto rename to CompBot/src/main/deploy/pathplanner/autos/Alt Right.auto.bak diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Double.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Double.auto rename to CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Far.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Far.auto.bak similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/FLIP Far.auto rename to CompBot/src/main/deploy/pathplanner/autos/FLIP Far.auto.bak diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy .auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy .auto rename to CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Single + Auto-beach.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Single + Auto-beach.auto new file mode 100644 index 0000000..dc33196 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Single + Auto-beach.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "wait", + "data": { + "waitTime": 9.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Auto-beach" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/Left Depot.auto b/CompBot/src/main/deploy/pathplanner/autos/Left Depot.auto.bak similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/Left Depot.auto rename to CompBot/src/main/deploy/pathplanner/autos/Left Depot.auto.bak diff --git a/CompBot/src/main/deploy/pathplanner/autos/Loopy Depot.auto b/CompBot/src/main/deploy/pathplanner/autos/Loopy Depot.auto.bak similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/Loopy Depot.auto rename to CompBot/src/main/deploy/pathplanner/autos/Loopy Depot.auto.bak diff --git a/CompBot/src/main/deploy/pathplanner/autos/Right Depot.auto b/CompBot/src/main/deploy/pathplanner/autos/Right Depot.auto.bak similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/Right Depot.auto rename to CompBot/src/main/deploy/pathplanner/autos/Right Depot.auto.bak diff --git a/CompBot/src/main/deploy/pathplanner/paths/Auto-beach.path b/CompBot/src/main/deploy/pathplanner/paths/Auto-beach.path new file mode 100644 index 0000000..c448198 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/Auto-beach.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.7187228915662653, + "y": 2.4168295454545454 + }, + "prevControl": null, + "nextControl": { + "x": 3.909671897289586, + "y": 3.1098858773181166 + }, + "isLocked": false, + "linkedName": "Right 2 Start" + }, + { + "anchor": { + "x": 8.25706134094151, + "y": 2.566462196861627 + }, + "prevControl": { + "x": 6.691483594864478, + "y": 1.8418972895863055 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -44.2926806314556 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -41.30950981274552 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Far Right.path b/CompBot/src/main/deploy/pathplanner/paths/Far Right.path index d0db508..a14d433 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Far Right.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Far Right.path @@ -130,7 +130,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": -41.30950981274551 + "rotation": -41.30950981274552 }, "reversed": false, "folder": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path b/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path index 77f67a3..f811e85 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 2.806144578313253, + "x": 2.7187228915662653, "y": 2.4168295454545454 }, "prevControl": { - "x": 3.055855915741729, + "x": 2.968434228994741, "y": 2.4048192182449206 }, "nextControl": null, @@ -164,7 +164,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 138.22729829188262 }, "reversed": false, "folder": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Outpost.path b/CompBot/src/main/deploy/pathplanner/paths/Outpost.path index 689d1e1..d66469c 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Outpost.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Outpost.path @@ -55,7 +55,7 @@ "folder": null, "idealStartingState": { "velocity": 0.0, - "rotation": -41.30950981274551 + "rotation": -41.30950981274552 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right Cleanup.path b/CompBot/src/main/deploy/pathplanner/paths/Right Cleanup.path index db2495e..1e6f170 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right Cleanup.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right Cleanup.path @@ -164,7 +164,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -41.30950981274551 + "rotation": -41.30950981274552 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path index a3ef875..4464f44 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path @@ -20,7 +20,7 @@ "y": 5.991060240963854 }, "prevControl": { - "x": 1.8554337349397616, + "x": 1.8554337349397614, "y": 6.307963855421685 }, "nextControl": null, diff --git a/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java b/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java index 9fa48c3..c55c74f 100644 --- a/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -4,6 +4,7 @@ import com.pathplanner.lib.events.EventTrigger; import frc.robot.commands.AimPrep; +import frc.robot.commands.DrivetrainShake; import frc.robot.commands.RetractIntake; import frc.robot.commands.RunIntake; import frc.robot.superstructure.Superstructure; @@ -13,6 +14,7 @@ public void bind(Superstructure superstructure) { NamedCommands.registerCommand("Aim", superstructure.build(new AimPrep())); NamedCommands.registerCommand("Intake", superstructure.build(new RunIntake())); NamedCommands.registerCommand("Retract", superstructure.build(new RetractIntake())); + NamedCommands.registerCommand("Shake", superstructure.buildWithoutProxy(new DrivetrainShake())); new EventTrigger("Aim").whileTrue(superstructure.build(new AimPrep())); new EventTrigger("Intake").whileTrue(superstructure.build(new RunIntake())); diff --git a/CompBot/src/main/java/frc/robot/commands/DrivetrainShake.java b/CompBot/src/main/java/frc/robot/commands/DrivetrainShake.java new file mode 100644 index 0000000..91e70e7 --- /dev/null +++ b/CompBot/src/main/java/frc/robot/commands/DrivetrainShake.java @@ -0,0 +1,11 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.StateManager; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class DrivetrainShake implements CommandBuilder { + public Command build(Subsystems subsystems, StateManager state) { + return subsystems.drivetrain().shake(); + } +} diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/AutopilotConstants.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/AutopilotConstants.java index fea8d59..2663c27 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/AutopilotConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/AutopilotConstants.java @@ -10,7 +10,7 @@ public class AutopilotConstants { private static final APConstraints kConstraints = new APConstraints() - .withVelocity(4.0) + .withVelocity(2.0) .withAcceleration(10.0) .withJerk(8.0); @@ -24,7 +24,7 @@ public class AutopilotConstants { private static final APProfile kLooseProfile = new APProfile(kConstraints) .withBeelineRadius(Meters.of(0.25)) .withErrorTheta(Degrees.of(30)) - .withErrorXY(Inches.of(12)); + .withErrorXY(Inches.of(4)); public static final Autopilot kLooseAutopilot = new Autopilot(kLooseProfile); diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index f7d9230..7724ca7 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -589,7 +589,7 @@ private Command shakeAbout(Supplier center) { return Commands.sequence( this.runOnce(() -> { Rotation2d randomAngle = Rotation2d.fromRotations(Math.random()); - double radius = 0.5; + double radius = 0.25; target[0] = new APTarget(center.get().plus(new Transform2d( radius * randomAngle.getCos(), radius * randomAngle.getSin(), From 4df9e5af8a0702fec2cfce7d6ffef43a6cd99069 Mon Sep 17 00:00:00 2001 From: Nolan Barker <107814443+TheRekrab@users.noreply.github.com> Date: Wed, 22 Apr 2026 21:09:36 -0400 Subject: [PATCH 20/26] shoots better at long shots because of more moi --- .../src/main/java/frc/robot/Constants.java | 2 + .../frc/robot/binding/DriverXboxBindings.java | 6 +-- .../frc/robot/binding/KeyboardBindings.java | 6 +-- .../subsystems/drivetrain/Drivetrain.java | 2 +- .../subsystems/shooter/ShooterConstants.java | 52 ++++++++++--------- .../robot/superstructure/StateManager.java | 10 ++-- .../robot/superstructure/Superstructure.java | 7 +-- 7 files changed, 45 insertions(+), 40 deletions(-) diff --git a/CompBot/src/main/java/frc/robot/Constants.java b/CompBot/src/main/java/frc/robot/Constants.java index 2983477..34830c8 100644 --- a/CompBot/src/main/java/frc/robot/Constants.java +++ b/CompBot/src/main/java/frc/robot/Constants.java @@ -15,6 +15,7 @@ import frc.robot.aiming.AimConstraints; import frc.robot.aiming.AimStrategy; import frc.robot.aiming.ToFAim; +import frc.robot.aiming.TuneAim; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.util.BoundingBox; @@ -46,6 +47,7 @@ public static class AimConstants { public static final AimStrategy kScoringAim = new ToFAim(ShooterConstants.scoringMeasurements, constraints); // public static final AimStrategy kScoringAim = new TuneAim(); public static final AimStrategy kFeedingAim = new ToFAim(ShooterConstants.feedingMeasurements, constraints); + // public static final AimStrategy kFeedingAim = kScoringAim; } diff --git a/CompBot/src/main/java/frc/robot/binding/DriverXboxBindings.java b/CompBot/src/main/java/frc/robot/binding/DriverXboxBindings.java index 1b4e38c..81363bb 100644 --- a/CompBot/src/main/java/frc/robot/binding/DriverXboxBindings.java +++ b/CompBot/src/main/java/frc/robot/binding/DriverXboxBindings.java @@ -18,7 +18,7 @@ public class DriverXboxBindings implements Binder { private final DoubleSupplier vx, vy, vrot; - private final Trigger shoot, intake, resetPerspective, slowbot, retract; + private final Trigger shoot, intake, resetPerspective, antislowbot, retract; public DriverXboxBindings() { controller = new CommandXboxController(Driver.kDriveControllerPort); @@ -30,12 +30,12 @@ public DriverXboxBindings() { shoot = controller.rightBumper(); intake = controller.rightTrigger(); resetPerspective = controller.leftBumper(); - slowbot = controller.leftTrigger(); + antislowbot = controller.leftTrigger(); retract = controller.x(); } public void bind(Superstructure superstructure) { - superstructure.bindDrive(vx, vy, vrot, slowbot); + superstructure.bindDrive(vx, vy, vrot, antislowbot); shoot.toggleOnTrue(superstructure.build(new AimPrep())); intake.toggleOnTrue(superstructure.build(new RunIntake())); diff --git a/CompBot/src/main/java/frc/robot/binding/KeyboardBindings.java b/CompBot/src/main/java/frc/robot/binding/KeyboardBindings.java index 668aeb0..6f61a80 100644 --- a/CompBot/src/main/java/frc/robot/binding/KeyboardBindings.java +++ b/CompBot/src/main/java/frc/robot/binding/KeyboardBindings.java @@ -14,7 +14,7 @@ public class KeyboardBindings implements Binder { private final CommandGenericHID controller; - private final Trigger shoot, intake, slowbot, resetForwards, index, retractIntake; + private final Trigger shoot, intake, antislowbot, resetForwards, index, retractIntake; private final DoubleSupplier vx, vy, vrot; @@ -23,7 +23,7 @@ public KeyboardBindings() { shoot = controller.button(1); intake = controller.button(2); - slowbot = controller.button(3); + antislowbot = controller.button(3); resetForwards = controller.button(4); index = controller.button(5); retractIntake = controller.button(6); @@ -34,7 +34,7 @@ public KeyboardBindings() { } public void bind(Superstructure superstructure) { - superstructure.bindDrive(vx, vy, vrot, slowbot); + superstructure.bindDrive(vx, vy, vrot, antislowbot); shoot.toggleOnTrue(superstructure.build(new AimPrep())); intake.whileTrue(superstructure.build(new RunIntake())); diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 7724ca7..09752aa 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -573,7 +573,7 @@ public Command track(Supplier params) { } public Command setMemorySpot() { - return Commands.runOnce(() -> memorySpot = robotPose()); + return Commands.runOnce(() -> memorySpot = robotPose()).ignoringDisable(true); } public Command goHome() { diff --git a/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 4024bdd..a9fd13a 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -55,12 +55,12 @@ public final class ShooterConstants { .withTorqueCurrent(new TorqueCurrentConfigs() .withPeakReverseTorqueCurrent(0.0) - .withPeakForwardTorqueCurrent(80.0)) + .withPeakForwardTorqueCurrent(100.0)) .withCurrentLimits(new CurrentLimitsConfigs() .withSupplyCurrentLimitEnable(true) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(80) + .withSupplyCurrentLimit(100) .withStatorCurrentLimit(100)); public static final Distance kRadius = Inches.of(2); @@ -69,7 +69,7 @@ public final class ShooterConstants { public static final MotorAlignmentValue kMotor2Alignment = MotorAlignmentValue.Aligned; public static final LinearVelocity kMaxLinearSpeed = MetersPerSecond.of(15.5); - public static final AngularVelocity kMaxRotationalSpeed = RotationsPerSecond.of(85.0); + public static final AngularVelocity kMaxRotationalSpeed = RotationsPerSecond.of(100.0); public static final class HoodConstants { protected static final int kMotorID = 56; @@ -124,30 +124,32 @@ public static final class HoodConstants { protected static final Angle kOffset = Degrees.of(18.0); } + private static final double dipper = 0.75; + public static final List scoringMeasurements = List.of( - new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 31, Seconds.of(0.962)), - new AimMeasurement(Meters.of(2.41), Rotation2d.fromDegrees(70), 34, Seconds.of(1.006)), - new AimMeasurement(Meters.of(2.93), Rotation2d.fromDegrees(67), 35, Seconds.of(1.016)), - new AimMeasurement(Meters.of(3.33), Rotation2d.fromDegrees(65), 37, Seconds.of(1.014)), - new AimMeasurement(Meters.of(3.81), Rotation2d.fromDegrees(61), 39, Seconds.of(1.07)), - new AimMeasurement(Meters.of(4.29), Rotation2d.fromDegrees(59), 39, Seconds.of(1.018)), - new AimMeasurement(Meters.of(4.77), Rotation2d.fromDegrees(59), 41.5 , Seconds.of(0.924)), - new AimMeasurement(Meters.of(5.26), Rotation2d.fromDegrees(58), 43.5 , Seconds.of(1.136)), - new AimMeasurement(Meters.of(5.73), Rotation2d.fromDegrees(55), 44.75, Seconds.of(1.15)), - new AimMeasurement(Meters.of(6.22), Rotation2d.fromDegrees(52), 47, Seconds.of(1.126)), - new AimMeasurement(Meters.of(6.84), Rotation2d.fromDegrees(50), 50, Seconds.of(1.134))); + new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 29 - dipper, Seconds.of(0.962)), + new AimMeasurement(Meters.of(2.41), Rotation2d.fromDegrees(70), 32 - dipper, Seconds.of(1.006)), + new AimMeasurement(Meters.of(2.93), Rotation2d.fromDegrees(67), 33 - dipper, Seconds.of(1.016)), + new AimMeasurement(Meters.of(3.33), Rotation2d.fromDegrees(65), 35 - dipper, Seconds.of(1.014)), + new AimMeasurement(Meters.of(3.81), Rotation2d.fromDegrees(61), 36.5 - dipper, Seconds.of(1.07)), + new AimMeasurement(Meters.of(4.23), Rotation2d.fromDegrees(60), 37.5 - dipper, Seconds.of(1.0325)), + new AimMeasurement(Meters.of(4.77), Rotation2d.fromDegrees(59), 40.5 - dipper, Seconds.of(1.122)), + new AimMeasurement(Meters.of(5.26), Rotation2d.fromDegrees(58), 41 - dipper, Seconds.of(1.136)), + new AimMeasurement(Meters.of(5.73), Rotation2d.fromDegrees(55), 42 - dipper, Seconds.of(1.15)), + new AimMeasurement(Meters.of(6.22), Rotation2d.fromDegrees(55), 44 - dipper, Seconds.of(1.202)), + new AimMeasurement(Meters.of(6.84), Rotation2d.fromDegrees(53), 46.25 - dipper, Seconds.of(1.265))); public static final List feedingMeasurements = List.of( - new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 31, Seconds.of(0.962)), - new AimMeasurement(Meters.of(2.41), Rotation2d.fromDegrees(70), 34, Seconds.of(1.006)), - new AimMeasurement(Meters.of(2.93), Rotation2d.fromDegrees(67), 35, Seconds.of(1.016)), - new AimMeasurement(Meters.of(3.33), Rotation2d.fromDegrees(65), 37, Seconds.of(1.014)), - new AimMeasurement(Meters.of(3.81), Rotation2d.fromDegrees(61), 39, Seconds.of(1.07)), - new AimMeasurement(Meters.of(4.29), Rotation2d.fromDegrees(59), 39, Seconds.of(1.018)), - new AimMeasurement(Meters.of(4.77), Rotation2d.fromDegrees(59), 41.5 , Seconds.of(0.924)), - new AimMeasurement(Meters.of(5.26), Rotation2d.fromDegrees(58), 43.5 , Seconds.of(1.136)), - new AimMeasurement(Meters.of(5.73), Rotation2d.fromDegrees(55), 44.75, Seconds.of(1.15)), - new AimMeasurement(Meters.of(6.22), Rotation2d.fromDegrees(52), 47, Seconds.of(1.126)), - new AimMeasurement(Meters.of(6.84), Rotation2d.fromDegrees(50), 50, Seconds.of(1.134)), + new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 29, Seconds.of(0)), + new AimMeasurement(Meters.of(2.41), Rotation2d.fromDegrees(70), 32, Seconds.of(1.006)), + new AimMeasurement(Meters.of(2.93), Rotation2d.fromDegrees(67), 33, Seconds.of(1.016)), + new AimMeasurement(Meters.of(3.33), Rotation2d.fromDegrees(65), 35, Seconds.of(1.014)), + new AimMeasurement(Meters.of(3.81), Rotation2d.fromDegrees(61), 36.5, Seconds.of(1.07)), + new AimMeasurement(Meters.of(4.23), Rotation2d.fromDegrees(60), 37.5, Seconds.of(1.018)), + new AimMeasurement(Meters.of(4.77), Rotation2d.fromDegrees(59), 40.5, Seconds.of(0.924)), + new AimMeasurement(Meters.of(5.26), Rotation2d.fromDegrees(58), 41, Seconds.of(1.136)), + new AimMeasurement(Meters.of(5.73), Rotation2d.fromDegrees(55), 42, Seconds.of(1.15)), + new AimMeasurement(Meters.of(6.22), Rotation2d.fromDegrees(55), 44, Seconds.of(1.126)), + new AimMeasurement(Meters.of(6.84), Rotation2d.fromDegrees(53), 46.25, Seconds.of(1.134)), new AimMeasurement(Meters.of(15), Rotation2d.fromDegrees(50), 80, Seconds.of(2))); } diff --git a/CompBot/src/main/java/frc/robot/superstructure/StateManager.java b/CompBot/src/main/java/frc/robot/superstructure/StateManager.java index a6730a7..a4839f9 100644 --- a/CompBot/src/main/java/frc/robot/superstructure/StateManager.java +++ b/CompBot/src/main/java/frc/robot/superstructure/StateManager.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; 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.button.Trigger; @@ -72,12 +73,14 @@ private ShootMode calculateWantedShootMode() { // If we're not in alliance zone, then all requirements for feeding have been met. // We want to feed. + boolean testing = DriverStation.isTestEnabled(); + if (!inAllianceZone) { - return FieldUtils.inNoFeedZone(robotPose()) ? ShootMode.Donut : ShootMode.Feeding; + return FieldUtils.inNoFeedZone(robotPose()) && !testing ? ShootMode.Donut : ShootMode.Feeding; } // By now, we're in a scoring position. - if (FieldUtils.inTowerZone(robotPose())) { + if (FieldUtils.inTowerZone(robotPose()) && !testing) { return ShootMode.Donut; } @@ -179,8 +182,9 @@ public Command getAuton() { public void update() { params = new AimParams(AimStatus.Unchecked); - params = aimParams(); wantedShootMode = calculateWantedShootMode(); + params = aimParams(); + SmartDashboard.putString("Wanted Shoot Mode", wantedShootMode.toString()); } public Command runPartyMode(double duration) { diff --git a/CompBot/src/main/java/frc/robot/superstructure/Superstructure.java b/CompBot/src/main/java/frc/robot/superstructure/Superstructure.java index 282367b..67ab225 100644 --- a/CompBot/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/CompBot/src/main/java/frc/robot/superstructure/Superstructure.java @@ -98,13 +98,10 @@ private Subsystems createTestBotSubsystems() { return new Subsystems(drivetrain, turret, shooter, indexer, intake, climber, led); } - public void bindDrive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier vrot, BooleanSupplier slowbot) { + public void bindDrive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier vrot, BooleanSupplier antislowbot) { subsystems.drivetrain.setDefaultCommand(subsystems.drivetrain.teleopDrive(vx, vy, vrot, () -> { - if (slowbot.getAsBoolean()) { - return TeleopDriveMode.AccelerationLimitedFieldRelative; - } if (state.shooting(ShootMode.Scoring).getAsBoolean()) { - return TeleopDriveMode.SlowFieldRelative; + return antislowbot.getAsBoolean() ? TeleopDriveMode.SlowFieldRelative : TeleopDriveMode.AccelerationLimitedFieldRelative; } return TeleopDriveMode.FieldRelative; })); From d20fb551c2b736d6679eca6f450e5171099174a9 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Thu, 23 Apr 2026 17:22:35 -0400 Subject: [PATCH 21/26] autons configured --- .../pathplanner/autos/FLIP Alt Inverse.auto | 44 +++++++++++++++++++ .../pathplanner/autos/FLIP Alt Single.auto | 25 ----------- .../main/deploy/pathplanner/paths/Alt.path | 4 +- 3 files changed, 46 insertions(+), 27 deletions(-) create mode 100644 CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Inverse.auto diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Inverse.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Inverse.auto new file mode 100644 index 0000000..0a407b2 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Inverse.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Alt" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Inverse Right" + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto index 02ecbc7..edbd557 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto @@ -16,31 +16,6 @@ "pathName": "Right" } }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Aim" - } - }, - { - "type": "wait", - "data": { - "waitTime": 10.0 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Right Cleanup" - } - }, { "type": "named", "data": { diff --git a/CompBot/src/main/deploy/pathplanner/paths/Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Alt.path index 5c9f812..fda6989 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Alt.path @@ -40,7 +40,7 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 2.0, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -57,5 +57,5 @@ "velocity": 0, "rotation": 0.8493895342646307 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file From fab4fb416f720243f4f0a543d48f299e0b77cde3 Mon Sep 17 00:00:00 2001 From: Nolan Barker <107814443+TheRekrab@users.noreply.github.com> Date: Wed, 29 Apr 2026 18:32:23 -0400 Subject: [PATCH 22/26] Autons Have been Fixed whoops --- .../pathplanner/autos/FLIP Alt Single.auto | 28 ++--- .../deploy/pathplanner/autos/FLIP Loopy.auto | 19 ---- ...epot.auto.bak => Simple Middle Depot.auto} | 0 .../main/deploy/pathplanner/paths/Alt.path | 16 +-- .../pathplanner/paths/Center Loop-de-loo.path | 4 +- .../main/deploy/pathplanner/paths/Loopy.path | 105 +++++++++--------- .../pathplanner/paths/Middle Depot.path | 10 +- .../main/deploy/pathplanner/paths/Right.path | 4 +- 8 files changed, 79 insertions(+), 107 deletions(-) rename CompBot/src/main/deploy/pathplanner/autos/{Simple Middle Depot.auto.bak => Simple Middle Depot.auto} (100%) diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto index 02ecbc7..96c0917 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto @@ -4,32 +4,20 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "Alt" - } - }, - { - "type": "path", - "data": { - "pathName": "Right" - } - }, { "type": "race", "data": { "commands": [ { - "type": "named", + "type": "wait", "data": { - "name": "Aim" + "waitTime": 0.5 } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 10.0 + "name": "Aim" } } ] @@ -38,7 +26,13 @@ { "type": "path", "data": { - "pathName": "Right Cleanup" + "pathName": "Alt" + } + }, + { + "type": "path", + "data": { + "pathName": "Inverse Right" } }, { diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto index 9a8bf88..6d1b8cc 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto @@ -4,25 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "named", - "data": { - "name": "Aim" - } - } - ] - } - }, { "type": "path", "data": { diff --git a/CompBot/src/main/deploy/pathplanner/autos/Simple Middle Depot.auto.bak b/CompBot/src/main/deploy/pathplanner/autos/Simple Middle Depot.auto similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/Simple Middle Depot.auto.bak rename to CompBot/src/main/deploy/pathplanner/autos/Simple Middle Depot.auto diff --git a/CompBot/src/main/deploy/pathplanner/paths/Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Alt.path index 5c9f812..2d1cf35 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Alt.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.8573536084423283, - "y": 4.85660485021398 + "x": 2.2925421686746983, + "y": 4.821795180722892 }, "isLocked": false, "linkedName": "Left Start" @@ -20,8 +20,8 @@ "y": 2.4168295454545454 }, "prevControl": { - "x": 2.9537847045685006, - "y": 3.1616405135520687 + "x": 2.4783132530120486, + "y": 3.0624337349397592 }, "nextControl": null, "isLocked": false, @@ -34,13 +34,13 @@ "eventMarkers": [ { "name": "Aim", - "waypointRelativePos": 0, - "endWaypointRelativePos": 0, + "waypointRelativePos": 0.1121588089329983, + "endWaypointRelativePos": 1.0, "command": null } ], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 1.5, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -57,5 +57,5 @@ "velocity": 0, "rotation": 0.8493895342646307 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-loo.path b/CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-loo.path index 42273a9..f230ee5 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-loo.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-loo.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 0.4129759036144578, + "x": 0.41297590361445735, "y": 4.155204819277109 }, "isLocked": false, @@ -52,7 +52,7 @@ "y": 5.317665405269765 }, "prevControl": { - "x": 4.852289389845073, + "x": 4.852289389845074, "y": 5.874027744784743 }, "nextControl": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path index 334ad61..727b9c2 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Loopy.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path @@ -16,80 +16,64 @@ }, { "anchor": { - "x": 2.379963855421687, - "y": 3.3246987951807228 + "x": 2.3690361445783132, + "y": 3.1280000000000006 }, "prevControl": { - "x": 2.1832650602409642, - "y": 3.63067469879518 + "x": 2.088097540023956, + "y": 3.359052310287696 }, "nextControl": { - "x": 3.4852291968414937, - "y": 1.6053971529721363 + "x": 3.5383012048192777, + "y": 2.166361445783133 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.297433734939758, - "y": 2.7783132530120485 + "x": 6.0079638554216865, + "y": 2.811096385542169 }, "prevControl": { - "x": 6.303012048192771, - "y": 2.461409638554217 + "x": 5.323849123256161, + "y": 2.022877237612325 }, "nextControl": { - "x": 8.583996122143095, - "y": 3.188316651131794 + "x": 6.510638554216868, + "y": 3.3902650602409645 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.062373493975903, - "y": 4.450253012048193 + "x": 6.0079638554216865, + "y": 5.073132530120482 }, "prevControl": { - "x": 8.160722891566266, - "y": 3.860156626506024 + "x": 6.893108433734941, + "y": 4.22077108433735 }, "nextControl": { - "x": 7.903547856180251, - "y": 5.403206838822087 + "x": 4.747916887146947, + "y": 6.286511092162821 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.029819277108434, - "y": 4.450253012048193 + "x": 2.7952168674698794, + "y": 5.3463253012048195 }, "prevControl": { - "x": 6.582304041411692, - "y": 3.0258782290788577 - }, - "nextControl": { - "x": 5.33044578313253, - "y": 6.2533253012048196 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.286711643768041, - "y": 5.317665405269765 - }, - "prevControl": { - "x": 4.401338149792139, - "y": 5.17560516430591 + "x": 3.909843373493978, + "y": 5.204265060240965 }, "nextControl": null, "isLocked": false, - "linkedName": "Left 2 Start" + "linkedName": null } ], "rotationTargets": [ @@ -102,16 +86,16 @@ "rotationDegrees": 0.0 }, { - "waypointRelativePos": 1.7800829875518442, - "rotationDegrees": 32.762556552753445 + "waypointRelativePos": 2.008298755186713, + "rotationDegrees": 43.48173935506697 }, { - "waypointRelativePos": 3.6556016597510514, - "rotationDegrees": -150.84535599436526 + "waypointRelativePos": 2.85, + "rotationDegrees": 134.62591321412003 }, { - "waypointRelativePos": 4.54356846473027, - "rotationDegrees": 145.60146017476993 + "waypointRelativePos": 3.485477178423249, + "rotationDegrees": 180.0 } ], "constraintZones": [ @@ -120,7 +104,7 @@ "minWaypointRelativePos": 0.45, "maxWaypointRelativePos": 1.3, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 0.6, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -130,8 +114,8 @@ }, { "name": "Bump End ", - "minWaypointRelativePos": 4.183622828784128, - "maxWaypointRelativePos": 4.799007444168695, + "minWaypointRelativePos": 3.183622828784128, + "maxWaypointRelativePos": 3.799007444168695, "constraints": { "maxVelocity": 3.0, "maxAcceleration": 5.0, @@ -144,9 +128,22 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 1.672456575682393, - "maxWaypointRelativePos": 4.084367245657575, + "maxWaypointRelativePos": 3.0843672456575746, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Intake", + "minWaypointRelativePos": 1.854094292803968, + "maxWaypointRelativePos": 3.188089330024804, "constraints": { - "maxVelocity": 2.5, + "maxVelocity": 1.75, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -159,8 +156,8 @@ "eventMarkers": [ { "name": "Aim", - "waypointRelativePos": 0.4516129032257933, - "endWaypointRelativePos": 1.2952853598014844, + "waypointRelativePos": 0.33746898263028435, + "endWaypointRelativePos": 1.2754342431761725, "command": null }, { @@ -171,7 +168,7 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 3.0, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -188,5 +185,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Middle Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Middle Depot.path index 7a730f3..3e62805 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Middle Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Middle Depot.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 2.9919156626506025, - "y": 5.870855421686747 + "x": 0.5714978601997143, + "y": 5.982268188302426 }, "prevControl": { - "x": 4.784060240963855, - "y": 5.892710843373494 + "x": 2.3636424385129686, + "y": 6.004123609989173 }, "nextControl": null, "isLocked": false, - "linkedName": "Depot" + "linkedName": null } ], "rotationTargets": [], diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right.path b/CompBot/src/main/deploy/pathplanner/paths/Right.path index 2266640..3261ded 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right.path @@ -74,7 +74,7 @@ "rotationDegrees": -141.73750857193497 }, { - "waypointRelativePos": 2.113692946058101, + "waypointRelativePos": 2.1834024896265594, "rotationDegrees": -35.84339182244795 } ], @@ -97,7 +97,7 @@ "minWaypointRelativePos": 2.158808933002488, "maxWaypointRelativePos": 2.7841191066997464, "constraints": { - "maxVelocity": 2.0, + "maxVelocity": 3.0, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, From 4e323f4c8904c79691691119d9fb0394493b264c Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Wed, 29 Apr 2026 17:42:07 -0500 Subject: [PATCH 23/26] removed dipper and added tag filter --- .../subsystems/shooter/ShooterConstants.java | 24 +++++++++---------- .../localization/LocalizationConstants.java | 6 ++--- 2 files changed, 14 insertions(+), 16 deletions(-) diff --git a/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index a9fd13a..f3fd3f8 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -124,20 +124,18 @@ public static final class HoodConstants { protected static final Angle kOffset = Degrees.of(18.0); } - private static final double dipper = 0.75; - public static final List scoringMeasurements = List.of( - new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 29 - dipper, Seconds.of(0.962)), - new AimMeasurement(Meters.of(2.41), Rotation2d.fromDegrees(70), 32 - dipper, Seconds.of(1.006)), - new AimMeasurement(Meters.of(2.93), Rotation2d.fromDegrees(67), 33 - dipper, Seconds.of(1.016)), - new AimMeasurement(Meters.of(3.33), Rotation2d.fromDegrees(65), 35 - dipper, Seconds.of(1.014)), - new AimMeasurement(Meters.of(3.81), Rotation2d.fromDegrees(61), 36.5 - dipper, Seconds.of(1.07)), - new AimMeasurement(Meters.of(4.23), Rotation2d.fromDegrees(60), 37.5 - dipper, Seconds.of(1.0325)), - new AimMeasurement(Meters.of(4.77), Rotation2d.fromDegrees(59), 40.5 - dipper, Seconds.of(1.122)), - new AimMeasurement(Meters.of(5.26), Rotation2d.fromDegrees(58), 41 - dipper, Seconds.of(1.136)), - new AimMeasurement(Meters.of(5.73), Rotation2d.fromDegrees(55), 42 - dipper, Seconds.of(1.15)), - new AimMeasurement(Meters.of(6.22), Rotation2d.fromDegrees(55), 44 - dipper, Seconds.of(1.202)), - new AimMeasurement(Meters.of(6.84), Rotation2d.fromDegrees(53), 46.25 - dipper, Seconds.of(1.265))); + new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 28.25, Seconds.of(0.962)), + new AimMeasurement(Meters.of(2.41), Rotation2d.fromDegrees(70), 31.25, Seconds.of(1.006)), + new AimMeasurement(Meters.of(2.93), Rotation2d.fromDegrees(67), 32.25, Seconds.of(1.016)), + new AimMeasurement(Meters.of(3.33), Rotation2d.fromDegrees(65), 34.25, Seconds.of(1.014)), + new AimMeasurement(Meters.of(3.81), Rotation2d.fromDegrees(61), 35.75, Seconds.of(1.07)), + new AimMeasurement(Meters.of(4.23), Rotation2d.fromDegrees(60), 36.75, Seconds.of(1.0325)), + new AimMeasurement(Meters.of(4.77), Rotation2d.fromDegrees(59), 39.75, Seconds.of(1.122)), + new AimMeasurement(Meters.of(5.26), Rotation2d.fromDegrees(58), 40.25, Seconds.of(1.136)), + new AimMeasurement(Meters.of(5.73), Rotation2d.fromDegrees(55), 41.25, Seconds.of(1.15)), + new AimMeasurement(Meters.of(6.22), Rotation2d.fromDegrees(55), 43.25, Seconds.of(1.202)), + new AimMeasurement(Meters.of(6.84), Rotation2d.fromDegrees(53), 45.5, Seconds.of(1.265))); public static final List feedingMeasurements = List.of( new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 29, Seconds.of(0)), diff --git a/CompBot/src/main/java/frc/robot/vision/localization/LocalizationConstants.java b/CompBot/src/main/java/frc/robot/vision/localization/LocalizationConstants.java index b2d6ce3..1bbac8b 100644 --- a/CompBot/src/main/java/frc/robot/vision/localization/LocalizationConstants.java +++ b/CompBot/src/main/java/frc/robot/vision/localization/LocalizationConstants.java @@ -78,15 +78,15 @@ public class LocalizationConstants { protected static final double kAvgErr = 0.08; protected static final double kErrStdDevs = 0.02; - public static final boolean kEnableTagFilter = false; + public static final boolean kEnableTagFilter = true; - protected static final Set kApprovedTagIds = Set.of(2, 3, 4, 5, 8, 9, 10, 11); + protected static final Set kApprovedTagIds = Set.of(2,3,4,5,8,9,10,11, 18,19,20,21,24,25,26,27, 15,16,13,14,29,30); public static final Transform3d kTurretAoRToTurretCameraOffset = new Transform3d(0.064, -0.02, 0.038, new Rotation3d(0, Units.degreesToRadians(-30), 0)); /** Maximum time since last pose estimate for odometry to be considered valid */ public static final double kValidOdometryCutoff = 0.5; - + public static final boolean kUsePnPDistanceTrigSolve = false; } From 4e0c3532118bc9c76192bd92a42562aa67ba769d Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Wed, 29 Apr 2026 18:52:04 -0500 Subject: [PATCH 24/26] some changes ig --- CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto index a0dbc33..069352d 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto @@ -32,7 +32,7 @@ { "type": "path", "data": { - "pathName": "" + "pathName": "Right" } }, { From 6352db4e81a2bb10597a65f00ec09da26a677590 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Wed, 29 Apr 2026 23:15:25 -0500 Subject: [PATCH 25/26] Add extra loopy autons - loopy inverse (which is perfect and really shouldn't change) - stupid loopy single-side-swipe --- .../pathplanner/autos/FLIP Loopy Inverse.auto | 25 +++ .../autos/FLIP Loopy Single-side.auto | 25 +++ .../deploy/pathplanner/autos/FLIP Loopy.auto | 17 +- .../pathplanner/paths/Loopy Single-side.path | 192 ++++++++++++++++++ .../pathplanner/paths/Loopy to Inverse.path | 169 +++++++++++++++ 5 files changed, 413 insertions(+), 15 deletions(-) create mode 100644 CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Inverse.auto create mode 100644 CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Single-side.auto create mode 100644 CompBot/src/main/deploy/pathplanner/paths/Loopy Single-side.path create mode 100644 CompBot/src/main/deploy/pathplanner/paths/Loopy to Inverse.path diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Inverse.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Inverse.auto new file mode 100644 index 0000000..fba90f7 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Inverse.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Loopy to Inverse" + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Single-side.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Single-side.auto new file mode 100644 index 0000000..b42fe88 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Single-side.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Loopy Single-side" + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto index 6d1b8cc..02d734f 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto @@ -11,22 +11,9 @@ } }, { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 10.0 - } - }, - { - "type": "named", - "data": { - "name": "Aim" - } - } - ] + "name": "Aim" } } ] diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy Single-side.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy Single-side.path new file mode 100644 index 0000000..62270ec --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy Single-side.path @@ -0,0 +1,192 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.54922891566265, + "y": 4.155204819277109 + }, + "prevControl": null, + "nextControl": { + "x": 2.4236746987951805, + "y": 4.461180722891566 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3690361445783132, + "y": 3.1280000000000006 + }, + "prevControl": { + "x": 2.088097540023956, + "y": 3.359052310287696 + }, + "nextControl": { + "x": 3.5383012048192777, + "y": 2.166361445783133 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.838162072416303, + "y": 2.3540733601710326 + }, + "prevControl": { + "x": 7.031582571415575, + "y": 2.156205820141921 + }, + "nextControl": { + "x": 8.58296681905999, + "y": 2.536786512237935 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.632914392285299, + "y": 4.392238673580787 + }, + "prevControl": { + "x": 8.245827845648694, + "y": 4.413772601937025 + }, + "nextControl": { + "x": 5.44575003411572, + "y": 4.315395458060408 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.363330831513828, + "y": 3.411328295578603 + }, + "prevControl": { + "x": 6.265998851279204, + "y": 3.9566283943131904 + }, + "nextControl": { + "x": 6.661378786844978, + "y": 1.7415217317139742 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.480130492631004, + "y": 2.4469733669941776 + }, + "prevControl": { + "x": 3.974808326510189, + "y": 2.6547392706058943 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4730290456431554, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 2.008298755186713, + "rotationDegrees": 43.48173935506697 + }, + { + "waypointRelativePos": 2.85, + "rotationDegrees": 156.39894927428526 + }, + { + "waypointRelativePos": 3.485477178423249, + "rotationDegrees": 177.63685215395574 + } + ], + "constraintZones": [ + { + "name": "Preload Shooting", + "minWaypointRelativePos": 0.45, + "maxWaypointRelativePos": 1.3, + "constraints": { + "maxVelocity": 0.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Bump End ", + "minWaypointRelativePos": 4.465896869024856, + "maxWaypointRelativePos": 4.766147825047803, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Intake", + "minWaypointRelativePos": 1.854094292803968, + "maxWaypointRelativePos": 3.80414973709369, + "constraints": { + "maxVelocity": 1.75, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Aim", + "waypointRelativePos": 0.33746898263028435, + "endWaypointRelativePos": 1.2754342431761725, + "command": null + }, + { + "name": "Intake", + "waypointRelativePos": 1.6228287841191003, + "endWaypointRelativePos": 4.094292803970218, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.26912534302616 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy to Inverse.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy to Inverse.path new file mode 100644 index 0000000..cdf236e --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy to Inverse.path @@ -0,0 +1,169 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5254176332787486, + "y": 4.010216691684862 + }, + "prevControl": null, + "nextControl": { + "x": 1.3613411458333333, + "y": 2.4043879639737993 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.712915662650602, + "y": 2.4168295454545454 + }, + "prevControl": { + "x": 4.685710843373494, + "y": 2.461409638554217 + }, + "nextControl": { + "x": 6.823767668618597, + "y": 2.368619212990509 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.614337349397591, + "y": 4.668807228915662 + }, + "prevControl": { + "x": 5.428795180722891, + "y": 5.3463253012048195 + }, + "nextControl": { + "x": 9.012246699976645, + "y": 4.235455330236155 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.062373493975903, + "y": 2.4168295454545454 + }, + "prevControl": { + "x": 8.750410708249534, + "y": 2.4738619231276058 + }, + "nextControl": { + "x": 6.095385542168674, + "y": 2.2537831325301205 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7187228915662653, + "y": 2.4168295454545454 + }, + "prevControl": { + "x": 2.968434228994741, + "y": 2.4048192182449206 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.972614107883791, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4174273858921158, + "rotationDegrees": 79.22193857481086 + }, + { + "waypointRelativePos": 1.961825726141073, + "rotationDegrees": -23.228432521120364 + }, + { + "waypointRelativePos": 2.7319502074688704, + "rotationDegrees": -140.30640603093977 + }, + { + "waypointRelativePos": 3.5087136929460336, + "rotationDegrees": 133.41473277836218 + } + ], + "constraintZones": [ + { + "name": "Intake Zone", + "minWaypointRelativePos": 1.2188585607940443, + "maxWaypointRelativePos": 3.2992555831265475, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.6287643403441671, + "constraints": { + "maxVelocity": 0.75, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Aim", + "waypointRelativePos": 0.11062978011472276, + "endWaypointRelativePos": 0.6247012428298275, + "command": null + }, + { + "name": "Intake", + "waypointRelativePos": 1.0759305210918124, + "endWaypointRelativePos": 3.386600496277901, + "command": null + }, + { + "name": "Aim", + "waypointRelativePos": 3.6803970223325035, + "endWaypointRelativePos": 4.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 138.22729829188262 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 931c80c75788acf19fb26aa2ac290a43ea0ec608 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Fri, 1 May 2026 18:35:00 -0500 Subject: [PATCH 26/26] shtuff --- .../deploy/pathplanner/paths/Auto-beach.path | 4 +-- .../deploy/pathplanner/paths/Far Right.path | 4 +-- .../pathplanner/paths/Loopy Single-side.path | 10 +++--- .../pathplanner/paths/Loopy to Inverse.path | 34 +++++++++---------- .../main/deploy/pathplanner/paths/Loopy.path | 8 ++--- .../deploy/pathplanner/paths/Outpost.path | 4 +-- .../pathplanner/paths/Right Cleanup.path | 4 +-- .../deploy/pathplanner/paths/Right Depot.path | 4 +-- .../main/deploy/pathplanner/paths/Right.path | 18 ++++++---- .../main/java/frc/robot/aiming/ToFAim.java | 3 +- .../subsystems/drivetrain/Drivetrain.java | 5 +++ .../subsystems/shooter/ShooterConstants.java | 2 +- .../java/frc/robot/util/OnboardLogger.java | 25 ++++++++++++++ 13 files changed, 81 insertions(+), 44 deletions(-) diff --git a/CompBot/src/main/deploy/pathplanner/paths/Auto-beach.path b/CompBot/src/main/deploy/pathplanner/paths/Auto-beach.path index c448198..437ac87 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Auto-beach.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Auto-beach.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.7187228915662653, + "x": 2.5766626506024095, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 3.909671897289586, + "x": 3.7676116563257303, "y": 3.1098858773181166 }, "isLocked": false, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Far Right.path b/CompBot/src/main/deploy/pathplanner/paths/Far Right.path index a14d433..b4cc7b2 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Far Right.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Far Right.path @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 2.7187228915662653, + "x": 2.5766626506024095, "y": 2.4168295454545454 }, "prevControl": { - "x": 3.0123097604468962, + "x": 2.8702495194830404, "y": 2.6449383544921874 }, "nextControl": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy Single-side.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy Single-side.path index 62270ec..5bd656b 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Loopy Single-side.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy Single-side.path @@ -80,12 +80,12 @@ }, { "anchor": { - "x": 2.480130492631004, - "y": 2.4469733669941776 + "x": 2.3690361445783132, + "y": 3.729024096385543 }, "prevControl": { - "x": 3.974808326510189, - "y": 2.6547392706058943 + "x": 2.270686746987952, + "y": 2.1991445783132537 }, "nextControl": null, "isLocked": false, @@ -180,7 +180,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -179.26912534302616 + "rotation": -179.2691253430262 }, "reversed": false, "folder": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy to Inverse.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy to Inverse.path index cdf236e..d84da84 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Loopy to Inverse.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy to Inverse.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.3613411458333333, + "x": 1.3613411458333338, "y": 2.4043879639737993 }, "isLocked": false, @@ -32,44 +32,44 @@ }, { "anchor": { - "x": 7.614337349397591, - "y": 4.668807228915662 + "x": 7.406710843373492, + "y": 4.636024096385543 }, "prevControl": { - "x": 5.428795180722891, - "y": 5.3463253012048195 + "x": 5.221168674698792, + "y": 5.313542168674701 }, "nextControl": { - "x": 9.012246699976645, - "y": 4.235455330236155 + "x": 8.804620193952545, + "y": 4.202672197706036 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.062373493975903, - "y": 2.4168295454545454 + "x": 7.636192771084337, + "y": 2.2756385542168687 }, "prevControl": { - "x": 8.750410708249534, - "y": 2.4738619231276058 + "x": 8.324229985357968, + "y": 2.332670931889929 }, "nextControl": { - "x": 6.095385542168674, - "y": 2.2537831325301205 + "x": 5.669204819277108, + "y": 2.1125921412924438 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.7187228915662653, - "y": 2.4168295454545454 + "x": 2.0739879518072284, + "y": 3.6088192771084344 }, "prevControl": { - "x": 2.968434228994741, - "y": 2.4048192182449206 + "x": 3.013771084337349, + "y": 2.264710843373495 }, "nextControl": null, "isLocked": false, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path index 727b9c2..8526295 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Loopy.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path @@ -64,12 +64,12 @@ }, { "anchor": { - "x": 2.7952168674698794, - "y": 5.3463253012048195 + "x": 2.3690361445783132, + "y": 4.155204819277109 }, "prevControl": { - "x": 3.909843373493978, - "y": 5.204265060240965 + "x": 2.6968674698795176, + "y": 5.543024096385542 }, "nextControl": null, "isLocked": false, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Outpost.path b/CompBot/src/main/deploy/pathplanner/paths/Outpost.path index d66469c..870975f 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Outpost.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Outpost.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.7187228915662653, + "x": 2.5766626506024095, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 2.1518478915662653, + "x": 2.0097876506024095, "y": 1.860261363636364 }, "isLocked": false, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right Cleanup.path b/CompBot/src/main/deploy/pathplanner/paths/Right Cleanup.path index 1e6f170..384182c 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right Cleanup.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right Cleanup.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.7187228915662653, + "x": 2.5766626506024095, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 3.718722891566267, + "x": 3.5766626506024113, "y": 2.4168295454545454 }, "isLocked": false, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path index 4464f44..b32909d 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.7187228915662653, + "x": 2.5766626506024095, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 2.1067710843373493, + "x": 1.9647108433734934, "y": 3.182638554216868 }, "isLocked": false, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right.path b/CompBot/src/main/deploy/pathplanner/paths/Right.path index 3261ded..4166613 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right.path @@ -48,11 +48,11 @@ }, { "anchor": { - "x": 2.7187228915662653, + "x": 2.5766626506024095, "y": 2.4168295454545454 }, "prevControl": { - "x": 4.897062705366924, + "x": 4.755002464403075, "y": 2.4941927710843377 }, "nextControl": null, @@ -93,11 +93,11 @@ } }, { - "name": "MORE BUMPING", - "minWaypointRelativePos": 2.158808933002488, - "maxWaypointRelativePos": 2.7841191066997464, + "name": "Constraints Zone", + "minWaypointRelativePos": 2.8198511166253133, + "maxWaypointRelativePos": 3.0, "constraints": { - "maxVelocity": 3.0, + "maxVelocity": 0.1, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -113,6 +113,12 @@ "waypointRelativePos": 0.6739864864864873, "endWaypointRelativePos": 2.2652027027027057, "command": null + }, + { + "name": "Aim", + "waypointRelativePos": 2.617369727047151, + "endWaypointRelativePos": 3.0, + "command": null } ], "globalConstraints": { diff --git a/CompBot/src/main/java/frc/robot/aiming/ToFAim.java b/CompBot/src/main/java/frc/robot/aiming/ToFAim.java index 895a04b..efa0e70 100644 --- a/CompBot/src/main/java/frc/robot/aiming/ToFAim.java +++ b/CompBot/src/main/java/frc/robot/aiming/ToFAim.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.aiming.AimParams.AimStatus; import frc.robot.aiming.AimParams.SpeedControl; @@ -43,7 +44,7 @@ public ToFAim(List measurements, AimConstraints constraints) { } } - public AimParams update(Pose3d aimTarget, Pose3d shooterPose, Translation2d shooterVelocity) { + public AimParams update(Pose3d aimTarget, Pose3d shooterPose, Translation2d shooterVelocity) { Translation2d target = aimTarget.getTranslation().toTranslation2d(); Translation2d start = shooterPose.getTranslation().toTranslation2d(); diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 09752aa..811fe0f 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -38,6 +38,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.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; @@ -221,6 +222,7 @@ public Drivetrain( () -> Timer.getTimestamp() - lastOkayVisionUpdateTime); SwerveModule[] actualModules = getModules(); for (int i = 0; i < actualModules.length; i++) { + int j = i; TalonFX drive, steer; SwerveModule module = actualModules[i]; drive = module.getDriveMotor(); @@ -279,6 +281,9 @@ public Drivetrain( ologger.registerMeasurement("Steer " + steer.getDeviceID() + " Position", steer.getPosition(false)::getValue, Rotations); } + ologger.registerSwerveModuleState("Module States", () -> state.ModuleStates); + ologger.registerSwerveModuleState("Module Targets", () -> state.ModuleTargets); + ologger.registerSwerveModulePosition("Module Positions", () -> state.ModulePositions); sysIDCommands(); SmartDashboard.putData("Drivetrain/Set Home", setMemorySpot()); SmartDashboard.putData("Drivetrain/Go Home", goHome()); diff --git a/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index f3fd3f8..0a50ae8 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -138,7 +138,7 @@ public static final class HoodConstants { new AimMeasurement(Meters.of(6.84), Rotation2d.fromDegrees(53), 45.5, Seconds.of(1.265))); public static final List feedingMeasurements = List.of( - new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 29, Seconds.of(0)), + new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 29, Seconds.of(0)), new AimMeasurement(Meters.of(2.41), Rotation2d.fromDegrees(70), 32, Seconds.of(1.006)), new AimMeasurement(Meters.of(2.93), Rotation2d.fromDegrees(67), 33, Seconds.of(1.016)), new AimMeasurement(Meters.of(3.33), Rotation2d.fromDegrees(65), 35, Seconds.of(1.014)), diff --git a/CompBot/src/main/java/frc/robot/util/OnboardLogger.java b/CompBot/src/main/java/frc/robot/util/OnboardLogger.java index f7a2b11..38edada 100644 --- a/CompBot/src/main/java/frc/robot/util/OnboardLogger.java +++ b/CompBot/src/main/java/frc/robot/util/OnboardLogger.java @@ -5,10 +5,15 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import java.util.function.Supplier; + +import com.ctre.phoenix6.swerve.SwerveModule; + import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Unit; import edu.wpi.first.util.datalog.BooleanLogEntry; @@ -38,6 +43,8 @@ public class OnboardLogger { private final List, StructLogEntry>> pose3dEntries; private final List, StructArrayLogEntry>> pose3dArrayEntries; private final List, StructLogEntry>> transform2dEntries; + private final List, StructArrayLogEntry>> swerveModuleStateEntries; + private final List, StructArrayLogEntry>> swerveModulePositionEntries; public OnboardLogger(String name) { this.name = name; @@ -49,6 +56,8 @@ public OnboardLogger(String name) { pose3dEntries = new ArrayList<>(); pose3dArrayEntries = new ArrayList<>(); transform2dEntries = new ArrayList<>(); + swerveModuleStateEntries = new ArrayList<>(); + swerveModulePositionEntries = new ArrayList<>(); loggers.add(this); } @@ -105,6 +114,16 @@ public void registerPoses3d(String name, Supplier supplier) { pose3dArrayEntries.add(new Pair<>(supplier, entry)); } + public void registerSwerveModuleState(String name, Supplier supplier) { + StructArrayLogEntry entry = StructArrayLogEntry.create(datalog, this.name + "/" + name, SwerveModuleState.struct); + swerveModuleStateEntries.add(new Pair<>(supplier, entry)); + } + + public void registerSwerveModulePosition(String name, Supplier supplier) { + StructArrayLogEntry entry = StructArrayLogEntry.create(datalog, this.name + "/" + name, SwerveModulePosition.struct); + swerveModulePositionEntries.add(new Pair<>(supplier, entry)); + } + public void registerTransform2d(String name, Supplier supplier) { StructLogEntry entry = StructLogEntry.create(datalog, this.name + "/" + name, Transform2d.struct); @@ -136,6 +155,12 @@ private void log() { for (Pair, StructLogEntry> pair : transform2dEntries) { pair.getSecond().update(pair.getFirst().get()); } + for (Pair, StructArrayLogEntry> pair : swerveModuleStateEntries) { + pair.getSecond().update(pair.getFirst().get()); + } + for (Pair, StructArrayLogEntry> pair : swerveModulePositionEntries) { + pair.getSecond().update(pair.getFirst().get()); + } } public static void logAll() {