From e8404759c4ae1ce181ed20f995b74a6084f20167 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Mon, 27 Apr 2026 22:14:40 -0400 Subject: [PATCH 01/97] FEAT: added tall robot corner bites to fit with them --- .../autos/Left Shallow 2 Cycle.auto | 37 ++++++++ .../autos/Right Shallow 2 Cycle.auto | 37 ++++++++ .../paths/Left Bite Score To Score.path | 28 ++++-- .../paths/Left Corner Bite To Score.path | 12 +-- .../pathplanner/paths/Left Corner Bite.path | 8 +- .../paths/Left Follow To Score.path | 2 +- .../pathplanner/paths/Left NZ To Score.path | 4 +- .../paths/Left Score To Corner.path | 12 +-- .../paths/Left Score To NZ (F).path | 8 +- .../paths/Left Score To Score.path | 8 +- .../paths/Left Shallow To Score.path | 59 ++++++++++++ .../pathplanner/paths/Left To Shallow.path | 81 +++++++++++++++++ .../paths/Right Bite Score To Score.path | 20 ++++- .../pathplanner/paths/Right Corner Bite.path | 10 +-- .../paths/Right Follow To Score.path | 4 +- .../paths/Right Score To Corner.path | 10 +-- .../paths/Right Score To NZ (F).path | 6 +- .../paths/Right Shallow To Score.path | 59 ++++++++++++ .../pathplanner/paths/Right To Shallow.path | 81 +++++++++++++++++ src/main/deploy/pathplanner/settings.json | 1 + .../com/stuypulse/robot/RobotContainer.java | 10 +++ .../auton/regular/LeftTwoCornerShallow.java | 89 +++++++++++++++++++ .../auton/regular/RightTwoCornerShallow.java | 89 +++++++++++++++++++ 23 files changed, 623 insertions(+), 52 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto create mode 100644 src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto create mode 100644 src/main/deploy/pathplanner/paths/Left Shallow To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Left To Shallow.path create mode 100644 src/main/deploy/pathplanner/paths/Right Shallow To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Right To Shallow.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java diff --git a/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto new file mode 100644 index 00000000..d15def7a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left To Shallow" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Shallow To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Bite Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Trench Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto new file mode 100644 index 00000000..50836609 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right To Shallow" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Shallow To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Bite Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Trench Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path index 0a057cd8..d5e090bd 100644 --- a/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.63796005706134, + "x": 3.6506870229007635, "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 7.894778887303849, + "x": 7.907505853143271, "y": 7.509029957203994 }, "isLocked": false, @@ -64,12 +64,12 @@ }, { "anchor": { - "x": 3.63796005706134, + "x": 3.6506870229007635, "y": 7.440906488549619 }, "prevControl": { - "x": 6.0057346647646215, - "y": 7.664293865905849 + "x": 6.341276584160035, + "y": 7.591536259541984 }, "nextControl": null, "isLocked": false, @@ -90,7 +90,7 @@ "rotationDegrees": -90.0 }, { - "waypointRelativePos": 1.9104477611940158, + "waypointRelativePos": 1.7421203438395327, "rotationDegrees": 180.0 }, { @@ -106,7 +106,21 @@ "rotationDegrees": 0.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8835202761000936, + "maxWaypointRelativePos": 3.057808455565133, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path b/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path index 5e2c9e3f..25d25e6b 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.231184022824536, - "y": 5.257703281027104 + "x": 8.295104961832061, + "y": 4.65425572519084 }, "prevControl": null, "nextControl": { - "x": 7.403109843081312, - "y": 4.339058487874465 + "x": 7.4670307820888375, + "y": 3.7356109320382007 }, "isLocked": false, "linkedName": "Left NZ Corner" @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 3.63796005706134, + "x": 3.6506870229007635, "y": 7.440906488549619 }, "prevControl": { - "x": 6.743238231098433, + "x": 6.755965196937856, "y": 7.6125392296718974 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite.path b/src/main/deploy/pathplanner/paths/Left Corner Bite.path index 9ec5ab2a..cc3f32a8 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.231184022824536, - "y": 5.257703281027104 + "x": 8.295104961832061, + "y": 4.65425572519084 }, "prevControl": { - "x": 6.782054208273893, - "y": 7.276134094151212 + "x": 7.039856870229008, + "y": 7.189856870229007 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Follow To Score.path b/src/main/deploy/pathplanner/paths/Left Follow To Score.path index 5d36788c..36f13005 100644 --- a/src/main/deploy/pathplanner/paths/Left Follow To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Follow To Score.path @@ -48,7 +48,7 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.8317617866005099, + "minWaypointRelativePos": 0.8662640207075034, "maxWaypointRelativePos": 2.0, "constraints": { "maxVelocity": 1.75, diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 88a55f71..c2a3c711 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 3.63796005706134, + "x": 3.6506870229007635, "y": 7.440906488549619 }, "prevControl": { - "x": 6.01867332382311, + "x": 6.031400289662534, "y": 7.444336661911555 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Corner.path b/src/main/deploy/pathplanner/paths/Left Score To Corner.path index 0be2c04f..98999296 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Corner.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.63796005706134, + "x": 3.6506870229007635, "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 3.2245896483318734, - "y": 7.343909351042221 + "x": 3.2366011042814318, + "y": 7.349786421407005 }, "isLocked": false, "linkedName": "Left Trench Score" }, { "anchor": { - "x": 3.2808444444444445, + "x": 3.3075858778625955, "y": 7.440906488549619 }, "prevControl": { - "x": 3.524888980928413, - "y": 7.49514913056054 + "x": 3.553822420306369, + "y": 7.484121823389645 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index 4bd6d40f..b8c3689a 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.63796005706134, + "x": 3.3075858778625955, "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 6.846747503566332, - "y": 7.440906488549619 + "x": 6.889227099236641, + "y": 7.524589694656489 }, "isLocked": false, - "linkedName": "Left Trench Score" + "linkedName": "Left Corner" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index 7e3d5fa7..d65e01e1 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.63796005706134, + "x": 3.6506870229007635, "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 6.717360912981455, + "x": 6.7300878788208784, "y": 7.521968616262482 }, "isLocked": false, @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 3.63796005706134, + "x": 3.6506870229007635, "y": 7.440906488549619 }, "prevControl": { - "x": 5.29410841654779, + "x": 5.306835382387213, "y": 7.399604078672524 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Left Shallow To Score.path b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path new file mode 100644 index 00000000..2ed2e437 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.24489503816794, + "y": 5.382299618320611 + }, + "prevControl": null, + "nextControl": { + "x": 6.713492366412213, + "y": 7.675219465648856 + }, + "isLocked": false, + "linkedName": "Left Shallow" + }, + { + "anchor": { + "x": 3.6506870229007635, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.09859528645011, + "y": 7.650114503816795 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6174785100286533, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Non-Collision", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left To Shallow.path b/src/main/deploy/pathplanner/paths/Left To Shallow.path new file mode 100644 index 00000000..543a18c6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 6.639728958630526, + "y": 7.677232524964337 + }, + "isLocked": false, + "linkedName": "Left Trench Start" + }, + { + "anchor": { + "x": 8.24489503816794, + "y": 5.382299618320611 + }, + "prevControl": { + "x": 7.466641221374045, + "y": 7.030858778625954 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Shallow" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.9253731343283487, + "rotationDegrees": -55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6867989646246767, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Non-Collision", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path index 2d259744..81d66657 100644 --- a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path @@ -57,7 +57,7 @@ }, "nextControl": { "x": 6.070427960057061, - "y": 0.15987161198288136 + "y": 0.15987161198288113 }, "isLocked": false, "linkedName": null @@ -106,7 +106,21 @@ "rotationDegrees": 0.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0353753235547816, + "maxWaypointRelativePos": 3.1406384814495194, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -114,7 +128,7 @@ "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite.path b/src/main/deploy/pathplanner/paths/Right Corner Bite.path index b8e70915..4ff41797 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.0667047075606275, - "y": 0.4315834522111266 + "x": 7.349484732824428, + "y": 0.5705152671755729 }, "isLocked": false, "linkedName": "Right Trench Start" @@ -20,8 +20,8 @@ "y": 3.4271055753262156 }, "prevControl": { - "x": 7.687760342368046, - "y": 2.1265477888730384 + "x": 7.617270992366413, + "y": 2.0684446564885492 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.2665245202558647, + "waypointRelativePos": 0.17621776504297812, "rotationDegrees": 90.0 }, { diff --git a/src/main/deploy/pathplanner/paths/Right Follow To Score.path b/src/main/deploy/pathplanner/paths/Right Follow To Score.path index 90adc8cf..08cf189b 100644 --- a/src/main/deploy/pathplanner/paths/Right Follow To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Score.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.7509666666666668, + "x": 1.1067175572519086, "y": 0.6256633380884444 }, "prevControl": { - "x": 3.7688065844293814, + "x": 3.1245574750146234, "y": 0.6251768362147326 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Corner.path b/src/main/deploy/pathplanner/paths/Right Score To Corner.path index 7697becc..385b6020 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Corner.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 3.36543714002239, - "y": 0.5460313281265844 + "x": 3.2000538693416494, + "y": 0.6346833588090371 }, "isLocked": false, "linkedName": "Right Trench Score" }, { "anchor": { - "x": 3.2711, + "x": 3.2824809160305346, "y": 0.5868473609129818 }, "prevControl": { - "x": 3.717247367182559, - "y": 0.5630880724749401 + "x": 3.516912705949025, + "y": 0.5000041926404413 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 71ca80d6..c0cb71bc 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, + "x": 3.2824809160305346, "y": 0.5868473609129818 }, "prevControl": null, "nextControl": { - "x": 7.183152639087018, + "x": 6.853550816173187, "y": 0.5739087018544944 }, "isLocked": false, - "linkedName": "Right Trench Score" + "linkedName": "Right Corner" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path new file mode 100644 index 00000000..9f5c9462 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.21142175572519, + "y": 2.679332061068702 + }, + "prevControl": null, + "nextControl": { + "x": 6.512652671755725, + "y": 0.4868320610687009 + }, + "isLocked": false, + "linkedName": "Right Shallow NZ" + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 6.044026717557253, + "y": 0.5370419847328243 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6647564469914042, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Non-Collision", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right To Shallow.path b/src/main/deploy/pathplanner/paths/Right To Shallow.path new file mode 100644 index 00000000..0aa62f2c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 6.939437022900763, + "y": 0.36130725190839597 + }, + "isLocked": false, + "linkedName": "Right Trench Start" + }, + { + "anchor": { + "x": 8.21142175572519, + "y": 2.679332061068702 + }, + "prevControl": { + "x": 8.24489503816794, + "y": 1.4491889312977104 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Shallow NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.20916905444126394, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.5200573065902583, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.7736389684813755, + "rotationDegrees": 55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6143226919758464, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Non-Collision", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 551c8ead..5871e05a 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -5,6 +5,7 @@ "pathFolders": [ "Bump Stuff", "Follow", + "Non-Collision", "To Depot", "To NZ", "To Score" diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index f3c29829..c8199414 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -11,10 +11,12 @@ import com.stuypulse.robot.commands.auton.regular.LeftBump; import com.stuypulse.robot.commands.auton.regular.LeftFollow; import com.stuypulse.robot.commands.auton.regular.LeftTwoCorner; +import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerShallow; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; import com.stuypulse.robot.commands.auton.regular.RightBump; import com.stuypulse.robot.commands.auton.regular.RightFollow; import com.stuypulse.robot.commands.auton.regular.RightTwoCorner; +import com.stuypulse.robot.commands.auton.regular.RightTwoCornerShallow; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.auton.test.BoxTest; import com.stuypulse.robot.commands.auton.test.EmptyTest; @@ -409,6 +411,14 @@ public void configureAutons() { "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CORNER.register(autonChooser); + AutonConfig LEFT_TWO_CORNER_SHALLOW = new AutonConfig("Left Two Corner Shallow", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + "Left To Shallow", "Left Shallow To Score", "Left Bite Score To Score", "Left Trench Score To Corner", "Left Score To NZ (F)"); + LEFT_TWO_CORNER_SHALLOW.register(autonChooser); + + AutonConfig RIGHT_TWO_CORNER_SHALLOW = new AutonConfig("Right Two Corner Shallow", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right To Shallow", "Right Shallow To Score", "Right Bite Score To Score", "Right Trench Score To Corner", "Right Score To NZ (F)"); + RIGHT_TWO_CORNER_SHALLOW.register(autonChooser); + // FOLLOWS AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, "Left Follow To Bump", "Left Follow To Score", "Left Corner To Depot"); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java new file mode 100644 index 00000000..33d221a9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java @@ -0,0 +1,89 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeDigest; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class LeftTwoCornerShallow extends SequentialCommandGroup { + + public LeftTwoCornerShallow(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) + ), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java new file mode 100644 index 00000000..15b1bcd4 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java @@ -0,0 +1,89 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeDigest; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class RightTwoCornerShallow extends SequentialCommandGroup { + + public RightTwoCornerShallow(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) + ), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]) + + ); + + } + +} From 4ec8b7e78430f38da701d8fd69755c9c2042430a Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Mon, 27 Apr 2026 22:14:50 -0400 Subject: [PATCH 02/97] FEAT: ditto --- .../pathplanner/autos/Left Corner Bite.auto | 2 +- .../autos/Left Shallow 2 Cycle.auto | 2 +- .../pathplanner/autos/Left Two Cycle.auto | 2 +- .../pathplanner/autos/Right Corner Bite.auto | 2 +- .../autos/Right Shallow 2 Cycle.auto | 2 +- .../pathplanner/autos/Right Two Cycle.auto | 2 +- .../pathplanner/paths/Left Score Jiggle.path | 134 ------------------ .../pathplanner/paths/Right Score Jiggle.path | 134 ------------------ .../com/stuypulse/robot/RobotContainer.java | 18 +-- 9 files changed, 15 insertions(+), 283 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/Left Score Jiggle.path delete mode 100644 src/main/deploy/pathplanner/paths/Right Score Jiggle.path diff --git a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto index 55916030..ce84e81a 100644 --- a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Left Score Jiggle" + "pathName": "Left Score To Corner" } } ] diff --git a/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto index d15def7a..89cb8412 100644 --- a/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Left Trench Score To Corner" + "pathName": "Left Score To Corner" } } ] diff --git a/src/main/deploy/pathplanner/autos/Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto index 513b4a1c..3a598107 100644 --- a/src/main/deploy/pathplanner/autos/Left Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Left Score Jiggle" + "pathName": "Left Score To Corner" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto index 9000ca89..2690d7aa 100644 --- a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Right Score Jiggle" + "pathName": "Right Score To Corner" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto index 50836609..6f06e921 100644 --- a/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Right Trench Score To Corner" + "pathName": "Right Score To Corner" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right Two Cycle.auto b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto index 78af2d36..c64320b2 100644 --- a/src/main/deploy/pathplanner/autos/Right Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Right Score Jiggle" + "pathName": "Right Score To Corner" } } ] diff --git a/src/main/deploy/pathplanner/paths/Left Score Jiggle.path b/src/main/deploy/pathplanner/paths/Left Score Jiggle.path deleted file mode 100644 index 75ea4914..00000000 --- a/src/main/deploy/pathplanner/paths/Left Score Jiggle.path +++ /dev/null @@ -1,134 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.63796005706134, - "y": 7.440906488549619 - }, - "prevControl": null, - "nextControl": { - "x": 3.3895155587886268, - "y": 7.413061717442285 - }, - "isLocked": false, - "linkedName": "Left Trench Score" - }, - { - "anchor": { - "x": 3.648639663594734, - "y": 7.442254096935589 - }, - "prevControl": { - "x": 3.400211195655958, - "y": 7.4142666655137415 - }, - "nextControl": { - "x": 3.89706813153351, - "y": 7.470241528357436 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.648639663594734, - "y": 7.458193687201929 - }, - "prevControl": { - "x": 3.8970801117553107, - "y": 7.486074571652958 - }, - "nextControl": { - "x": 3.400199215434157, - "y": 7.430312802750899 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.648639663594734, - "y": 7.458193687201929 - }, - "prevControl": { - "x": 3.894935520576916, - "y": 7.4153176935293486 - }, - "nextControl": { - "x": 3.4023438066125493, - "y": 7.501069680874509 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.63796005706134, - "y": 7.440906488549619 - }, - "prevControl": { - "x": 3.87561279347943, - "y": 7.5185027311970165 - }, - "nextControl": { - "x": 3.400307320643253, - "y": 7.3633102459022215 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.63796005706134, - "y": 7.440906488549619 - }, - "prevControl": { - "x": 3.391228490406765, - "y": 7.40061338720238 - }, - "nextControl": { - "x": 3.884691623715916, - "y": 7.481199589896857 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.2808444444444445, - "y": 7.440906488549619 - }, - "prevControl": { - "x": 3.530395607526691, - "y": 7.455880365946537 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Left Corner" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 1.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score Jiggle.path b/src/main/deploy/pathplanner/paths/Right Score Jiggle.path deleted file mode 100644 index 29c167c7..00000000 --- a/src/main/deploy/pathplanner/paths/Right Score Jiggle.path +++ /dev/null @@ -1,134 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 - }, - "prevControl": null, - "nextControl": { - "x": 3.3131806164768474, - "y": 0.5777032745480257 - }, - "isLocked": false, - "linkedName": "Right Trench Score" - }, - { - "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 - }, - "prevControl": { - "x": 3.858050216469342, - "y": 0.6315687204629786 - }, - "nextControl": { - "x": 3.3651621367923017, - "y": 0.541952705976242 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.470799257649026, - "y": 0.5880453279537765 - }, - "prevControl": { - "x": 3.220842004837689, - "y": 0.5834223671050037 - }, - "nextControl": { - "x": 3.720756510460363, - "y": 0.5926682888025493 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.330297914985203, - "y": 0.585259293085392 - }, - "prevControl": { - "x": 3.0833481036127086, - "y": 0.54632613673904 - }, - "nextControl": { - "x": 3.5772477263576974, - "y": 0.6241924494317441 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.424220644336901, - "y": 0.5868906824364817 - }, - "prevControl": { - "x": 3.17424461599113, - "y": 0.5834287100977454 - }, - "nextControl": { - "x": 3.6741966726826725, - "y": 0.5903526547752179 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.5181433736885994, - "y": 0.5885220717875714 - }, - "prevControl": { - "x": 3.768126174889212, - "y": 0.5914544946586795 - }, - "nextControl": { - "x": 3.2681605724879867, - "y": 0.5855896489164634 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.2711, - "y": 0.5868473609129818 - }, - "prevControl": { - "x": 3.021101790448806, - "y": 0.5877935222084847 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Right Corner" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 1.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c8199414..14efea5d 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -412,11 +412,11 @@ public void configureAutons() { RIGHT_TWO_CORNER.register(autonChooser); AutonConfig LEFT_TWO_CORNER_SHALLOW = new AutonConfig("Left Two Corner Shallow", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left To Shallow", "Left Shallow To Score", "Left Bite Score To Score", "Left Trench Score To Corner", "Left Score To NZ (F)"); + "Left To Shallow", "Left Shallow To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); LEFT_TWO_CORNER_SHALLOW.register(autonChooser); AutonConfig RIGHT_TWO_CORNER_SHALLOW = new AutonConfig("Right Two Corner Shallow", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right To Shallow", "Right Shallow To Score", "Right Bite Score To Score", "Right Trench Score To Corner", "Right Score To NZ (F)"); + "Right To Shallow", "Right Shallow To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CORNER_SHALLOW.register(autonChooser); // FOLLOWS @@ -428,9 +428,9 @@ public void configureAutons() { "Right Follow To Bump", "Right Follow To Score"); RIGHT_FOLLOW.register(autonChooser); - AutonConfig EMPTY_TEST = new AutonConfig("Empty Test", EmptyTest::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Trench Score To Corner"); - EMPTY_TEST.register(autonChooser); + // AutonConfig EMPTY_TEST = new AutonConfig("Empty Test", EmptyTest::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Trench Score To Corner"); + // EMPTY_TEST.register(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); @@ -450,10 +450,10 @@ public boolean hasWaitTimeTwoChanged() { } public void configureSysids() { - autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); - autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); - autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); - autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); + // autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); + // autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); // autonChooser.addOption("SysID Rotation Translation Dynamic Forwards", swerve.sysidRotationDynamic(Direction.kForward)); // autonChooser.addOption("SysID Rotation Translation Dynamic Backwards", swerve.sysidRotationDynamic(Direction.kReverse)); From 8285f4ed347e5e103a446d5539ec15468319a8b5 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Wed, 29 Apr 2026 15:35:41 -0500 Subject: [PATCH 03/97] feat: stop ferrying when behind opponent's hub (rectangle restriction zone) --- simgui.json | 3 ++- .../com/stuypulse/robot/constants/Field.java | 2 ++ .../superstructure/Superstructure.java | 3 +++ .../swerve/CommandSwerveDrivetrain.java | 17 +++++++++++++++++ 4 files changed, 24 insertions(+), 1 deletion(-) diff --git a/simgui.json b/simgui.json index 92d7c18b..c86a9772 100644 --- a/simgui.json +++ b/simgui.json @@ -108,7 +108,8 @@ "Robot": { "Auton": { "open": true - } + }, + "open": true }, "Spindexer": { "open": true diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 5a1c76b9..907b02e9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -43,6 +43,8 @@ public interface Field { public static final double OPPONENT_ZONE_X = LENGTH - Units.inchesToMeters(158.6); + public static final double OPPONENT_HUB_DS_X = LENGTH - HUB_FAR_LEFT_CORNER.getX() + 2.0 * HUB_RADIUS; + public static final double BEHIND_HUB_TOLERANCE_X = Units.inchesToMeters(144); // To extend the triangle vertex public static final double BEHIND_HUB_TOLERANCE_Y = Units.inchesToMeters(12) + Units.inchesToMeters(2); // To extend base of triangle (colinear with back hub) diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index c2c07c70..9f3cd1d8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -189,10 +189,12 @@ public boolean shouldStop() { getState() == SuperstructureState.RIGHT_CORNER && getState() == SuperstructureState.KB; boolean isBehindTower = swerve.isBehindTower() && getState() == SuperstructureState.SOTM; + boolean isBtwnOppHubAndWall = swerve.isBtwnOppHubAndWall() && getState() == SuperstructureState.FOTM; boolean turretLaggingSOTM = !isTurretAtTolerance() && getState() == SuperstructureState.SOTM; DogLog.log("Spindexer/Should Stop/Is Behind Hub While Ferrying?", isBehindHubWhileFerrying); + DogLog.log("Spindexer/Should Stop/Is behind Opponent's Hub While Ferrying?", isBtwnOppHubAndWall); DogLog.log("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); DogLog.log("Spindexer/Should Stop/Is Outside Alliance Zone?", isOutsideAllianceZone); DogLog.log("Spindexer/Should Stop/Is Under Trench?", isUnderTrench); @@ -203,6 +205,7 @@ public boolean shouldStop() { isHandOffStopState || isTurretWrapping || (isBehindHubWhileFerrying && !inManualState) || + isBtwnOppHubAndWall || turretLaggingSOTM || (isOutsideAllianceZone && !inManualState) || (isUnderTrench && !inManualState) || diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 1368e99a..b3abc799 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -86,6 +86,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private Optional isInOpponentZone = Optional.empty(); private Optional isUnderTrench = Optional.empty(); private Optional isBehindTower = Optional.empty(); + private Optional isBtwnOppHubAndWall = Optional.empty(); // private StructPublisher robotPose = NetworkTableInstance.getDefault() // .getStructTopic("Robot Pose", Pose2d.struct).publish(); @@ -650,12 +651,28 @@ public boolean isOutsideAllianceZone() { return isOutsideAllianceZone.get(); } + public boolean isBtwnOppHubAndWall() { + if (!isBtwnOppHubAndWall.isEmpty()) { + return isBtwnOppHubAndWall.get(); + } + + Translation2d turretTranslation = getTurretPose().getTranslation(); + + boolean btwnOppHubAndWallX = turretTranslation.getX() < Field.LENGTH && turretTranslation.getX() > Field.OPPONENT_HUB_DS_X; + boolean btwnOppHubAndWallY = turretTranslation.getY() < Field.HUB_FAR_LEFT_CORNER.getY() && turretTranslation.getY() > Field.HUB_FAR_RIGHT_CORNER.getY(); + + isBtwnOppHubAndWall = Optional.of(btwnOppHubAndWallX && btwnOppHubAndWallY); + + return isBtwnOppHubAndWall.get(); + } + public void clearMemoized() { isBehindHub = Optional.empty(); isOutsideAllianceZone = Optional.empty(); isInOpponentZone = Optional.empty(); isUnderTrench = Optional.empty(); isBehindTower = Optional.empty(); + isBtwnOppHubAndWall = Optional.empty(); } public void teleopInit() { From 51d696e563a5f9460713cbc9b43c0e91a65d18a6 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 29 Apr 2026 16:38:52 -0400 Subject: [PATCH 04/97] feat: superstruct state to STOW when moving out of your own alliance zone into neutral zone. Change Ferrying interpolation table to end at the start of opposite alliance zone --- .../deploy/pathplanner/paths/Left Corner Bite.path | 4 ++-- .../deploy/pathplanner/paths/Right Corner Bite.path | 10 +++++----- .../java/com/stuypulse/robot/constants/Settings.java | 8 ++++---- .../subsystems/superstructure/Superstructure.java | 2 +- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite.path b/src/main/deploy/pathplanner/paths/Left Corner Bite.path index cc3f32a8..771483ab 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite.path @@ -20,8 +20,8 @@ "y": 4.65425572519084 }, "prevControl": { - "x": 7.039856870229008, - "y": 7.189856870229007 + "x": 7.584251069900143, + "y": 7.3149500713266775 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite.path b/src/main/deploy/pathplanner/paths/Right Corner Bite.path index 4ff41797..8dcbf94d 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.349484732824428, - "y": 0.5705152671755729 + "x": 7.584251069900143, + "y": 0.48333808844507875 }, "isLocked": false, "linkedName": "Right Trench Start" @@ -20,8 +20,8 @@ "y": 3.4271055753262156 }, "prevControl": { - "x": 7.617270992366413, - "y": 2.0684446564885492 + "x": 7.998288159771754, + "y": 1.906590584878745 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "rotationDegrees": 90.0 }, { - "waypointRelativePos": 0.5, + "waypointRelativePos": 0.45842217484008524, "rotationDegrees": 55.0 }, { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 7a504b27..541bc550 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -167,10 +167,10 @@ public interface FerryRPMInterpolation { {7.87, 3800.0}, {9.77, 4300.0}, {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS EXTRAPOLATED!!! - {11.516, 4900.0}, - {12.416, 5200.0}, - {13.316, 5500.0}, - {14.216, 5600.0} + {11.516, 5200.0}, + {12.416, 5500.0} // AFTER OPP ALLIANCE ZONE, RPM SHOULD BE AT 5500 -blay + // {13.316, 5500.0}, + // {14.216, 5600.0} }; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index c2c07c70..ccebb4d8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -241,7 +241,7 @@ else if (getState() == SuperstructureState.FOTM && shouldStop() && DriverStation if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && state == SuperstructureState.SOTM && Robot.getMode() != RobotMode.AUTON) { // allows us to start SOTM earlier in auto, but currently not desired in teleop - setState(SuperstructureState.FOTM); + setState(SuperstructureState.STOW); Spindexer.getInstance().setState(SpindexerState.STOP); Handoff.getInstance().setState(HandoffState.STOP); } From d34d211a002a132bef2584ddbf733d8f6801b413 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 29 Apr 2026 17:56:52 -0400 Subject: [PATCH 05/97] Revert "feat: stop ferrying when behind opponent's hub (rectangle restriction zone)" This reverts commit 8285f4ed347e5e103a446d5539ec15468319a8b5. --- simgui.json | 3 +-- .../com/stuypulse/robot/constants/Field.java | 2 -- .../superstructure/Superstructure.java | 3 --- .../swerve/CommandSwerveDrivetrain.java | 17 ----------------- 4 files changed, 1 insertion(+), 24 deletions(-) diff --git a/simgui.json b/simgui.json index c86a9772..92d7c18b 100644 --- a/simgui.json +++ b/simgui.json @@ -108,8 +108,7 @@ "Robot": { "Auton": { "open": true - }, - "open": true + } }, "Spindexer": { "open": true diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 907b02e9..5a1c76b9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -43,8 +43,6 @@ public interface Field { public static final double OPPONENT_ZONE_X = LENGTH - Units.inchesToMeters(158.6); - public static final double OPPONENT_HUB_DS_X = LENGTH - HUB_FAR_LEFT_CORNER.getX() + 2.0 * HUB_RADIUS; - public static final double BEHIND_HUB_TOLERANCE_X = Units.inchesToMeters(144); // To extend the triangle vertex public static final double BEHIND_HUB_TOLERANCE_Y = Units.inchesToMeters(12) + Units.inchesToMeters(2); // To extend base of triangle (colinear with back hub) diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index fafadd29..ccebb4d8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -189,12 +189,10 @@ public boolean shouldStop() { getState() == SuperstructureState.RIGHT_CORNER && getState() == SuperstructureState.KB; boolean isBehindTower = swerve.isBehindTower() && getState() == SuperstructureState.SOTM; - boolean isBtwnOppHubAndWall = swerve.isBtwnOppHubAndWall() && getState() == SuperstructureState.FOTM; boolean turretLaggingSOTM = !isTurretAtTolerance() && getState() == SuperstructureState.SOTM; DogLog.log("Spindexer/Should Stop/Is Behind Hub While Ferrying?", isBehindHubWhileFerrying); - DogLog.log("Spindexer/Should Stop/Is behind Opponent's Hub While Ferrying?", isBtwnOppHubAndWall); DogLog.log("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); DogLog.log("Spindexer/Should Stop/Is Outside Alliance Zone?", isOutsideAllianceZone); DogLog.log("Spindexer/Should Stop/Is Under Trench?", isUnderTrench); @@ -205,7 +203,6 @@ public boolean shouldStop() { isHandOffStopState || isTurretWrapping || (isBehindHubWhileFerrying && !inManualState) || - isBtwnOppHubAndWall || turretLaggingSOTM || (isOutsideAllianceZone && !inManualState) || (isUnderTrench && !inManualState) || diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index b3abc799..1368e99a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -86,7 +86,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private Optional isInOpponentZone = Optional.empty(); private Optional isUnderTrench = Optional.empty(); private Optional isBehindTower = Optional.empty(); - private Optional isBtwnOppHubAndWall = Optional.empty(); // private StructPublisher robotPose = NetworkTableInstance.getDefault() // .getStructTopic("Robot Pose", Pose2d.struct).publish(); @@ -651,28 +650,12 @@ public boolean isOutsideAllianceZone() { return isOutsideAllianceZone.get(); } - public boolean isBtwnOppHubAndWall() { - if (!isBtwnOppHubAndWall.isEmpty()) { - return isBtwnOppHubAndWall.get(); - } - - Translation2d turretTranslation = getTurretPose().getTranslation(); - - boolean btwnOppHubAndWallX = turretTranslation.getX() < Field.LENGTH && turretTranslation.getX() > Field.OPPONENT_HUB_DS_X; - boolean btwnOppHubAndWallY = turretTranslation.getY() < Field.HUB_FAR_LEFT_CORNER.getY() && turretTranslation.getY() > Field.HUB_FAR_RIGHT_CORNER.getY(); - - isBtwnOppHubAndWall = Optional.of(btwnOppHubAndWallX && btwnOppHubAndWallY); - - return isBtwnOppHubAndWall.get(); - } - public void clearMemoized() { isBehindHub = Optional.empty(); isOutsideAllianceZone = Optional.empty(); isInOpponentZone = Optional.empty(); isUnderTrench = Optional.empty(); isBehindTower = Optional.empty(); - isBtwnOppHubAndWall = Optional.empty(); } public void teleopInit() { From 9ee3d8ac63d5ab649e21e57306ea5dbfa97f1870 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 29 Apr 2026 19:08:48 -0400 Subject: [PATCH 06/97] feat: max ferry rpm 4900, ferry angle 44.75, isTurretLaggingFOTM instead of isTurretWrapping --- .../pathplanner/paths/Left Shallow To Score.path | 6 +++--- .../pathplanner/paths/Right Shallow To Score.path | 10 +++++----- .../com/stuypulse/robot/constants/Settings.java | 7 ++++--- .../subsystems/superstructure/Superstructure.java | 15 ++++++++++++--- 4 files changed, 24 insertions(+), 14 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left Shallow To Score.path b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path index 2ed2e437..0cc25861 100644 --- a/src/main/deploy/pathplanner/paths/Left Shallow To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.713492366412213, - "y": 7.675219465648856 + "x": 6.911440798858774, + "y": 7.819557774607704 }, "isLocked": false, "linkedName": "Left Shallow" @@ -30,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.6174785100286533, + "waypointRelativePos": 0.3816631130063977, "rotationDegrees": 0.0 } ], diff --git a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path index 9f5c9462..bc133dac 100644 --- a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.512652671755725, - "y": 0.4868320610687009 + "x": 6.549158345221112, + "y": 0.27631954350927357 }, "isLocked": false, "linkedName": "Right Shallow NZ" @@ -20,8 +20,8 @@ "y": 0.5868473609129818 }, "prevControl": { - "x": 6.044026717557253, - "y": 0.5370419847328243 + "x": 6.018673323823109, + "y": 0.392767475035663 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.6647564469914042, + "waypointRelativePos": 0.37526652452025683, "rotationDegrees": 0.0 } ], diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 541bc550..9b6b6a37 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -167,8 +167,9 @@ public interface FerryRPMInterpolation { {7.87, 3800.0}, {9.77, 4300.0}, {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS EXTRAPOLATED!!! - {11.516, 5200.0}, - {12.416, 5500.0} // AFTER OPP ALLIANCE ZONE, RPM SHOULD BE AT 5500 -blay + {11.516, 4900.0} + // {11.516, 5200.0}, + // {12.416, 5500.0}, // AFTER OPP ALLIANCE ZONE, RPM SHOULD BE AT 5500 -blay // {13.316, 5500.0}, // {14.216, 5600.0} }; @@ -238,9 +239,9 @@ public interface Hood { public interface Angles { public final SmartNumber MANUAL_OVERRIDE = new SmartNumber("InterpolationTesting/Shoot State Target Angle (deg)", 20.0); - public final Rotation2d FERRY_ANGLE = Rotation2d.fromDegrees(44.0); public final Rotation2d MAX = FORWARD_SOFT_LIMIT; public final Rotation2d MIN = REVERSE_SOFT_LIMIT; + public final Rotation2d FERRY_ANGLE = MAX;//Rotation2d.fromDegrees(44.0); public final Rotation2d STOW = Rotation2d.fromDegrees(21.0); public final Rotation2d KB = Rotation2d.fromDegrees(20.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index ccebb4d8..a8b66448 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -9,6 +9,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.spindexer.Spindexer; @@ -143,6 +144,11 @@ public boolean isTurretAtTolerance(){ return turret.atTolerance(); } + public boolean isTurretLaggingFOTM() { + double error = Math.abs(turret.getAngle().minus(turret.getTargetAngle()).getRotations()); + return error >= Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD_START.getRotations() && getState() == SuperstructureState.FOTM; + } + public double getTargetRPM() { return shooter.getTargetRPM(); } @@ -176,7 +182,7 @@ public boolean shouldStop() { boolean isSpindexerStopState = Spindexer.getInstance().getState() == SpindexerState.STOP; boolean isHandOffStopState = Handoff.getInstance().getState() == HandoffState.STOP; - boolean isTurretWrapping = isTurretWrapping(); + // boolean isTurretWrapping = isTurretWrapping(); boolean isBehindHubWhileFerrying = getState() == SuperstructureState.FOTM && swerve.isBehindHub(); boolean isOutsideAllianceZone = @@ -191,19 +197,22 @@ public boolean shouldStop() { boolean isBehindTower = swerve.isBehindTower() && getState() == SuperstructureState.SOTM; boolean turretLaggingSOTM = !isTurretAtTolerance() && getState() == SuperstructureState.SOTM; + boolean turretLaggingFOTM = isTurretLaggingFOTM(); + DogLog.log("Spindexer/Should Stop/Is Behind Tower?", isBehindTower); DogLog.log("Spindexer/Should Stop/Is Behind Hub While Ferrying?", isBehindHubWhileFerrying); - DogLog.log("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); + // DogLog.log("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); DogLog.log("Spindexer/Should Stop/Is Outside Alliance Zone?", isOutsideAllianceZone); DogLog.log("Spindexer/Should Stop/Is Under Trench?", isUnderTrench); DogLog.log("Spindexer/Should Stop/Turret Lagging SOTM", turretLaggingSOTM); + DogLog.log("Spindexer/Should Stop/Turret Lagging FOTM", turretLaggingFOTM); DogLog.log("Spindexer/Should Stop/In Manual State", inManualState); boolean shouldStop = isSpindexerStopState || isHandOffStopState || - isTurretWrapping || (isBehindHubWhileFerrying && !inManualState) || turretLaggingSOTM || + turretLaggingFOTM || (isOutsideAllianceZone && !inManualState) || (isUnderTrench && !inManualState) || isBehindTower; From f9135140e733b77fdec0b5b4a1dd351361a32fc2 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 29 Apr 2026 20:30:04 -0400 Subject: [PATCH 07/97] feat: left bite auton change --- src/main/deploy/pathplanner/autos/Left Corner Bite.auto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto index ce84e81a..06e4600e 100644 --- a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto @@ -13,7 +13,7 @@ { "type": "path", "data": { - "pathName": "Left Corner Bite To Score" + "pathName": "Left NZ To Score" } }, { From 897019b93a05bae5995b0e22d345cb269e1c9e3c Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Wed, 29 Apr 2026 22:34:31 -0400 Subject: [PATCH 08/97] FIX: auton path revert --- src/main/deploy/pathplanner/paths/Left Corner Bite.path | 4 ++-- src/main/deploy/pathplanner/paths/Right Corner Bite.path | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite.path b/src/main/deploy/pathplanner/paths/Left Corner Bite.path index 771483ab..944dd430 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite.path @@ -20,8 +20,8 @@ "y": 4.65425572519084 }, "prevControl": { - "x": 7.584251069900143, - "y": 7.3149500713266775 + "x": 7.226358244365363, + "y": 7.3219335705812565 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite.path b/src/main/deploy/pathplanner/paths/Right Corner Bite.path index 8dcbf94d..399ce580 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite.path @@ -20,8 +20,8 @@ "y": 3.4271055753262156 }, "prevControl": { - "x": 7.998288159771754, - "y": 1.906590584878745 + "x": 7.656725978647687, + "y": 2.168279952550415 }, "nextControl": null, "isLocked": false, From 911f5c7d8ea9921251dfe53e345ee649eae57f41 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 30 Apr 2026 12:14:22 -0400 Subject: [PATCH 09/97] FEAT: added back is wrapping logging --- .../robot/subsystems/superstructure/Superstructure.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index a8b66448..a42787c8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -182,7 +182,7 @@ public boolean shouldStop() { boolean isSpindexerStopState = Spindexer.getInstance().getState() == SpindexerState.STOP; boolean isHandOffStopState = Handoff.getInstance().getState() == HandoffState.STOP; - // boolean isTurretWrapping = isTurretWrapping(); + boolean isTurretWrapping = isTurretWrapping(); boolean isBehindHubWhileFerrying = getState() == SuperstructureState.FOTM && swerve.isBehindHub(); boolean isOutsideAllianceZone = @@ -201,7 +201,7 @@ public boolean shouldStop() { DogLog.log("Spindexer/Should Stop/Is Behind Tower?", isBehindTower); DogLog.log("Spindexer/Should Stop/Is Behind Hub While Ferrying?", isBehindHubWhileFerrying); - // DogLog.log("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); + DogLog.log("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); DogLog.log("Spindexer/Should Stop/Is Outside Alliance Zone?", isOutsideAllianceZone); DogLog.log("Spindexer/Should Stop/Is Under Trench?", isUnderTrench); DogLog.log("Spindexer/Should Stop/Turret Lagging SOTM", turretLaggingSOTM); From 10a37ce4b2d36bd90376bfd3d1aa8546120babbf Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 30 Apr 2026 12:14:46 -0400 Subject: [PATCH 10/97] Reapply "feat: stop ferrying when behind opponent's hub (rectangle restriction zone)" This reverts commit d34d211a002a132bef2584ddbf733d8f6801b413. --- simgui.json | 3 ++- .../com/stuypulse/robot/constants/Field.java | 2 ++ .../superstructure/Superstructure.java | 3 +++ .../swerve/CommandSwerveDrivetrain.java | 17 +++++++++++++++++ 4 files changed, 24 insertions(+), 1 deletion(-) diff --git a/simgui.json b/simgui.json index 92d7c18b..c86a9772 100644 --- a/simgui.json +++ b/simgui.json @@ -108,7 +108,8 @@ "Robot": { "Auton": { "open": true - } + }, + "open": true }, "Spindexer": { "open": true diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 5a1c76b9..907b02e9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -43,6 +43,8 @@ public interface Field { public static final double OPPONENT_ZONE_X = LENGTH - Units.inchesToMeters(158.6); + public static final double OPPONENT_HUB_DS_X = LENGTH - HUB_FAR_LEFT_CORNER.getX() + 2.0 * HUB_RADIUS; + public static final double BEHIND_HUB_TOLERANCE_X = Units.inchesToMeters(144); // To extend the triangle vertex public static final double BEHIND_HUB_TOLERANCE_Y = Units.inchesToMeters(12) + Units.inchesToMeters(2); // To extend base of triangle (colinear with back hub) diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index a42787c8..c3058adb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -195,12 +195,14 @@ public boolean shouldStop() { getState() == SuperstructureState.RIGHT_CORNER && getState() == SuperstructureState.KB; boolean isBehindTower = swerve.isBehindTower() && getState() == SuperstructureState.SOTM; + boolean isBtwnOppHubAndWall = swerve.isBtwnOppHubAndWall() && getState() == SuperstructureState.FOTM; boolean turretLaggingSOTM = !isTurretAtTolerance() && getState() == SuperstructureState.SOTM; boolean turretLaggingFOTM = isTurretLaggingFOTM(); DogLog.log("Spindexer/Should Stop/Is Behind Tower?", isBehindTower); DogLog.log("Spindexer/Should Stop/Is Behind Hub While Ferrying?", isBehindHubWhileFerrying); + DogLog.log("Spindexer/Should Stop/Is behind Opponent's Hub While Ferrying?", isBtwnOppHubAndWall); DogLog.log("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); DogLog.log("Spindexer/Should Stop/Is Outside Alliance Zone?", isOutsideAllianceZone); DogLog.log("Spindexer/Should Stop/Is Under Trench?", isUnderTrench); @@ -211,6 +213,7 @@ public boolean shouldStop() { boolean shouldStop = isSpindexerStopState || isHandOffStopState || (isBehindHubWhileFerrying && !inManualState) || + isBtwnOppHubAndWall || turretLaggingSOTM || turretLaggingFOTM || (isOutsideAllianceZone && !inManualState) || diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 1368e99a..b3abc799 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -86,6 +86,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private Optional isInOpponentZone = Optional.empty(); private Optional isUnderTrench = Optional.empty(); private Optional isBehindTower = Optional.empty(); + private Optional isBtwnOppHubAndWall = Optional.empty(); // private StructPublisher robotPose = NetworkTableInstance.getDefault() // .getStructTopic("Robot Pose", Pose2d.struct).publish(); @@ -650,12 +651,28 @@ public boolean isOutsideAllianceZone() { return isOutsideAllianceZone.get(); } + public boolean isBtwnOppHubAndWall() { + if (!isBtwnOppHubAndWall.isEmpty()) { + return isBtwnOppHubAndWall.get(); + } + + Translation2d turretTranslation = getTurretPose().getTranslation(); + + boolean btwnOppHubAndWallX = turretTranslation.getX() < Field.LENGTH && turretTranslation.getX() > Field.OPPONENT_HUB_DS_X; + boolean btwnOppHubAndWallY = turretTranslation.getY() < Field.HUB_FAR_LEFT_CORNER.getY() && turretTranslation.getY() > Field.HUB_FAR_RIGHT_CORNER.getY(); + + isBtwnOppHubAndWall = Optional.of(btwnOppHubAndWallX && btwnOppHubAndWallY); + + return isBtwnOppHubAndWall.get(); + } + public void clearMemoized() { isBehindHub = Optional.empty(); isOutsideAllianceZone = Optional.empty(); isInOpponentZone = Optional.empty(); isUnderTrench = Optional.empty(); isBehindTower = Optional.empty(); + isBtwnOppHubAndWall = Optional.empty(); } public void teleopInit() { From 0575684778513591241e2ecbdfec893f738c12ee Mon Sep 17 00:00:00 2001 From: Apetrock24 Date: Thu, 30 Apr 2026 12:39:35 -0500 Subject: [PATCH 11/97] feat: log time since boot for each camera --- src/main/java/com/stuypulse/robot/constants/Cameras.java | 6 ++++++ .../stuypulse/robot/subsystems/vision/LimelightVision.java | 7 ++----- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index fc9e2cfb..d9daa6b4 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -8,6 +8,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.util.vision.LimelightHelpers; +import com.stuypulse.robot.util.vision.LimelightHelpers.LimelightResults; import com.stuypulse.robot.util.vision.LimelightHelpers.RawFiducial; import com.stuypulse.stuylib.network.SmartBoolean; @@ -48,6 +49,7 @@ public static class Camera { private int rejectedCounterAngularVelocity; private int rejectedCounterInvalidPosition; private int rejectedCounterTargetArea; + private LimelightResults result; private Pipeline currentPipeline; @@ -56,6 +58,7 @@ public Camera(String name, Pose3d location, SmartBoolean isEnabled) { this.location = location; this.isEnabled = isEnabled; this.keyName = "Vision/" + name + "/"; + this.result = LimelightHelpers.getLatestResults(name); } public enum Pipeline { @@ -128,6 +131,9 @@ public void log() { ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).pose : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(name).pose)); DogLog.log(keyName + "Pipeline", LimelightHelpers.getCurrentPipelineIndex(name)); + + result = LimelightHelpers.getLatestResults(name); + DogLog.log(keyName + "Time since last boot", result.timestamp_LIMELIGHT_publish / 1000.0, "Seconds"); RawFiducial[] rawFiducials = LimelightHelpers.getRawFiducials(name); for(Integer i = 0; i < rawFiducials.length; i++) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index c7b02853..39723015 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -5,6 +5,7 @@ /** ************************************************************ */ package com.stuypulse.robot.subsystems.vision; +import java.sql.ResultSet; import java.util.Arrays; import com.stuypulse.robot.Robot; @@ -17,6 +18,7 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.robot.util.vision.LimelightHelpers.IMUData; +import com.stuypulse.robot.util.vision.LimelightHelpers.LimelightResults; import com.stuypulse.robot.util.vision.LimelightHelpers.PoseEstimate; import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.streams.booleans.BStream; @@ -263,11 +265,6 @@ public void periodicAfterScheduler() { Cameras.LimelightCameras[i].incrementRejection(RejectionValue.ANGULAR_VELOCITY); } - // if (poseEstimate.avgTagArea >= Settings.Vision.MIN_TAG_AREA) { - // withinTargetAreaTolerance = true; - // } else { - // Cameras.LimelightCameras[i].incrementRejection(RejectionValue.TARGET_AREA); - // } Pose2d robotPose = poseEstimate.pose; double timestamp = poseEstimate.timestampSeconds; From 5bbe4c341b31981a3445538915ff4bbd3ff543d1 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 30 Apr 2026 13:41:20 -0400 Subject: [PATCH 12/97] FIX: auton timeout wacky --- .../pathplanner/paths/Left Corner Bite.path | 10 +++++----- .../pathplanner/paths/Left NZ To Score.path | 16 ++++++++-------- .../pathplanner/paths/Left Score To Score.path | 12 ++++++------ .../paths/Right Bite Score To Score.path | 4 ++-- .../pathplanner/paths/Right Score To Score.path | 2 +- .../auton/regular/LeftTwoCornerShallow.java | 2 +- .../auton/regular/RightTwoCornerShallow.java | 2 +- 7 files changed, 24 insertions(+), 24 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite.path b/src/main/deploy/pathplanner/paths/Left Corner Bite.path index 944dd430..5b217fce 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 8.295104961832061, - "y": 4.65425572519084 + "x": 8.248481613285884, + "y": 4.621376037959667 }, "prevControl": { - "x": 7.226358244365363, - "y": 7.3219335705812565 + "x": 7.179734895819186, + "y": 7.289053883350084 }, "nextControl": null, "isLocked": false, - "linkedName": "Left NZ Corner" + "linkedName": "Left NZ" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index c2a3c711..11c8cf9d 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 6.052395038167939, - "y": 6.378129770992366 + "x": 6.212753209700427, + "y": 6.38336661911555 }, "prevControl": { - "x": 6.055976129616249, - "y": 5.761441920527078 + "x": 6.2369188800296484, + "y": 5.767142025720414 }, "nextControl": { - "x": 6.044550641940085, - "y": 7.7289871611982885 + "x": 6.160998573466475, + "y": 7.703109843081313 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 7.440906488549619 }, "prevControl": { - "x": 6.031400289662534, - "y": 7.444336661911555 + "x": 6.484465049928673, + "y": 7.483152639087019 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index d65e01e1..58803166 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 6.937318116975749, - "y": 4.571954350927247 + "x": 7.0667047075606275, + "y": 4.533138373751783 }, "prevControl": { - "x": 6.567013040113212, - "y": 4.295700209513629 + "x": 6.69639963069809, + "y": 4.256884232338165 }, "nextControl": { - "x": 7.5790326308920575, - "y": 5.0506847360913 + "x": 7.708419221476936, + "y": 5.011868758915837 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path index 81d66657..6ed6c379 100644 --- a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path @@ -57,7 +57,7 @@ }, "nextControl": { "x": 6.070427960057061, - "y": 0.15987161198288113 + "y": 0.1598716119828809 }, "isLocked": false, "linkedName": null @@ -116,7 +116,7 @@ "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "nominalVoltage": 12.7, "unlimited": false } } diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 22f7f845..8097a650 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -52,7 +52,7 @@ "y": 0.5868473609129818 }, "prevControl": { - "x": 7.673089129599263, + "x": 7.673089129599262, "y": 0.5487960706826538 }, "nextControl": null, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java index 33d221a9..c56183cb 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java @@ -72,7 +72,7 @@ public LeftTwoCornerShallow(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java index 15b1bcd4..74d1a52b 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java @@ -72,7 +72,7 @@ public RightTwoCornerShallow(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) From ba4fd4dec665aa7b46e7da4522b4e512c4355430 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 30 Apr 2026 18:34:28 -0400 Subject: [PATCH 13/97] feat: is hopper empty debounce 1->2 + auton changes --- .../pathplanner/autos/Left Corner Bite.auto | 6 ++++++ .../autos/Left Shallow 2 Cycle.auto | 6 ++++++ .../pathplanner/autos/Left Two Cycle.auto | 6 ++++++ .../pathplanner/autos/Right Corner Bite.auto | 6 ++++++ .../autos/Right Shallow 2 Cycle.auto | 6 ++++++ .../pathplanner/autos/Right Two Cycle.auto | 6 ++++++ .../paths/Left Bite Score To Score.path | 6 +++--- .../pathplanner/paths/Left Corner Bite.path | 12 +++++------ .../pathplanner/paths/Left NZ To Score.path | 18 ++++++++--------- .../paths/Left Score To NZ (F).path | 8 ++++---- .../paths/Left Score To Score.path | 8 ++++---- .../paths/Left Shallow To Score.path | 8 ++++---- .../pathplanner/paths/Left To Shallow.path | 8 ++++---- .../pathplanner/paths/Left Trench To NZ.path | 8 ++++---- .../paths/Right Bite Score To Score.path | 8 ++++---- .../pathplanner/paths/Right Corner Bite.path | 14 ++++++------- .../pathplanner/paths/Right NZ To Score.path | 20 +++++++++---------- .../paths/Right Score To NZ (F).path | 8 ++++---- .../paths/Right Score To Score.path | 10 +++++----- .../paths/Right Shallow To Score.path | 8 ++++---- .../pathplanner/paths/Right To Shallow.path | 8 ++++---- .../pathplanner/paths/Right Trench To NZ.path | 12 +++++------ .../commands/auton/regular/LeftTwoCorner.java | 1 + .../auton/regular/LeftTwoCornerShallow.java | 2 +- .../commands/auton/regular/LeftTwoCycle.java | 1 + .../auton/regular/RightTwoCorner.java | 3 ++- .../auton/regular/RightTwoCornerShallow.java | 1 + .../commands/auton/regular/RightTwoCycle.java | 1 + .../superstructure/shooter/ShooterImpl.java | 2 +- 29 files changed, 126 insertions(+), 85 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto index 06e4600e..b2366e54 100644 --- a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto @@ -16,6 +16,12 @@ "pathName": "Left NZ To Score" } }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto index 89cb8412..46ee95a1 100644 --- a/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto @@ -16,6 +16,12 @@ "pathName": "Left Shallow To Score" } }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto index 3a598107..45cb3da1 100644 --- a/src/main/deploy/pathplanner/autos/Left Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto @@ -16,6 +16,12 @@ "pathName": "Left NZ To Score" } }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto index 2690d7aa..1953c829 100644 --- a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto @@ -16,6 +16,12 @@ "pathName": "Right NZ To Score" } }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto index 6f06e921..baa1fe96 100644 --- a/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto @@ -16,6 +16,12 @@ "pathName": "Right Shallow To Score" } }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/Right Two Cycle.auto b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto index c64320b2..60f97953 100644 --- a/src/main/deploy/pathplanner/autos/Right Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto @@ -16,6 +16,12 @@ "pathName": "Right NZ To Score" } }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path index d5e090bd..53c32926 100644 --- a/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.6506870229007635, + "x": 3.3075858778625955, "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 7.907505853143271, + "x": 7.564404708105102, "y": 7.509029957203994 }, "isLocked": false, - "linkedName": "Left Trench Score" + "linkedName": "Left Corner" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite.path b/src/main/deploy/pathplanner/paths/Left Corner Bite.path index 5b217fce..272d23e2 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.248481613285884, - "y": 4.621376037959667 + "x": 7.843024251069899, + "y": 4.6366476462196875 }, "prevControl": { - "x": 7.179734895819186, - "y": 7.289053883350084 + "x": 7.493680456490727, + "y": 7.509029957203994 }, "nextControl": null, "isLocked": false, @@ -38,8 +38,8 @@ "rotationDegrees": -55.0 }, { - "waypointRelativePos": 0.9253731343283487, - "rotationDegrees": -55.0 + "waypointRelativePos": 0.9402985074626858, + "rotationDegrees": -90.0 } ], "constraintZones": [ diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 11c8cf9d..79ba0fc4 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.248481613285884, - "y": 4.621376037959667 + "x": 7.843024251069899, + "y": 4.6366476462196875 }, "prevControl": null, "nextControl": { - "x": 5.7738671411625155, - "y": 4.589098457888493 + "x": 6.43271041369472, + "y": 4.6495863052781745 }, "isLocked": false, "linkedName": "Left NZ" }, { "anchor": { - "x": 6.212753209700427, + "x": 6.510342368045648, "y": 6.38336661911555 }, "prevControl": { - "x": 6.2369188800296484, - "y": 5.767142025720414 + "x": 6.475815398217673, + "y": 5.767635657183343 }, "nextControl": { - "x": 6.160998573466475, - "y": 7.703109843081313 + "x": 6.587974322396576, + "y": 7.767803138373752 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index b8c3689a..b2095fbf 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.248481613285884, - "y": 4.621376037959667 + "x": 7.843024251069899, + "y": 4.6366476462196875 }, "prevControl": { - "x": 7.860321841531247, - "y": 7.196169190598754 + "x": 7.454864479315262, + "y": 7.211440798858774 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index 58803166..b8d29067 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.6506870229007635, + "x": 3.3075858778625955, "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 6.7300878788208784, - "y": 7.521968616262482 + "x": 6.678544935805991, + "y": 7.560784593437946 }, "isLocked": false, - "linkedName": "Left Trench Score" + "linkedName": "Left Corner" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Left Shallow To Score.path b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path index 0cc25861..ca317290 100644 --- a/src/main/deploy/pathplanner/paths/Left Shallow To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.24489503816794, - "y": 5.382299618320611 + "x": 7.791269614835947, + "y": 5.374151212553495 }, "prevControl": null, "nextControl": { - "x": 6.911440798858774, - "y": 7.819557774607704 + "x": 6.457815375526781, + "y": 7.811409368840588 }, "isLocked": false, "linkedName": "Left Shallow" diff --git a/src/main/deploy/pathplanner/paths/Left To Shallow.path b/src/main/deploy/pathplanner/paths/Left To Shallow.path index 543a18c6..1b597877 100644 --- a/src/main/deploy/pathplanner/paths/Left To Shallow.path +++ b/src/main/deploy/pathplanner/paths/Left To Shallow.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.24489503816794, - "y": 5.382299618320611 + "x": 7.791269614835947, + "y": 5.374151212553495 }, "prevControl": { - "x": 7.466641221374045, - "y": 7.030858778625954 + "x": 7.584251069900143, + "y": 7.133808844507846 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 3568773d..bbeaa4c0 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.248481613285884, - "y": 4.621376037959667 + "x": 7.843024251069899, + "y": 4.6366476462196875 }, "prevControl": { - "x": 8.196726977051933, - "y": 7.442003712710024 + "x": 7.791269614835947, + "y": 7.457275320970044 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path index 6ed6c379..7dba712c 100644 --- a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, + "x": 3.2824809160305346, "y": 0.5868473609129818 }, "prevControl": null, "nextControl": { - "x": 5.565820256776034, + "x": 5.236218433862203, "y": 0.5997860199714706 }, "isLocked": false, - "linkedName": "Right Jiggle" + "linkedName": "Right Corner" }, { "anchor": { @@ -57,7 +57,7 @@ }, "nextControl": { "x": 6.070427960057061, - "y": 0.1598716119828809 + "y": 0.1598716119828807 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite.path b/src/main/deploy/pathplanner/paths/Right Corner Bite.path index 399ce580..f672dfc5 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.237722419928826, - "y": 3.4271055753262156 + "x": 7.868901569186875, + "y": 3.472168330955778 }, "prevControl": { - "x": 7.656725978647687, - "y": 2.168279952550415 + "x": 7.648944365192582, + "y": 2.2041797432239663 }, "nextControl": null, "isLocked": false, @@ -34,12 +34,12 @@ "rotationDegrees": 90.0 }, { - "waypointRelativePos": 0.45842217484008524, + "waypointRelativePos": 0.41791044776119385, "rotationDegrees": 55.0 }, { - "waypointRelativePos": 0.8102345415778156, - "rotationDegrees": 55.0 + "waypointRelativePos": 0.7782515991471214, + "rotationDegrees": 90.0 } ], "constraintZones": [ diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index c569aff8..e3db961a 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.237722419928826, - "y": 3.4271055753262156 + "x": 7.868901569186875, + "y": 3.472168330955778 }, "prevControl": null, "nextControl": { - "x": 5.763107947805457, - "y": 3.5131791221826814 + "x": 5.9927960057061345, + "y": 3.6144935805991443 }, "isLocked": false, "linkedName": "Right NZ" }, { "anchor": { - "x": 6.053851272542522, - "y": 1.6151808747904899 + "x": 6.419771754636234, + "y": 1.4925534950071324 }, "prevControl": { - "x": 6.040480921648686, - "y": 2.6846376869648685 + "x": 6.342788623034002, + "y": 2.5593197472095173 }, "nextControl": { - "x": 6.070427960057061, - "y": 0.289258202567761 + "x": 6.510342368045648, + "y": 0.23750356633380876 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index c0cb71bc..d9dd7f97 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.237722419928826, - "y": 3.4271055753262156 + "x": 7.868901569186875, + "y": 3.472168330955778 }, "prevControl": { - "x": 8.185967783694872, - "y": 0.697048513985274 + "x": 7.817146932952921, + "y": 0.7421112696148362 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 8097a650..d2b56676 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, + "x": 3.2824809160305346, "y": 0.5868473609129818 }, "prevControl": null, "nextControl": { - "x": 7.0796433666191145, - "y": 0.5868473609129818 + "x": 6.885563480741797, + "y": 0.5480313837375184 }, "isLocked": false, - "linkedName": "Right Jiggle" + "linkedName": "Right Corner" }, { "anchor": { @@ -52,7 +52,7 @@ "y": 0.5868473609129818 }, "prevControl": { - "x": 7.673089129599262, + "x": 7.673089129599261, "y": 0.5487960706826538 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path index bc133dac..ea1e749d 100644 --- a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.21142175572519, - "y": 2.679332061068702 + "x": 7.868901569186875, + "y": 2.7346647646219684 }, "prevControl": null, "nextControl": { - "x": 6.549158345221112, - "y": 0.27631954350927357 + "x": 6.419771754636235, + "y": 0.3410128388017126 }, "isLocked": false, "linkedName": "Right Shallow NZ" diff --git a/src/main/deploy/pathplanner/paths/Right To Shallow.path b/src/main/deploy/pathplanner/paths/Right To Shallow.path index 0aa62f2c..a90eea65 100644 --- a/src/main/deploy/pathplanner/paths/Right To Shallow.path +++ b/src/main/deploy/pathplanner/paths/Right To Shallow.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.21142175572519, - "y": 2.679332061068702 + "x": 7.868901569186875, + "y": 2.7346647646219684 }, "prevControl": { - "x": 8.24489503816794, - "y": 1.4491889312977104 + "x": 7.902374851629624, + "y": 1.5045216348509767 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 13b73e23..3aee608c 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 9.06721902017291, - "y": 0.41484149855907726 + "x": 8.684037089871612, + "y": 0.37982881597717666 }, "isLocked": false, "linkedName": "Right Trench Start" }, { "anchor": { - "x": 8.237722419928826, - "y": 3.4271055753262156 + "x": 7.868901569186875, + "y": 3.472168330955778 }, "prevControl": { - "x": 8.14623827007292, - "y": 0.4342669586115182 + "x": 7.77741741933097, + "y": 0.4793297142410804 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java index c4e05f49..5605fd39 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java @@ -53,6 +53,7 @@ public LeftTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java index c56183cb..cd5b5249 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java @@ -53,7 +53,7 @@ public LeftTwoCornerShallow(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - new HandoffRun(), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index a6586094..5d493453 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -53,6 +53,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java index d8aed42f..1ab70122 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java @@ -53,12 +53,13 @@ public RightTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java index 74d1a52b..fb263646 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java @@ -53,6 +53,7 @@ public RightTwoCornerShallow(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index 2e860ec2..bdc1c3d9 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -53,6 +53,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 30bdae5b..0a332e22 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -111,7 +111,7 @@ public ShooterImpl() { shooterFollowerTemperature = shooterFollower.getDeviceTemp(); currentlyShooting = BStream.create(() -> (shooterLeadStatorCurrent.getValueAsDouble() > Settings.Superstructure.Shooter.IS_SHOOTING_CURRENT)) - .filtered(new BDebounce.Falling(1.0)); + .filtered(new BDebounce.Falling(2.0)); PhoenixUtil.registerToRio(shooterLeaderSpeed, shooterFollowerSpeed, shooterFollowSupplyCurrent, shooterFollowStatorCurrent, shooterLeadSupplyCurrent, shooterLeadStatorCurrent, From 46a5f84bd96a7750864e5ef668b10ebccb771ead Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 30 Apr 2026 18:37:24 -0400 Subject: [PATCH 14/97] feat: shallow autos --- .../java/com/stuypulse/robot/RobotContainer.java | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 14efea5d..fd3b5019 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -449,6 +449,19 @@ public boolean hasWaitTimeTwoChanged() { return hasWaitTimeTwoChanged; } + public boolean hasWaitTimeOneChanged() { + hasWaitTimeOneChanged = prevWaitTimeOne != getWaitTimeOne(); + prevWaitTimeOne = getWaitTimeOne(); + prevWaitTimeTwo = getWaitTimeTwo(); + return hasWaitTimeOneChanged; + } + + public boolean hasWaitTimeTwoChanged() { + hasWaitTimeTwoChanged = prevWaitTimeTwo != getWaitTimeTwo(); + prevWaitTimeTwo = getWaitTimeTwo(); + return hasWaitTimeTwoChanged; + } + public void configureSysids() { // autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); // autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); From cfb295f4cb564c4fd1a8ea3e442246d2eac76963 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 30 Apr 2026 18:38:36 -0400 Subject: [PATCH 15/97] fix: remove double time chaning method --- .../java/com/stuypulse/robot/RobotContainer.java | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index fd3b5019..14efea5d 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -449,19 +449,6 @@ public boolean hasWaitTimeTwoChanged() { return hasWaitTimeTwoChanged; } - public boolean hasWaitTimeOneChanged() { - hasWaitTimeOneChanged = prevWaitTimeOne != getWaitTimeOne(); - prevWaitTimeOne = getWaitTimeOne(); - prevWaitTimeTwo = getWaitTimeTwo(); - return hasWaitTimeOneChanged; - } - - public boolean hasWaitTimeTwoChanged() { - hasWaitTimeTwoChanged = prevWaitTimeTwo != getWaitTimeTwo(); - prevWaitTimeTwo = getWaitTimeTwo(); - return hasWaitTimeTwoChanged; - } - public void configureSysids() { // autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); // autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); From d6f45d92c562326c28f913e42b02d895ef971ed4 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 30 Apr 2026 18:59:08 -0400 Subject: [PATCH 16/97] fix: optimize haswaittimechanged function --- src/main/java/com/stuypulse/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 14efea5d..68b39a91 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -439,7 +439,6 @@ public void configureAutons() { public boolean hasWaitTimeOneChanged() { hasWaitTimeOneChanged = prevWaitTimeOne != getWaitTimeOne(); prevWaitTimeOne = getWaitTimeOne(); - prevWaitTimeTwo = getWaitTimeTwo(); return hasWaitTimeOneChanged; } From 89c85d0c1a99af001a82f44d470eab0c9cbb0ff1 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 30 Apr 2026 19:38:56 -0400 Subject: [PATCH 17/97] feat: auton timeout --- .../robot/commands/auton/regular/LeftTwoCorner.java | 2 +- .../robot/commands/auton/regular/LeftTwoCornerShallow.java | 5 +++-- .../stuypulse/robot/commands/auton/regular/LeftTwoCycle.java | 2 +- .../robot/commands/auton/regular/RightTwoCorner.java | 4 ++-- .../robot/commands/auton/regular/RightTwoCornerShallow.java | 2 +- .../robot/commands/auton/regular/RightTwoCycle.java | 2 +- 6 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java index 5605fd39..18cb2932 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java @@ -57,7 +57,7 @@ public LeftTwoCorner(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) ), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java index cd5b5249..77620136 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java @@ -53,10 +53,11 @@ public LeftTwoCornerShallow(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)), new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 5d493453..07e874e5 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -57,7 +57,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) ), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java index 1ab70122..94ca1766 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java @@ -57,9 +57,9 @@ public RightTwoCorner(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java index fb263646..c630a79e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java @@ -57,7 +57,7 @@ public RightTwoCornerShallow(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)), new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index bdc1c3d9..b3c583e4 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -57,7 +57,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) ), From 3211a7ad65ea8adff9597faf8370df1e5526ffce Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 1 May 2026 11:04:32 -0400 Subject: [PATCH 18/97] FEAT: added 0.5 to first pass autons --- .../robot/commands/auton/regular/LeftTwoCorner.java | 10 ++++------ .../commands/auton/regular/LeftTwoCornerShallow.java | 10 ++++------ .../robot/commands/auton/regular/LeftTwoCycle.java | 10 ++++------ .../robot/commands/auton/regular/RightTwoCorner.java | 10 ++++------ .../commands/auton/regular/RightTwoCornerShallow.java | 10 ++++------ .../robot/commands/auton/regular/RightTwoCycle.java | 10 ++++------ 6 files changed, 24 insertions(+), 36 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java index 18cb2932..c7f5043c 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java @@ -59,7 +59,7 @@ public LeftTwoCorner(PathPlannerPath... paths) { new WaitCommand(0.5) .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -77,11 +77,9 @@ public LeftTwoCorner(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) - ), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java index 77620136..6c2a3d86 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java @@ -57,7 +57,7 @@ public LeftTwoCornerShallow(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)), new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), @@ -77,11 +77,9 @@ public LeftTwoCornerShallow(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) - ), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 07e874e5..8faab6a6 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -59,7 +59,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new WaitCommand(0.5) .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -77,11 +77,9 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) - ), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java index 94ca1766..a386e83f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java @@ -59,7 +59,7 @@ public RightTwoCorner(PathPlannerPath... paths) { new WaitCommand(0.5) .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -77,11 +77,9 @@ public RightTwoCorner(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) - ), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java index c630a79e..fe74ea44 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java @@ -57,7 +57,7 @@ public RightTwoCornerShallow(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)), new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), @@ -77,11 +77,9 @@ public RightTwoCornerShallow(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) - ), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index b3c583e4..59fe6f3b 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -59,7 +59,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new WaitCommand(0.5) .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -77,12 +77,10 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) - ), + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) - ); } From c01b3148442dbd8cdc1d9878e4eb21d5ec1a88e6 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 1 May 2026 11:05:13 -0400 Subject: [PATCH 19/97] FEAT: Log dist to virtual pose, add extrapolated lerp point at 10 --- src/main/java/com/stuypulse/robot/constants/Settings.java | 6 ++++-- .../stuypulse/robot/util/superstructure/SOTMCalculator.java | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 9b6b6a37..14ddf9ed 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -142,7 +142,8 @@ public interface RPMInterpolation{ {3.38, 3075}, {4.43, 3350.0}, {5.66, 3650.0}, - {6.44, 3800} + {6.44, 3800.0}, + {10.0, 4646.0} // THIS POINT IS AN EXTRAPOLATION }; } @@ -154,7 +155,8 @@ public interface TOFInterpolation{ {3.38, 1.02}, {4.43, 1.165}, {5.50, 1.21}, - {6.44, 1.255} + {6.44, 1.255}, + {10.0, 1.46} // THIS POINT IS AN EXTRAPOLATION }; } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index debae0cc..a9023f9d 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -18,6 +18,7 @@ import com.stuypulse.robot.util.superstructure.InterpolationCalculator.InterpolatedShotInfo; import com.stuypulse.stuylib.network.SmartBoolean; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; @@ -338,7 +339,7 @@ public static void updateSOTMSolution() { // SmartDashboard.putNumber("Superstructure/SOTM/calculated turret angle", hubSol.targetTurretAngle().getDegrees()); // SmartDashboard.putNumber("Superstructure/SOTM/calculated hood angle", hubSol.targetHoodAngle().getDegrees()); // SmartDashboard.putNumber("Superstructure/SOTM/calculated flight time", hubSol.flightTime()); - // SmartDashboard.putNumber("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(hubSol.virtualPose().getTranslation())); + DogLog.log("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(hubSol.virtualPose().getTranslation())); } public static void updateFOTMSolution() { From a6295f774c597c7f09ce30470d0bc7885288cfad Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 1 May 2026 11:08:01 -0400 Subject: [PATCH 20/97] feat: (util) add log script to the root directory --- logs.py | 47 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 logs.py diff --git a/logs.py b/logs.py new file mode 100644 index 00000000..da4fab34 --- /dev/null +++ b/logs.py @@ -0,0 +1,47 @@ +#!/usr/bin/python3 +import os +from fabric import Connection + +MAX_NUMBER_OF_LOGS_TO_PULL = 5 +TARGET_IP_ADDRESS = "10.6.94.2" +USB_DIRECTORY = "D:\\MILSTEIN_LOGS" +LOCAL_DIRECTORY_TO_SAVE_LOGS = "C:\\Users\\test\\logs\\CHAMPS_LOGS_26" +os.makedirs(LOCAL_DIRECTORY_TO_SAVE_LOGS, exist_ok=True) # if the dir does not exist, create a new one + +SAVE_DIRECTORY = USB_DIRECTORY if os.path.exists(USB_DIRECTORY) else LOCAL_DIRECTORY_TO_SAVE_LOGS + +raw_local_logs = os.listdir(path=SAVE_DIRECTORY) + +print("DIRECTORY BEING WRITTEN TO: " + SAVE_DIRECTORY) +print(f"THESE FILES WERE ALREADY FOUND FROM {SAVE_DIRECTORY}: " + str(raw_local_logs)) + +wpilogs_local = set(filter(lambda x: x.endswith(".wpilog"), raw_local_logs)) + +ssh_connection = Connection( + host=TARGET_IP_ADDRESS, + user="lvuser", + connect_kwargs={"password": ""}, + connect_timeout=4 +) + +raw_remote_logs = ssh_connection.run("ls -t /home/lvuser/logs/", hide=True) +remote_files = raw_remote_logs.stdout.strip().split() + +all_remote_wpilog_logs = [f for f in remote_files if f.endswith(".wpilog")] +wpilogs_remote_newest = all_remote_wpilog_logs[:MAX_NUMBER_OF_LOGS_TO_PULL] + +logs_to_be_downloaded = [] +for log_name in wpilogs_remote_newest: + if log_name not in wpilogs_local: + print(f"Missing log found, appending {log_name} to {SAVE_DIRECTORY}" ) + logs_to_be_downloaded.append(log_name) + ssh_connection.get(f"/home/lvuser/logs/{log_name}", local=f"{SAVE_DIRECTORY}/{log_name}") # need to include the name of the file + if (os.path.exists(SAVE_DIRECTORY)): + print(f"SUCCESS: {SAVE_DIRECTORY} found and being written to") + else: + print(f"ERROR: {SAVE_DIRECTORY} not found!") + + +#todo: add a post request to an api endpoint that allows us to automatically upload logs to a server + +print(f"Downloaded {len(logs_to_be_downloaded)} log(s): {logs_to_be_downloaded} directory: {SAVE_DIRECTORY} ") From 77b15e474f4eb8ec334de0effbc5c04d23a6a68f Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 1 May 2026 16:15:45 -0400 Subject: [PATCH 21/97] feat: add extrapolation point --- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 14ddf9ed..e9c4538e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -143,7 +143,7 @@ public interface RPMInterpolation{ {4.43, 3350.0}, {5.66, 3650.0}, {6.44, 3800.0}, - {10.0, 4646.0} // THIS POINT IS AN EXTRAPOLATION + {10.0, 5766.0} // THIS POINT IS AN EXTRAPOLATION }; } From b24057f25f6090cb09ac3b19227269d5c8babf35 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 1 May 2026 17:35:37 -0400 Subject: [PATCH 22/97] feat: add more interpolation points for SOTM outside of the field --- .../java/com/stuypulse/robot/constants/Settings.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index e9c4538e..5bc284dd 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -143,7 +143,7 @@ public interface RPMInterpolation{ {4.43, 3350.0}, {5.66, 3650.0}, {6.44, 3800.0}, - {10.0, 5766.0} // THIS POINT IS AN EXTRAPOLATION + {8.23, 4500.0} // THIS POINT IS AN EXTRAPOLATION }; } @@ -156,7 +156,8 @@ public interface TOFInterpolation{ {4.43, 1.165}, {5.50, 1.21}, {6.44, 1.255}, - {10.0, 1.46} // THIS POINT IS AN EXTRAPOLATION + {6.6, 1.41}, + {8.23, 1.71} // THIS POINT IS AN EXTRAPOLATION }; } @@ -201,7 +202,7 @@ public interface Shooter { public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2.0); public interface RPM { - public final SmartNumber MANUAL_OVERRIDE = new SmartNumber("InterpolationTesting/Shoot State Target RPM", 3500.0); + public final SmartNumber MANUAL_OVERRIDE = new SmartNumber("InterpolationTesting/Shoot State Target RPM", 3863.0); public final double REVERSE = 0.0; public final double KB = 2675.0; @@ -240,7 +241,7 @@ public interface Hood { public final double STALL_DEBOUNCE = 0.5; public interface Angles { - public final SmartNumber MANUAL_OVERRIDE = new SmartNumber("InterpolationTesting/Shoot State Target Angle (deg)", 20.0); + public final SmartNumber MANUAL_OVERRIDE = new SmartNumber("InterpolationTesting/Shoot State Target Angle (deg)", 44.0); public final Rotation2d MAX = FORWARD_SOFT_LIMIT; public final Rotation2d MIN = REVERSE_SOFT_LIMIT; public final Rotation2d FERRY_ANGLE = MAX;//Rotation2d.fromDegrees(44.0); From a6a987e6a097ffab93b5526e35274359ec279150 Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Fri, 1 May 2026 22:45:13 -0400 Subject: [PATCH 23/97] logging(sotm): add the virtual pose to be logged --- .../com/stuypulse/robot/util/superstructure/SOTMCalculator.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index a9023f9d..de1c359c 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -339,6 +339,8 @@ public static void updateSOTMSolution() { // SmartDashboard.putNumber("Superstructure/SOTM/calculated turret angle", hubSol.targetTurretAngle().getDegrees()); // SmartDashboard.putNumber("Superstructure/SOTM/calculated hood angle", hubSol.targetHoodAngle().getDegrees()); // SmartDashboard.putNumber("Superstructure/SOTM/calculated flight time", hubSol.flightTime()); + + DogLog.log("Superstructure/SOTM/virtual pose", virtualHubPose2d.getPose()); DogLog.log("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(hubSol.virtualPose().getTranslation())); } From 6af2fc88bb1017b0a2c3d5eb65404935cb528f1d Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Sat, 2 May 2026 00:42:11 -0400 Subject: [PATCH 24/97] branch name --- .../com/stuypulse/robot/RobotContainer.java | 6 +- .../SuperstructureCacheState.java | 60 +++++++++++++++++++ 2 files changed, 65 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureCacheState.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 68b39a91..8c1f097f 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -41,6 +41,7 @@ import com.stuypulse.robot.commands.spindexer.SpindexerReverse; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureCacheState; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; import com.stuypulse.robot.commands.superstructure.SuperstructureLeftCorner; @@ -196,8 +197,11 @@ private void configureButtonBindings() { // ) // ); - // Digest (TR) + // Shoot in place (TR) driver.getTopButton() + .whileTrue(new SuperstructureCacheState(driver)); + + driver.getDPadLeft() .whileTrue(new IntakeTeleopDigest().repeatedly()) .onFalse(new IntakeDeploy()); diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureCacheState.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureCacheState.java new file mode 100644 index 00000000..0219d989 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureCacheState.java @@ -0,0 +1,60 @@ +package com.stuypulse.robot.commands.superstructure; + +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; +import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; + +import edu.wpi.first.wpilibj2.command.Command; + + +public class SuperstructureCacheState extends Command { + SuperstructureState cachedState; + Superstructure superstructure; + CommandSwerveDrivetrain swerve; + Gamepad driver; + BStream isIdle; + + public SuperstructureCacheState(Gamepad driver) { + superstructure = Superstructure.getInstance(); + swerve = CommandSwerveDrivetrain.getInstance(); + this.driver = driver; + + isIdle = BStream.create( + () -> getDriverInputAsVelocity().magnitude() <= Drive.DEADBAND && Math.abs(driver.getRightX()) <= Turn.DEADBAND) + .filtered(new BDebounce.Both(0.1)); + + this.cachedState = superstructure.getState(); + + addRequirements(superstructure, swerve); + } + + @Override + public void initialize() { + this.cachedState = superstructure.getState(); + superstructure.setState(SuperstructureState.INTERPOLATION); + SwerveRequest request = new SwerveRequest.SwerveDriveBrake(); + swerve.setControl(request); + } + + @Override + public boolean isFinished() { + return !isIdle.get(); + } + + @Override + public void end(boolean interrupted) { + // this presumes that you already overrode the state in the following part of the command chain + superstructure.setState(cachedState); + } + + private Vector2D getDriverInputAsVelocity() { + return new Vector2D(driver.getLeftStick().y, -driver.getLeftStick().x); + } +} From 8fd64a6b76824238930d34fcd8ec044f95a446de Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 11 May 2026 16:37:55 -0400 Subject: [PATCH 25/97] feat: champs code (log cameras no matter what, more auton stuff, add corner variant auton) --- .../pathplanner/autos/Left Corner Bite.auto | 2 +- .../paths/Left Bite Score To Score.path | 2 +- .../pathplanner/paths/Left Corner Bite.path | 8 +- .../pathplanner/paths/Left NZ To Score.path | 16 ++-- .../paths/Left Score To NZ (F).path | 8 +- .../paths/Left Score To Score.path | 28 +++--- .../pathplanner/paths/Left Trench To NZ.path | 8 +- .../paths/Right Bite Score To Score.path | 2 +- .../pathplanner/paths/Right Corner Bite.path | 8 +- .../pathplanner/paths/Right NZ To Score.path | 20 ++--- .../paths/Right Score To NZ (F).path | 8 +- .../pathplanner/paths/Right Trench To NZ.path | 8 +- .../com/stuypulse/robot/RobotContainer.java | 30 ++++++- .../auton/regular/LeftTwoCornerVariant.java | 88 +++++++++++++++++++ .../auton/regular/RightTwoCornerVariant.java | 88 +++++++++++++++++++ .../robot/subsystems/intake/Intake.java | 2 +- .../subsystems/superstructure/hood/Hood.java | 2 +- .../superstructure/turret/Turret.java | 2 +- .../subsystems/vision/LimelightVision.java | 3 +- 19 files changed, 268 insertions(+), 65 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerVariant.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerVariant.java diff --git a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto index b2366e54..6fc6340e 100644 --- a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Left Bite Score To Score" + "pathName": "Left Score To Score" } }, { diff --git a/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path index 53c32926..358aa9d5 100644 --- a/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 7.564404708105102, + "x": 7.564404708105101, "y": 7.509029957203994 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite.path b/src/main/deploy/pathplanner/paths/Left Corner Bite.path index 272d23e2..c33f487b 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.843024251069899, - "y": 4.6366476462196875 + "x": 8.036133333333334, + "y": 4.62941111111111 }, "prevControl": { - "x": 7.493680456490727, - "y": 7.509029957203994 + "x": 7.539166666666667, + "y": 7.591722222222222 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 79ba0fc4..1f2d3ab9 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.843024251069899, - "y": 4.6366476462196875 + "x": 8.036133333333334, + "y": 4.62941111111111 }, "prevControl": null, "nextControl": { - "x": 6.43271041369472, - "y": 4.6495863052781745 + "x": 6.389322222222223, + "y": 4.580688888888889 }, "isLocked": false, "linkedName": "Left NZ" @@ -20,12 +20,12 @@ "y": 6.38336661911555 }, "prevControl": { - "x": 6.475815398217673, - "y": 5.767635657183343 + "x": 6.496511111111111, + "y": 5.428455555555555 }, "nextControl": { - "x": 6.587974322396576, - "y": 7.767803138373752 + "x": 6.530424413276881, + "y": 7.769832596935215 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index b2095fbf..a7cb3bb8 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.843024251069899, - "y": 4.6366476462196875 + "x": 8.036133333333334, + "y": 4.62941111111111 }, "prevControl": { - "x": 7.454864479315262, - "y": 7.211440798858774 + "x": 7.647973561578697, + "y": 7.204204263750197 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index b8d29067..da774b21 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -8,40 +8,40 @@ }, "prevControl": null, "nextControl": { - "x": 6.678544935805991, - "y": 7.560784593437946 + "x": 6.720633333333333, + "y": 7.572233333333333 }, "isLocked": false, "linkedName": "Left Corner" }, { "anchor": { - "x": 5.863409415121255, - "y": 5.244764621968616 + "x": 5.931333333333333, + "y": 5.399222222222222 }, "prevControl": { - "x": 5.962233970579355, - "y": 7.616553952963002 + "x": 5.950822222222223, + "y": 7.640444444444444 }, "nextControl": { - "x": 5.824593437945791, - "y": 4.31318116975749 + "x": 5.923225885345372, + "y": 4.466865703606724 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.0667047075606275, - "y": 4.533138373751783 + "x": 7.207855555555556, + "y": 4.9314888888888895 }, "prevControl": { - "x": 6.69639963069809, - "y": 4.256884232338165 + "x": 6.837550478693018, + "y": 4.655234747475271 }, "nextControl": { - "x": 7.708419221476936, - "y": 5.011868758915837 + "x": 7.849570069471864, + "y": 5.410219274052943 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index bbeaa4c0..2b6b9aa3 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.843024251069899, - "y": 4.6366476462196875 + "x": 8.036133333333334, + "y": 4.62941111111111 }, "prevControl": { - "x": 7.791269614835947, - "y": 7.457275320970044 + "x": 7.984378697099382, + "y": 7.450038785861467 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path index 7dba712c..a3ace306 100644 --- a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path @@ -57,7 +57,7 @@ }, "nextControl": { "x": 6.070427960057061, - "y": 0.1598716119828807 + "y": 0.15987161198288047 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite.path b/src/main/deploy/pathplanner/paths/Right Corner Bite.path index f672dfc5..5772cb4b 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.868901569186875, - "y": 3.472168330955778 + "x": 7.909455555555555, + "y": 3.508799999999999 }, "prevControl": { - "x": 7.648944365192582, - "y": 2.2041797432239663 + "x": 7.948433333333332, + "y": 2.2030444444444446 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index e3db961a..d6d029a1 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.868901569186875, - "y": 3.472168330955778 + "x": 7.909455555555555, + "y": 3.508799999999999 }, "prevControl": null, "nextControl": { - "x": 5.9927960057061345, - "y": 3.6144935805991443 + "x": 6.204177777777778, + "y": 3.4113555555555553 }, "isLocked": false, "linkedName": "Right NZ" @@ -20,12 +20,12 @@ "y": 1.4925534950071324 }, "prevControl": { - "x": 6.342788623034002, - "y": 2.5593197472095173 + "x": 6.44766178400938, + "y": 2.924425883014991 }, "nextControl": { - "x": 6.510342368045648, - "y": 0.23750356633380876 + "x": 6.399066666666666, + "y": 0.42955555555555525 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 0.5868473609129818 }, "prevControl": { - "x": 6.290385164051354, - "y": 0.6127246790299581 + "x": 6.311366666666668, + "y": 0.5562333333333322 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index d9dd7f97..9882af47 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.868901569186875, - "y": 3.472168330955778 + "x": 7.909455555555555, + "y": 3.508799999999999 }, "prevControl": { - "x": 7.817146932952921, - "y": 0.7421112696148362 + "x": 7.857700919321601, + "y": 0.7787429386590574 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 3aee608c..18200b70 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.868901569186875, - "y": 3.472168330955778 + "x": 7.909455555555555, + "y": 3.508799999999999 }, "prevControl": { - "x": 7.77741741933097, - "y": 0.4793297142410804 + "x": 7.81797140569965, + "y": 0.5159613832853016 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 8c1f097f..b2969dec 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -12,11 +12,13 @@ import com.stuypulse.robot.commands.auton.regular.LeftFollow; import com.stuypulse.robot.commands.auton.regular.LeftTwoCorner; import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerShallow; +import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; import com.stuypulse.robot.commands.auton.regular.RightBump; import com.stuypulse.robot.commands.auton.regular.RightFollow; import com.stuypulse.robot.commands.auton.regular.RightTwoCorner; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerShallow; +import com.stuypulse.robot.commands.auton.regular.RightTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.auton.test.BoxTest; import com.stuypulse.robot.commands.auton.test.EmptyTest; @@ -74,6 +76,7 @@ import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; import com.stuypulse.robot.subsystems.intake.Intake.RollerState; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; @@ -97,9 +100,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RepeatCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; @@ -199,7 +204,22 @@ private void configureButtonBindings() { // Shoot in place (TR) driver.getTopButton() - .whileTrue(new SuperstructureCacheState(driver)); + .whileTrue(new SuperstructureCacheState(driver) + .andThen(new WaitUntilCommand(superstructure::isReadyToShoot)) + .andThen( + Commands.parallel( + new IntakeDeploy(), + new RunCommand( + () -> handoff.setState(HandoffState.FORWARD), + handoff), + new RunCommand( + () -> spindexer.setState(SpindexerState.FORWARD), + spindexer) + ) + )) + .onFalse( + new SpindexerStop().alongWith(new HandoffStop()) + ); driver.getDPadLeft() .whileTrue(new IntakeTeleopDigest().repeatedly()) @@ -423,6 +443,14 @@ public void configureAutons() { "Right To Shallow", "Right Shallow To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CORNER_SHALLOW.register(autonChooser); + AutonConfig LEFT_TWO_CORNER_VARIANT = new AutonConfig("Left Two Corner Variant", LeftTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, + "Left Corner Bite", "Left NZ To Score", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + LEFT_TWO_CORNER_VARIANT.register(autonChooser); + + AutonConfig RIGHT_TWO_CORNER_VARIANT = new AutonConfig("Right Two Corner Variant", RightTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right Corner Bite", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + RIGHT_TWO_CORNER_VARIANT.register(autonChooser); + // FOLLOWS AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, "Left Follow To Bump", "Left Follow To Score", "Left Corner To Depot"); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerVariant.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerVariant.java new file mode 100644 index 00000000..0628093a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerVariant.java @@ -0,0 +1,88 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeDigest; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class LeftTwoCornerVariant extends SequentialCommandGroup { + + public LeftTwoCornerVariant(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerVariant.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerVariant.java new file mode 100644 index 00000000..57668d09 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerVariant.java @@ -0,0 +1,88 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeDigest; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class RightTwoCornerVariant extends SequentialCommandGroup { + + public RightTwoCornerVariant(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index cb0c1496..e0710c95 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -103,7 +103,7 @@ public void periodicAfterScheduler() { DogLog.log("Intake/Pivot State", getPivotState().toString()); DogLog.log("Intake/Roller State", getRollerState().toString()); - DogLog.log("Intake/Current Angle (deg)", getPivotAngle().getDegrees()); + DogLog.forceNt.log("Intake/Current Angle (deg)", getPivotAngle().getDegrees()); DogLog.log("Intake/Target Angle (deg)", getPivotState().getTargetAngle().getDegrees()); DogLog.log("Intake/Target Duty Cycle", getRollerState().getTargetDutyCycle()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 30dc518f..00a29c42 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -141,7 +141,7 @@ public void periodicAfterScheduler() { DogLog.log("Superstructure/Hood/State", state.name()); DogLog.log("Superstructure/Hood/Target Angle (deg)", getTargetAngle().getDegrees()); - DogLog.log("Superstructure/Hood/Current Angle (deg)", getAngle().getDegrees()); + DogLog.forceNt.log("Superstructure/Hood/Current Angle (deg)", getAngle().getDegrees()); if (Settings.DEBUG_MODE.get()) { if (EnabledSubsystems.HOOD.get()) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index b9866e4d..7f212c16 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -146,7 +146,7 @@ public void periodicAfterScheduler() { DogLog.log("Superstructure/Turret/State", state.name()); DogLog.log("Superstructure/Turret/Target Angle", getTargetAngle().getDegrees()); - DogLog.log("Superstructure/Turret/Current Angle", getAngle().getDegrees()); + DogLog.forceNt.log("Superstructure/Turret/Current Angle", getAngle().getDegrees()); if (Settings.DEBUG_MODE.get()) { if (EnabledSubsystems.TURRET.get()) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 39723015..8b286bf4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -312,9 +312,8 @@ public void periodicAfterScheduler() { // this is just the yaw of the internal imu DogLog.log("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); - //Rejection counters - Cameras.LimelightCameras[i].log(); } + Cameras.LimelightCameras[i].log(); } // Alternating pipelines for hdr From 1e86a9864d97a4bbe73e95e7cb4aa9bf6b160105 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 12 May 2026 12:34:43 -0400 Subject: [PATCH 26/97] feat: began writing the logic. tested through tuner and noticed that on tuner (need to see if this is actually the case) when you change the indices of the start and end, it doesnt update on the candle unless you disable. Started adding logic for that. --- .../robot/subsystems/leds/LEDController.java | 65 +++++++++++++++++-- 1 file changed, 59 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 54318f91..5560d273 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -7,6 +7,17 @@ package com.stuypulse.robot.subsystems.leds; +import com.ctre.phoenix6.configs.CANdleConfiguration; +import com.ctre.phoenix6.configs.CANdleFeaturesConfigs; +import com.ctre.phoenix6.configs.CustomParamsConfigs; +import com.ctre.phoenix6.configs.LEDConfigs; +import com.ctre.phoenix6.controls.EmptyAnimation; +import com.ctre.phoenix6.controls.SolidColor; +import com.ctre.phoenix6.hardware.CANdle; +import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue; +import com.ctre.phoenix6.signals.RGBWColor; +import com.ctre.phoenix6.signals.StatusLedWhenActiveValue; +import com.ctre.phoenix6.signals.StripTypeValue; import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -15,6 +26,7 @@ import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDController extends SubsystemBase { @@ -32,33 +44,74 @@ public static LEDController getInstance() { private AddressableLED leds; private AddressableLEDBuffer ledsBuffer; + private final CANdle candle; + private CANdleConfiguration candleConfigs; + private RGBWColor candleColor; + + private int startingIndex = 0; + private int endIndex = 7; + private boolean indicesChanged = false; + private final LEDPattern defaultPattern = LEDPattern.kOff; - protected LEDController(int port, int length) { - leds = new AddressableLED(port); - ledsBuffer = new AddressableLEDBuffer(length); + protected LEDController(int ledPort, int ledLength) { // TODO: add brightness of CANdle to the constructor + leds = new AddressableLED(ledPort); + ledsBuffer = new AddressableLEDBuffer(ledLength); - leds.setLength(length); + leds.setLength(ledLength); leds.setData(ledsBuffer); leds.start(); applyPattern(defaultPattern); SmartDashboard.putData(instance); + + candle = new CANdle(null, Ports.CANIVORE); // TODO: update ports value + + candleConfigs = new CANdleConfiguration() + .withLED( + new LEDConfigs() + .withBrightnessScalar(0.7) + .withStripType(StripTypeValue.RGB) + .withLossOfSignalBehavior(LossOfSignalBehaviorValue.DisableLEDs)) + + .withCANdleFeatures( + new CANdleFeaturesConfigs().withStatusLedWhenActive(StatusLedWhenActiveValue.Disabled)); + + candle.getConfigurator().apply(candleConfigs); } public void applyPattern(LEDPattern pattern) { pattern.applyTo(ledsBuffer); } + public void changeCandleIndices(int start, int end) { + this.startingIndex = start; + this.endIndex = end; + + indicesChanged = true; + } + public void periodicAfterScheduler() { if (RobotContainer.EnabledSubsystems.LEDS.get()) { // leds.start(); leds.setData(ledsBuffer); - } - else { + + candleColor = new RGBWColor( + new Color8Bit( + ledsBuffer.getLED(1).toHexString())); + + if (indicesChanged) { + //TODO: add logic that turns the candle on and off. Use output current amperage (if it is 0.01 A or lower) to determine if it fully turned off. + } + + candle.setControl(new SolidColor(startingIndex, endIndex).withColor(candleColor)); + + } else { LEDPattern.kOff.applyTo(ledsBuffer); leds.setData(ledsBuffer); + + //candle.setControl(new EmptyAnimation(0)); } // SmartDashboard.putString("Leds/Color", ledsBuffer.getLED(1).toString()); } From b04544582023c44450df478c3f6d3cdca60d9187 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 13 May 2026 17:47:31 -0400 Subject: [PATCH 27/97] feat: change to use control request and make candle actually control it hopefully --- .../com/stuypulse/robot/RobotContainer.java | 5 +- .../robot/commands/leds/LEDApplyPattern.java | 8 +- .../commands/leds/LEDDefaultCommand.java | 6 +- .../com/stuypulse/robot/constants/Ports.java | 1 + .../stuypulse/robot/constants/Settings.java | 67 +++++++++-------- .../robot/subsystems/leds/LEDController.java | 73 +++++-------------- 6 files changed, 67 insertions(+), 93 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index b2969dec..8b9bf755 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -78,6 +78,7 @@ import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.intake.Intake.PivotState; import com.stuypulse.robot.subsystems.intake.Intake.RollerState; +import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -142,7 +143,7 @@ public interface EnabledSubsystems { private final Shooter shooter = Shooter.getInstance(); private final Hood hood = Hood.getInstance(); - // private final LEDController leds = LEDController.getInstance(); + private final LEDController leds = LEDController.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); @@ -564,7 +565,7 @@ public void periodicAfterScheduler() { handoff.periodicAfterScheduler(); intake.periodicAfterScheduler(); - // leds.periodicAfterScheduler(); TODO: ADD THESE BACK TY + leds.periodicAfterScheduler(); spindexer.periodicAfterScheduler(); hood.periodicAfterScheduler(); shooter.periodicAfterScheduler(); diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java index 01a4ae3d..ee23e622 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java @@ -11,23 +11,25 @@ import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj2.command.Command; +import java.util.ResourceBundle.Control; import java.util.function.Supplier; +import com.ctre.phoenix6.controls.ControlRequest; import com.stuypulse.robot.subsystems.leds.LEDController; public class LEDApplyPattern extends Command { protected final LEDController leds; - protected final Supplier pattern; + protected final Supplier pattern; - public LEDApplyPattern(Supplier pattern) { + public LEDApplyPattern(Supplier pattern) { leds = LEDController.getInstance(); this.pattern = pattern; addRequirements(leds); } - public LEDApplyPattern(LEDPattern pattern) { + public LEDApplyPattern(ControlRequest pattern) { this(() -> pattern); } diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java index 08192002..708cae3a 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java @@ -65,10 +65,10 @@ public void execute() { if (Robot.getMode() == RobotMode.DISABLED) { if (LimelightVision.getInstance().getMaxTagCount() >= Settings.LED.DESIRED_TAGS_WHEN_DISABLED) { leds.applyPattern(Settings.LED.DISABLED_ALIGNED); - state = "DISABLED_ALLOWED"; + state = "DISABLED_ALIGNED"; } else { - leds.applyPattern(LEDPattern.solid(Color.kRed)); + leds.applyPattern(Settings.LED.DISABLED); state = "DISABLED_DISALLOWED"; } } @@ -117,7 +117,7 @@ else if (intake.getPivotState() == PivotState.DEPLOY) { state = "INTAKE_DEPLOYED"; } else { - leds.applyPattern(LEDPattern.solid(Color.kRed)); + leds.applyPattern(Settings.LED.DISABLED); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 4d4e11c9..12ca7647 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -18,6 +18,7 @@ public interface Gamepad { public interface LED { int LED_PORT = 1; + int CANDLE_PORT = 61; } public interface ClimberHopper { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 5bc284dd..480a8b43 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -6,10 +6,15 @@ package com.stuypulse.robot.constants; import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.RainbowAnimation; +import com.ctre.phoenix6.controls.SolidColor; +import com.ctre.phoenix6.signals.RGBWColor; import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; +import edu.wpi.first.hal.LEDJNI; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; @@ -62,7 +67,7 @@ public interface Handoff { } public interface Intake { - Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(71.0); + Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(71.0); Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(-10.0); Rotation2d PIVOT_DIGEST_ANGLE = Rotation2d.fromDegrees(30); @@ -168,13 +173,9 @@ public interface FerryRPMInterpolation { {5.16, 3300.0}, {6.94, 3600.0}, {7.87, 3800.0}, - {9.77, 4300.0}, - {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS EXTRAPOLATED!!! - {11.516, 4900.0} - // {11.516, 5200.0}, - // {12.416, 5500.0}, // AFTER OPP ALLIANCE ZONE, RPM SHOULD BE AT 5500 -blay - // {13.316, 5500.0}, - // {14.216, 5600.0} + // {9.77, 4300.0}, //TODO: ADD DATA BACK IN COMP + // {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS EXTRAPOLATED!!! + // {11.516, 4900.0} }; } @@ -366,42 +367,44 @@ public interface Tolerances { public interface LED { - LEDPattern PASSING_TRENCH = LEDPattern.solid(Color.kRed); - LEDPattern IS_BEHIND_HUB = LEDPattern.solid(Color.kRed); + public final int LED_LENGTH = 0 + 8; //CANdle already has 8 + SolidColor BASE_SOLID_COLOR_REQUEST = new SolidColor(0, LED_LENGTH - 1); + SolidColor PASSING_TRENCH = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kRed)); + SolidColor IS_BEHIND_HUB = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kRed)); - // LEDPattern CLIMB_ALIGNING = LEDPattern.solid(Color.kYellow); - // LEDPattern CLIMB_ALIGNED = LEDPattern.solid(Color.kGreen); - // LEDPattern CLIMBING = LEDPattern.solid(Color.kRed); + // SolidColor CLIMB_ALIGNING = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kYellow); + // SolidColor CLIMB_ALIGNED = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kGreen); + // SolidColor CLIMBING = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kRed); - LEDPattern TURRET_WRAPPING = LEDPattern.solid(Color.kRed); - LEDPattern LEFT_WARNING = LEDPattern.solid(Color.kBlack); // TBD - LEDPattern RIGHT_WARNING = LEDPattern.solid(Color.kBlack); // TBD + SolidColor TURRET_WRAPPING = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kRed)); + SolidColor LEFT_WARNING = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kBlack)); // TBD + SolidColor RIGHT_WARNING = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kBlack)); // TBD - LEDPattern SHOOT_IN_PLACE = LEDPattern.solid(Color.kPurple); + SolidColor SHOOT_IN_PLACE = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kPurple)); - LEDPattern SOTM_ON = LEDPattern.solid(Color.kCyan); - LEDPattern FOTM_ON = LEDPattern.rainbow(255, 128).scrollAtAbsoluteSpeed(MetersPerSecond.of(1), Meters.of(1 / 120.0)); + SolidColor SOTM_ON = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kCyan)); + RainbowAnimation FOTM_ON = new RainbowAnimation(0, LED_LENGTH - 1).withFrameRate(60); - LEDPattern LEFT_CORNER = LEDPattern.solid(Color.kPurple); - LEDPattern RIGHT_CORNER = LEDPattern.solid(Color.kBlue); + SolidColor LEFT_CORNER = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kPurple)); + SolidColor RIGHT_CORNER = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kBlue)); - LEDPattern KB_DISTANCE = LEDPattern.solid(Color.kPink); + SolidColor KB_DISTANCE = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kPink)); - LEDPattern REVERSE = LEDPattern.solid(Color.kWhite); - LEDPattern STOP_ROLLERS = LEDPattern.solid(Color.kYellow); + SolidColor REVERSE = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kWhite)); + SolidColor STOP_ROLLERS = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kYellow)); - LEDPattern RESET_HEADING = LEDPattern.solid(Color.kYellow); - LEDPattern X_WHEELS = LEDPattern.solid(Color.kRed); + SolidColor RESET_HEADING = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kYellow)); + SolidColor X_WHEELS = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kRed)); - LEDPattern INTAKE_STOW = LEDPattern.solid(Color.kBrown); //broken - LEDPattern INTAKE_DEPLOYED = LEDPattern.solid(Color.kOrange); //broken + SolidColor INTAKE_STOW = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kBrown)); //broken + SolidColor INTAKE_DEPLOYED = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kOrange)); //broken - LEDPattern DISABLED_ALIGNED = LEDPattern.solid(Color.kGreen); - // LEDPattern.gradient(GradientType.kDiscontinuous, Color.kRed, Color.kWhite).scrollAtRelativeSpeed(Percent.per(Second).of(25)); + SolidColor DISABLED_ALIGNED = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kGreen)); + SolidColor DISABLED = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kRed)); - public final int DESIRED_TAGS_WHEN_DISABLED = 2; - public final int LED_LENGTH = 9; // TBA + // SolidColor.gradient(GradientType.kDiscontinuous, Color.kRed, Color.kWhite).scrollAtRelativeSpeed(Percent.per(Second).of(25)); + public final int DESIRED_TAGS_WHEN_DISABLED = 2; } public interface Vision { diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 5560d273..026e0633 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -1,4 +1,3 @@ - /************************ PROJECT MARY *************************/ /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ /* Use of this source code is governed by an MIT-style license */ @@ -7,11 +6,15 @@ package com.stuypulse.robot.subsystems.leds; +import java.util.Optional; + import com.ctre.phoenix6.configs.CANdleConfiguration; import com.ctre.phoenix6.configs.CANdleFeaturesConfigs; import com.ctre.phoenix6.configs.CustomParamsConfigs; import com.ctre.phoenix6.configs.LEDConfigs; +import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.EmptyAnimation; +import com.ctre.phoenix6.controls.SingleFadeAnimation; import com.ctre.phoenix6.controls.SolidColor; import com.ctre.phoenix6.hardware.CANdle; import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue; @@ -22,11 +25,9 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color8Bit; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDController extends SubsystemBase { @@ -34,39 +35,28 @@ public class LEDController extends SubsystemBase { private final static LEDController instance; static { - instance = new LEDController(Ports.LED.LED_PORT, Settings.LED.LED_LENGTH); + instance = new LEDController(); } public static LEDController getInstance() { return instance; } - private AddressableLED leds; - private AddressableLEDBuffer ledsBuffer; - - private final CANdle candle; - private CANdleConfiguration candleConfigs; - private RGBWColor candleColor; private int startingIndex = 0; - private int endIndex = 7; - private boolean indicesChanged = false; + private int endIndex = null; //TODO: calculate the total - private final LEDPattern defaultPattern = LEDPattern.kOff; - protected LEDController(int ledPort, int ledLength) { // TODO: add brightness of CANdle to the constructor - leds = new AddressableLED(ledPort); - ledsBuffer = new AddressableLEDBuffer(ledLength); - - leds.setLength(ledLength); - leds.setData(ledsBuffer); - leds.start(); - - applyPattern(defaultPattern); + private final CANdle leds; + private CANdleConfiguration candleConfigs; + private ControlRequest ledPattern = new SolidColor(startingIndex, endIndex).withColor(RGBWColor.fromHSV(0, 0, 100)); - SmartDashboard.putData(instance); + public void applyPattern(ControlRequest ledPattern) { + this.ledPattern = ledPattern; + } - candle = new CANdle(null, Ports.CANIVORE); // TODO: update ports value + private LEDController() { + leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.RIO); // TODO: update ports value candleConfigs = new CANdleConfiguration() .withLED( @@ -76,43 +66,20 @@ protected LEDController(int ledPort, int ledLength) { // TODO: add brightness of .withLossOfSignalBehavior(LossOfSignalBehaviorValue.DisableLEDs)) .withCANdleFeatures( - new CANdleFeaturesConfigs().withStatusLedWhenActive(StatusLedWhenActiveValue.Disabled)); + new CANdleFeaturesConfigs().withStatusLedWhenActive(StatusLedWhenActiveValue.Enabled)); - candle.getConfigurator().apply(candleConfigs); - } - - public void applyPattern(LEDPattern pattern) { - pattern.applyTo(ledsBuffer); - } + leds.getConfigurator().apply(candleConfigs); - public void changeCandleIndices(int start, int end) { - this.startingIndex = start; - this.endIndex = end; + leds.setControl(ledPattern); - indicesChanged = true; } public void periodicAfterScheduler() { if (RobotContainer.EnabledSubsystems.LEDS.get()) { - // leds.start(); - leds.setData(ledsBuffer); - - candleColor = new RGBWColor( - new Color8Bit( - ledsBuffer.getLED(1).toHexString())); - - if (indicesChanged) { - //TODO: add logic that turns the candle on and off. Use output current amperage (if it is 0.01 A or lower) to determine if it fully turned off. - } - - candle.setControl(new SolidColor(startingIndex, endIndex).withColor(candleColor)); + leds.setControl(ledPattern); } else { - LEDPattern.kOff.applyTo(ledsBuffer); - leds.setData(ledsBuffer); - - //candle.setControl(new EmptyAnimation(0)); + leds.clearAllAnimations(); } - // SmartDashboard.putString("Leds/Color", ledsBuffer.getLED(1).toString()); } } \ No newline at end of file From 429ab2defefb6623020f15f0c3f4e311c42bdc7b Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 18 May 2026 17:42:14 -0400 Subject: [PATCH 28/97] feat: working leds using just default command --- .../com/stuypulse/robot/RobotContainer.java | 23 +++++---- .../robot/commands/leds/LEDApplyPattern.java | 13 ++--- .../commands/leds/LEDDefaultCommand.java | 12 ++--- .../stuypulse/robot/constants/Settings.java | 50 ++++++++++--------- .../robot/subsystems/leds/LEDController.java | 30 +++++------ 5 files changed, 64 insertions(+), 64 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 8b9bf755..df7cc990 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -40,6 +40,7 @@ import com.stuypulse.robot.commands.intake.SeedPivotDeployed; import com.stuypulse.robot.commands.intake.SeedPivotStowed; import com.stuypulse.robot.commands.leds.LEDApplyPattern; +import com.stuypulse.robot.commands.leds.LEDDefaultCommand; import com.stuypulse.robot.commands.spindexer.SpindexerReverse; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; @@ -172,7 +173,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - // leds.setDefaultCommand(new LEDDefaultCommand()); + leds.setDefaultCommand(new LEDDefaultCommand()); } /***************/ @@ -232,31 +233,31 @@ private void configureButtonBindings() { // Intake Deploy driver.getRightTriggerButton() - .onTrue(new LEDApplyPattern(Settings.LED.INTAKE_DEPLOYED)) + // .onTrue(new LEDApplyPattern(Settings.LED.INTAKE_DEPLOYED)) .onTrue(new IntakeDeploy()); // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()) .onTrue(new ResetLimelightIMU()) - .onTrue(new LEDApplyPattern(Settings.LED.RESET_HEADING)) + // .onTrue(new LEDApplyPattern(Settings.LED.RESET_HEADING)) .onFalse(new SetIMUMode(0)); // Stop Rollers driver.getLeftBumper() - .onTrue(new LEDApplyPattern(Settings.LED.STOP_ROLLERS)) + // .onTrue(new LEDApplyPattern(Settings.LED.STOP_ROLLERS)) .onTrue(new IntakeDeploy() .andThen(new IntakeStopRollers())); // Outtake driver.getRightBumper() - .whileTrue(new LEDApplyPattern(Settings.LED.REVERSE)) + // .whileTrue(new LEDApplyPattern(Settings.LED.REVERSE)) .whileTrue(new IntakeOuttake()) .onFalse(new IntakeRunRollers()); // SOTM (BR) driver.getRightMenuButton() - .onTrue(new LEDApplyPattern(Settings.LED.SOTM_ON)) + // .onTrue(new LEDApplyPattern(Settings.LED.SOTM_ON)) .onTrue(new WaitUntilCommand(() -> spindexer.getState() == SpindexerState.FORWARD) .andThen(new WaitCommand(0.75).andThen(new IntakeDeploy()))) .whileTrue(new RepeatCommand(new BuzzController(driver).onlyWhile(() -> !vision.hasData() && superstructure.getState() == SuperstructureState.SOTM))) @@ -277,7 +278,7 @@ private void configureButtonBindings() { // FOTM (BL) driver.getLeftMenuButton() - .onTrue(new LEDApplyPattern(Settings.LED.FOTM_ON)) + // .onTrue(new LEDApplyPattern(Settings.LED.FOTM_ON)) .onTrue(new IntakeRunRollers()) .onTrue(new ConditionalCommand( new ParallelCommandGroup( @@ -295,8 +296,8 @@ private void configureButtonBindings() { )); driver.getDPadDown() - .whileTrue(new SwerveXMode()) - .onTrue(new LEDApplyPattern(Settings.LED.X_WHEELS)); + .whileTrue(new SwerveXMode()); + // .onTrue(new LEDApplyPattern(Settings.LED.X_WHEELS)); // Reset (TL) driver.getDPadRight() @@ -320,7 +321,7 @@ private void configureButtonBindings() { // Manual Right Corner Scoring driver.getRightButton() - .whileTrue(new LEDApplyPattern(Settings.LED.RIGHT_CORNER)) + // .whileTrue(new LEDApplyPattern(Settings.LED.RIGHT_CORNER)) .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) .onTrue(new SwerveResetPoseRightCorner()) @@ -331,7 +332,7 @@ private void configureButtonBindings() { // Manual KB Distance Scoring driver.getBottomButton() - .whileTrue(new LEDApplyPattern(Settings.LED.KB_DISTANCE)) + // .whileTrue(new LEDApplyPattern(Settings.LED.KB_DISTANCE)) .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) .onTrue(new SwerveResetPoseKBShot()) diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java index ee23e622..9cc60bc6 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import java.util.ResourceBundle.Control; import java.util.function.Supplier; @@ -17,25 +18,21 @@ import com.ctre.phoenix6.controls.ControlRequest; import com.stuypulse.robot.subsystems.leds.LEDController; -public class LEDApplyPattern extends Command { +public class LEDApplyPattern extends InstantCommand { protected final LEDController leds; - protected final Supplier pattern; + protected final ControlRequest pattern; - public LEDApplyPattern(Supplier pattern) { + public LEDApplyPattern(ControlRequest pattern) { leds = LEDController.getInstance(); this.pattern = pattern; addRequirements(leds); } - public LEDApplyPattern(ControlRequest pattern) { - this(() -> pattern); - } - @Override public void execute() { - leds.applyPattern(pattern.get()); + leds.applyPattern(pattern); } } diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java index 708cae3a..4e0189a0 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java @@ -7,6 +7,8 @@ package com.stuypulse.robot.commands.leds; +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.SolidColor; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; import com.stuypulse.robot.constants.Settings; @@ -27,11 +29,10 @@ import com.stuypulse.robot.subsystems.vision.LimelightVision; import dev.doglog.DogLog; -import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class LEDDefaultCommand extends Command{ +public class LEDDefaultCommand extends InstantCommand{ private final LEDController leds; private final CommandSwerveDrivetrain swerve; private final Handoff handoff; @@ -59,7 +60,7 @@ public LEDDefaultCommand() { } @Override - public void execute() { + public void initialize() { String state = "NONE"; if (Robot.getMode() == RobotMode.DISABLED) { @@ -116,9 +117,6 @@ else if (intake.getPivotState() == PivotState.DEPLOY) { leds.applyPattern(Settings.LED.INTAKE_DEPLOYED); state = "INTAKE_DEPLOYED"; } - else { - leds.applyPattern(Settings.LED.DISABLED); - } } DogLog.log("Leds/State", state); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 480a8b43..e9780677 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -366,41 +366,43 @@ public interface Tolerances { } public interface LED { + public static SolidColor solidColor(Color color) { + return new SolidColor(0, LED_LENGTH - 1).withColor(new RGBWColor(color)); + } - public final int LED_LENGTH = 0 + 8; //CANdle already has 8 - SolidColor BASE_SOLID_COLOR_REQUEST = new SolidColor(0, LED_LENGTH - 1); - SolidColor PASSING_TRENCH = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kRed)); - SolidColor IS_BEHIND_HUB = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kRed)); + public final int LED_LENGTH = 8 + 21; //CANdle already has 8 + SolidColor PASSING_TRENCH = solidColor(Color.kRed); + SolidColor IS_BEHIND_HUB = solidColor(Color.kRed); - // SolidColor CLIMB_ALIGNING = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kYellow); - // SolidColor CLIMB_ALIGNED = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kGreen); - // SolidColor CLIMBING = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kRed); + // SolidColor CLIMB_ALIGNING = solidColor(Color.kYellow); + // SolidColor CLIMB_ALIGNED = solidColor(Color.kGreen); + // SolidColor CLIMBING = solidColor(Color.kRed); - SolidColor TURRET_WRAPPING = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kRed)); - SolidColor LEFT_WARNING = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kBlack)); // TBD - SolidColor RIGHT_WARNING = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kBlack)); // TBD + SolidColor TURRET_WRAPPING = solidColor(Color.kRed); + SolidColor LEFT_WARNING = solidColor(Color.kBlack); // TBD + SolidColor RIGHT_WARNING = solidColor(Color.kBlack); // TBD - SolidColor SHOOT_IN_PLACE = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kPurple)); + SolidColor SHOOT_IN_PLACE = solidColor(Color.kPurple); - SolidColor SOTM_ON = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kCyan)); - RainbowAnimation FOTM_ON = new RainbowAnimation(0, LED_LENGTH - 1).withFrameRate(60); + SolidColor SOTM_ON = solidColor(Color.kCyan); + RainbowAnimation FOTM_ON = new RainbowAnimation(0, LED_LENGTH - 1).withFrameRate(60).withSlot(0); - SolidColor LEFT_CORNER = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kPurple)); - SolidColor RIGHT_CORNER = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kBlue)); + SolidColor LEFT_CORNER = solidColor(Color.kPurple); + SolidColor RIGHT_CORNER = solidColor(Color.kBlue); - SolidColor KB_DISTANCE = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kPink)); + SolidColor KB_DISTANCE = solidColor(Color.kPink); - SolidColor REVERSE = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kWhite)); - SolidColor STOP_ROLLERS = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kYellow)); + SolidColor REVERSE = solidColor(Color.kWhite); + SolidColor STOP_ROLLERS = solidColor(Color.kYellow); - SolidColor RESET_HEADING = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kYellow)); - SolidColor X_WHEELS = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kRed)); + SolidColor RESET_HEADING = solidColor(Color.kYellow); + SolidColor X_WHEELS = solidColor(Color.kRed); - SolidColor INTAKE_STOW = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kBrown)); //broken - SolidColor INTAKE_DEPLOYED = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kOrange)); //broken + SolidColor INTAKE_STOW = solidColor(Color.kBrown); //broken + SolidColor INTAKE_DEPLOYED = solidColor(Color.kOrange); //broken - SolidColor DISABLED_ALIGNED = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kGreen)); - SolidColor DISABLED = BASE_SOLID_COLOR_REQUEST.withColor(new RGBWColor(Color.kRed)); + SolidColor DISABLED_ALIGNED = solidColor(Color.kGreen); + SolidColor DISABLED = solidColor(Color.kRed); // SolidColor.gradient(GradientType.kDiscontinuous, Color.kRed, Color.kWhite).scrollAtRelativeSpeed(Percent.per(Second).of(25)); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 026e0633..51477596 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -42,28 +42,27 @@ public static LEDController getInstance() { return instance; } - - private int startingIndex = 0; - private int endIndex = null; //TODO: calculate the total - - private final CANdle leds; private CANdleConfiguration candleConfigs; - private ControlRequest ledPattern = new SolidColor(startingIndex, endIndex).withColor(RGBWColor.fromHSV(0, 0, 100)); + private ControlRequest ledPattern = Settings.LED.DISABLED; + private boolean isChanged; public void applyPattern(ControlRequest ledPattern) { - this.ledPattern = ledPattern; + if(this.ledPattern != ledPattern) { + this.ledPattern = ledPattern; + isChanged = true; + } } private LEDController() { - leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.RIO); // TODO: update ports value + leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); // TODO: update ports value candleConfigs = new CANdleConfiguration() .withLED( new LEDConfigs() - .withBrightnessScalar(0.7) - .withStripType(StripTypeValue.RGB) - .withLossOfSignalBehavior(LossOfSignalBehaviorValue.DisableLEDs)) + .withBrightnessScalar(1.0) + .withStripType(StripTypeValue.GRB) + .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)) .withCANdleFeatures( new CANdleFeaturesConfigs().withStatusLedWhenActive(StatusLedWhenActiveValue.Enabled)); @@ -71,13 +70,16 @@ private LEDController() { leds.getConfigurator().apply(candleConfigs); leds.setControl(ledPattern); - + isChanged = true; } public void periodicAfterScheduler() { if (RobotContainer.EnabledSubsystems.LEDS.get()) { - leds.setControl(ledPattern); - + if(isChanged) { + leds.clearAllAnimations(); + leds.setControl(ledPattern); + isChanged = false; + } } else { leds.clearAllAnimations(); } From a84448be4c8f69a11928ea18b437549bafe95f36 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 19 May 2026 15:45:04 -0400 Subject: [PATCH 29/97] fix: states for leds --- .../commands/leds/LEDDefaultCommand.java | 2 +- .../robot/subsystems/leds/LEDController.java | 83 ++++++++++++++++++- 2 files changed, 81 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java index 4e0189a0..6f1f0583 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java @@ -73,7 +73,7 @@ public void initialize() { state = "DISABLED_DISALLOWED"; } } - + else { if (swerve.isUnderTrench()) { leds.applyPattern(Settings.LED.PASSING_TRENCH); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 51477596..378b3d03 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -8,12 +8,14 @@ import java.util.Optional; + import com.ctre.phoenix6.configs.CANdleConfiguration; import com.ctre.phoenix6.configs.CANdleFeaturesConfigs; import com.ctre.phoenix6.configs.CustomParamsConfigs; import com.ctre.phoenix6.configs.LEDConfigs; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.EmptyAnimation; +import com.ctre.phoenix6.controls.RainbowAnimation; import com.ctre.phoenix6.controls.SingleFadeAnimation; import com.ctre.phoenix6.controls.SolidColor; import com.ctre.phoenix6.hardware.CANdle; @@ -25,12 +27,15 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDController extends SubsystemBase { + private SolidColor solidColorRequest = new SolidColor(0, Settings.LED.LED_LENGTH - 1).withColor(new RGBWColor(Color.kRed)); + private RainbowAnimation rainbowRequest = new RainbowAnimation(0, Settings.LED.LED_LENGTH - 1).withFrameRate(60).withSlot(0); private final static LEDController instance; @@ -47,13 +52,81 @@ public static LEDController getInstance() { private ControlRequest ledPattern = Settings.LED.DISABLED; private boolean isChanged; + //different portions of the LED should be a different color to indicate whether certain limelights are dead + //add the flashing aspect based on if we don't see a tag (with debounce) + // one way to go further with the flashing aspect is make it flash faster over DISTANCE (since last tag was seen) rather than time + + public enum LEDSTATE { + PASSING_TRENCH, + IS_BEHIND_HUB, + TURRET_WRAPPING, + LEFT_WARNING, + RIGHT_WARNING, + SHOOT_IN_PLACE, + SOTM_ON, + FOTM_ON, + LEFT_CORNER, + RIGHT_CORNER, + KB_DISTANCE, + REVERSE, + STOP_ROLLERS, + RESET_HEADING, + X_WHEELS, + INTAKE_STOW, + INTAKE_DEPLOYED, + DISABLED_ALIGNED, + DISABLED + } + + private LEDSTATE state = LEDSTATE.DISABLED; + private LEDSTATE cachedState = LEDSTATE.DISABLED; + + + public ControlRequest stateToPattern(LEDSTATE state) { + return switch (state) { + case PASSING_TRENCH -> solidColorRequest.withColor(new RGBWColor(Color.kRed)); + case IS_BEHIND_HUB -> solidColorRequest.withColor(new RGBWColor(Color.kRed)); + case TURRET_WRAPPING -> solidColorRequest.withColor(new RGBWColor(Color.kRed)); + case LEFT_WARNING -> solidColorRequest.withColor(new RGBWColor(Color.kBlack)); + case RIGHT_WARNING -> solidColorRequest.withColor(new RGBWColor(Color.kBlack)); + case SHOOT_IN_PLACE -> solidColorRequest.withColor(new RGBWColor(Color.kPurple)); + case SOTM_ON -> solidColorRequest.withColor(new RGBWColor(Color.kCyan)); + + case FOTM_ON -> rainbowRequest; //rainbow animation -> need to add cached states and a change pattern type method -> maybe apply pattern should take in the pattern and the color/frequency -> then i would need a system like the status signal where we just call them once and mutate them after + + case LEFT_CORNER -> solidColorRequest.withColor(new RGBWColor(Color.kPurple)); + case RIGHT_CORNER -> solidColorRequest.withColor(new RGBWColor(Color.kBlue)); + case KB_DISTANCE -> solidColorRequest.withColor(new RGBWColor(Color.kPink)); + case REVERSE -> solidColorRequest.withColor(new RGBWColor(Color.kWhite)); + case STOP_ROLLERS -> solidColorRequest.withColor(new RGBWColor(Color.kYellow)); + case RESET_HEADING -> solidColorRequest.withColor(new RGBWColor(Color.kYellow)); + case X_WHEELS -> solidColorRequest.withColor(new RGBWColor(Color.kRed)); + case INTAKE_STOW -> solidColorRequest.withColor(new RGBWColor(Color.kBrown)); + case INTAKE_DEPLOYED -> solidColorRequest.withColor(new RGBWColor(Color.kOrange)); + case DISABLED_ALIGNED -> solidColorRequest.withColor(new RGBWColor(Color.kGreen)); + case DISABLED -> solidColorRequest.withColor(new RGBWColor(Color.kRed)); + }; + + //CHANGE apply pattern command to change state + } + public void applyPattern(ControlRequest ledPattern) { - if(this.ledPattern != ledPattern) { - this.ledPattern = ledPattern; - isChanged = true; + if (cachedState != state) { + if (stateToPattern(cachedState) != stateToPattern(state)) { + this.ledPattern = stateToPattern(state); + } + + else if (stateToPattern(state) instanceof SolidColor){ + SolidColor.class.cast(ledPattern).withColor(null); //UPDATTTEEE + } + cachedState = state; } } + public void changeState(LEDSTATE state) { + this.state = state; + } + private LEDController() { leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); // TODO: update ports value @@ -83,5 +156,9 @@ public void periodicAfterScheduler() { } else { leds.clearAllAnimations(); } + + DogLog.log("LED/Pattern Name", ledPattern.getName()); + DogLog.log("LED/State", state.toString()); + } } \ No newline at end of file From 5bc1ea50a02342b4bed81afee2f955c536c23d53 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 20 May 2026 14:51:13 -0400 Subject: [PATCH 30/97] feat: CODE IS CLEAN and TESTED. Need to make pull request. Need one more modification which is to make manually applied states take priority over default command. --- AdvantageScope 5-20-2026.json | 1556 +++++++++++++++++ .../com/stuypulse/robot/RobotContainer.java | 13 +- ...EDApplyPattern.java => LEDApplyState.java} | 13 +- .../commands/leds/LEDDefaultCommand.java | 41 +- .../stuypulse/robot/constants/Settings.java | 51 +- .../robot/subsystems/leds/LEDController.java | 121 +- 6 files changed, 1670 insertions(+), 125 deletions(-) create mode 100644 AdvantageScope 5-20-2026.json rename src/main/java/com/stuypulse/robot/commands/leds/{LEDApplyPattern.java => LEDApplyState.java} (63%) diff --git a/AdvantageScope 5-20-2026.json b/AdvantageScope 5-20-2026.json new file mode 100644 index 00000000..dc42e2d3 --- /dev/null +++ b/AdvantageScope 5-20-2026.json @@ -0,0 +1,1556 @@ +{ + "hubs": [ + { + "x": -8, + "y": -8, + "width": 1552, + "height": 928, + "state": { + "sidebar": { + "width": 304, + "expanded": [ + "/SmartDashboard/Turret", + "/NT/SmartDashboard/Robot/Scheduled Commands/Names", + "/NT/Robot Pose/translation", + "/SmartDashboard/EnergyLogger", + "/SmartDashboard/EnergyLogger/Supply Current Amps", + "/SmartDashboard/EnergyLogger/Energy watt hours", + "/NT/SmartDashboard/Swerve/Modules/Module 3", + "/NT/SmartDashboard/Swerve/Modules/Module 2", + "/NT/SmartDashboard/Swerve/Modules/Module 0", + "/NT/SmartDashboard/Swerve/Modules/Module 1", + "/SmartDashboard/SuperStructure", + "/NT/SmartDashboard/SuperStructure/Turret", + "/NT/SmartDashboard/SuperStructure", + "/SmartDashboard/Intake/Pivot", + "/SmartDashboard/Superstructure/Shooter", + "/SmartDashboard/Roobt", + "/SmartDashboard/Swerve/SOTM", + "/SmartDashboard/Superstructure", + "/Robot Pose", + "/Robot Pose/rotation", + "/SmartDashboard/EnergyUtil", + "/SmartDashboard/EnergyUtil/Supply Current Amps", + "/SmartDashboard/Superstructure/Hood", + "/SmartDashboard/FieldPositions", + "/FieldPositions", + "/SmartDashboard/Spindexer", + "/SmartDashboard/Spindexer/Should Stop", + "/SmartDashboard/Handoff", + "/SmartDashboard/Robot/CAN", + "/SmartDashboard/Robot/CAN/Main", + "/SmartDashboard/Swerve", + "/NT/SmartDashboard/Robot/Scheduled Commands", + "/NT/SmartDashboard/Handoff/Should Stop", + "/NT/Robot Pose/rotation", + "/NT/SmartDashboard/Robot", + "/NT/PathPlanner/currentPose", + "/NTConnection", + "/SmartDashboard/Vision", + "/SmartDashboard/Vision/limelight-back", + "/NT/limelight-left/hw", + "/NT/limelight-right/hw", + "/NT/limelight-back/hw", + "/NT/LiveWindow/.status", + "/NT/CameraPublisher/limelight-left/streams", + "/Phoenix6", + "/DSLog", + "/Log1", + "/Log1/DSLog", + "/Log1/DSLog/Status", + "/NT/SmartDashboard", + "/NT", + "/Phoenix6/TalonFX-20", + "/NT/FMSInfo", + "/NT/SmartDashboard/EnergyUtil/Supply Current Amps", + "/NT/SmartDashboard/Swerve/Modules", + "/NT/SmartDashboard/EnergyUtil", + "/SmartDashboard/Swerve/Modules", + "/SmartDashboard/Swerve/Modules/Module 0", + "/SmartDashboard/Swerve/Modules/Module 1", + "/NT/SmartDashboard/Spindexer", + "/NT/SmartDashboard/Handoff", + "/Robot/Robot/CAN/Canivore", + "/DS", + "/Robot/RadioStatus/StatusJson", + "/limelight-back/rawtargets", + "/limelight-back/rawdetections", + "/limelight-left/rawfiducials", + "/limelight-left/rawtargets", + "/limelight-back/rawfiducials", + "/Robot/Swerve/Modules", + "/Robot/Swerve/Modules/Module 0", + "/Robot/Swerve/Modules/Module 3", + "/Robot/Vision/null", + "/SmartDashboard/Intake/Pivot/Gains", + "/Robot", + "/DS/joystick0", + "/Robot/Swerve/Modules/Module 2", + "/Robot/Swerve/Modules/Module 1", + "/Robot/Swerve/Pose", + "/Robot/Vision/limelight-right/Pose MT2", + "/Robot/Vision/limelight-right/Pose MT2/translation", + "/Robot/Vision/limelight-right/Pose MT2/rotation", + "/limelight-right/rawfiducials", + "/limelight-right/hw", + "/Robot/Robot/CAN", + "/Robot/Superstructure/SOTM", + "/limelight-back/hw", + "/Robot/Vision/limelight-right", + "/Robot/Vision/limelight-right/Temp (C)", + "/Robot/Vision/limelight-left/Temp (C)", + "/Robot/Vision/limelight-back/Temp (C)", + "/SmartDashboard", + "/SmartDashboard/Robot/Scheduled Commands/Names", + "/Robot/Superstructure", + "/Robot/Superstructure/Shooter", + "/Robot/EnergyUtil", + "/Robot/EnergyUtil/Supply Current Amps", + "/Robot/EnergyUtil/Energy Watt Hours" + ] + }, + "tabs": { + "selected": 1, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "o8ft5hbh5qnj2483ziz131p432efteo7", + "renderer": "", + "controlsHeight": 0 + }, + { + "type": 2, + "title": "2D Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "/Robot/Swerve/Pose", + "logType": "Pose2d", + "visible": true, + "options": { + "bumpers": "" + } + }, + { + "type": "ghost", + "logKey": "/Robot/Vision/limelight-back/Pose MT2", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ffff00" + } + }, + { + "type": "ghost", + "logKey": "/Robot/Vision/limelight-left/Pose MT2", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ff8c00" + } + }, + { + "type": "ghost", + "logKey": "/Robot/Vision/limelight-right/Pose MT2", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ff00ff" + } + }, + { + "type": "robot", + "logKey": "NT:/Robot/Swerve/Pose", + "logType": "Pose2d", + "visible": true, + "options": { + "bumpers": "#ff0000" + } + }, + { + "type": "ghost", + "logKey": "/Robot/Superstructure/SOTM/virtual pose", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#00ff00" + } + } + ], + "field": "FRC:2026 Field", + "orientation": 0, + "size": "large" + }, + "controllerUUID": "vkwhmy5zet2ict9f3q2r29w24rov9e6u", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 7, + "title": "Video", + "controller": null, + "controllerUUID": "iq9ir2li76pjy8a8ieh11h7czu0vl598", + "renderer": null, + "controlsHeight": 85 + }, + { + "type": 1, + "title": "LL data", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-right/Temp (C)/0", + "logType": "Number", + "visible": true, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-right/Heartbeat", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-left/Temp (C)/0", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-left/Heartbeat", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-back/Heartbeat", + "logType": "Number", + "visible": true, + "options": { + "color": "#858584", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-back/Temp (C)/0", + "logType": "Number", + "visible": true, + "options": { + "color": "#5f4528", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-back/Time since last boot", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-right/Time since last boot", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/FMSInfo/FMSControlData", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437" + } + }, + { + "type": "stripes", + "logKey": "DS:enabled", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#af2437" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "w211nur4mw66budagt98wnlzbi6tb1c9", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 5, + "title": "Console", + "controller": "console", + "controllerUUID": "xcdyalich0rba2icj86xy0lly302g17b", + "renderer": { + "highlight": false + }, + "controlsHeight": 0 + }, + { + "type": 4, + "title": "Table", + "controller": [ + "NT:/SmartDashboard/Robot/Scheduled Commands/Names", + "NT:/Robot/Leds/State" + ], + "controllerUUID": "1iy7p3lq4owbb5o2wvw4pk2dgyxvfgil", + "renderer": null, + "controlsHeight": 0 + }, + { + "type": 1, + "title": "Drive Train Current", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 0/Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#858584", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 1/Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 2/Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#d993aa", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 3/Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#5f4528", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 0/Stator Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 1/Stator Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 2/Stator Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 3/Stator Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/FMSInfo/FMSControlData", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "4v4b2vbhqa8pntvp2k6vib5cleiwpdum", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 4, + "title": "Table", + "controller": [ + "NT:/SmartDashboard/Robot/Scheduled Commands/Names" + ], + "controllerUUID": "qgz1f0xamwpqf6k7kx9tad3ovosakmi9", + "renderer": null, + "controlsHeight": 0 + }, + { + "type": 1, + "title": "Handoff", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Current RPM", + "logType": "Number", + "visible": true, + "options": { + "color": "#d993aa", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Follow Velocity", + "logType": "Number", + "visible": true, + "options": { + "color": "#5f4528", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Lead Velocity", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "/Robot/Handoff/Lead Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Follow Stator Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Follow Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Follow Voltage", + "logType": "Number", + "visible": true, + "options": { + "color": "#5f4528", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Lead Stator Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Lead Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Lead Voltage", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/Robot/Handoff/State", + "logType": "String", + "visible": true, + "options": { + "color": "#858584" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/State", + "logType": "String", + "visible": true, + "options": { + "color": "#3b875a" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Shooter At Tolerance?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#e5b31b" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Turret At Tolerance?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#d993aa" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Hood At Tolerance?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#2b66a2" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "3aqzg2twfsajtgvmavr5a5id4llsuy3s", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Intake", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Roller Leader Voltage (volts)", + "logType": "Number", + "visible": false, + "options": { + "color": "#5f4528", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Current Angle (deg)", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Target Angle (deg)", + "logType": "Number", + "visible": true, + "options": { + "color": "#858584", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Pivot Temperature (C)", + "logType": "Number", + "visible": false, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Pivot Supply Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Pivot Stator Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Roller Leader Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Roller Follower Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Roller Leader Voltage (volts)", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Roller Follower Voltage (volts)", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/Robot/Intake/Pivot State", + "logType": "String", + "visible": true, + "options": { + "color": "#c0b487" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Intake/Pivot At Tolerance?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#858584" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Intake/Roller State", + "logType": "String", + "visible": true, + "options": { + "color": "#2b66a2" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Intake/Pivot Pushdown Voltage Applied?", + "logType": "Boolean", + "visible": false, + "options": { + "color": "#80588e" + } + }, + { + "type": "stripes", + "logKey": "NT:/FMSInfo/FMSControlData", + "logType": "Number", + "visible": true, + "options": { + "color": "#d993aa" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Intake/Pivot is below pushdown Threshold", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#af2437" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Hood/State", + "logType": "String", + "visible": true, + "options": { + "color": "#2b66a2" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "lv0zopsqi5hzgd5qb1ogder5fkbl8lx4", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Spindexer", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Spindexer/Leader Motor RPM", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Spindexer/Leader Supply Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Spindexer/Leader Stator Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#5f4528", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Spindexer/Leader Voltage (volts)", + "logType": "Number", + "visible": false, + "options": { + "color": "#80588e", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/Robot/Spindexer/At Tolerance", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#e5b31b" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Spindexer/State", + "logType": "String", + "visible": true, + "options": { + "color": "#af2437" + } + }, + { + "type": "stripes", + "logKey": "NT:/SmartDashboard/Spindexer/At Tolerance", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#80588e" + } + }, + { + "type": "stripes", + "logKey": "NT:/SmartDashboard/Spindexer/State", + "logType": "String", + "visible": true, + "options": { + "color": "#af2437" + } + }, + { + "type": "stripes", + "logKey": "NT:/SmartDashboard/Superstructure/State", + "logType": "String", + "visible": true, + "options": { + "color": "#2b66a2" + } + }, + { + "type": "stripes", + "logKey": "/Robot/RadioStatus/Connected", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#3b875a" + } + }, + { + "type": "stripes", + "logKey": "/Robot/Superstructure/State", + "logType": "String", + "visible": true, + "options": { + "color": "#2b66a2" + } + }, + { + "type": "stripes", + "logKey": "/Robot/Superstructure/Should Stop?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#e5b31b" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "nzjev0kx9o0kc8x55u0zt5yd918sbr5s", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Turret", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Target Angle", + "logType": "Number", + "visible": false, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Current Angle", + "logType": "Number", + "visible": false, + "options": { + "color": "#d993aa", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Current Angle", + "logType": "Number", + "visible": true, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Wrapped Target Angle (deg)", + "logType": "Number", + "visible": true, + "options": { + "color": "#d993aa", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Encoder17t Abs Position (Rot)", + "logType": "Number", + "visible": false, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Encoder18t Abs Position (Rot)", + "logType": "Number", + "visible": false, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Relative Encoder Position (deg)", + "logType": "Number", + "visible": false, + "options": { + "color": "#858584", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Voltage (volts)", + "logType": "Number", + "visible": false, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Turret/State", + "logType": "String", + "visible": true, + "options": { + "color": "#5f4528" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Turret At Tolerance?", + "logType": "Boolean", + "visible": false, + "options": { + "color": "#e5b31b" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Spindexer/Should Stop/Is Turret Wrapping?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#af2437" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Turret At Tolerance?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#e48b32" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Spindexer/State", + "logType": "String", + "visible": true, + "options": { + "color": "#80588e" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "1l62ufdzjd96aeg8je5ptz4duv0yv8mb", + "renderer": null, + "controlsHeight": 202 + }, + { + "type": 1, + "title": "Shooter", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Target RPM", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Leader RPM", + "logType": "Number", + "visible": true, + "options": { + "color": "#858584", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Target RPM", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Follower Motor Temp (C)", + "logType": "Number", + "visible": false, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Follower Voltage (volts)", + "logType": "Number", + "visible": false, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Leader Motor Temp (C)", + "logType": "Number", + "visible": false, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Implemented Error (RPM)", + "logType": "Number", + "visible": false, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Leader Voltage (volts)", + "logType": "Number", + "visible": false, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Leader Supply Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Follower Supply Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Follower Stator Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Leader Stator Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#c0b487", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Shooter/State", + "logType": "String", + "visible": true, + "options": { + "color": "#3b875a" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Shooter At Tolerance?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#d993aa" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Handoff/State", + "logType": "String", + "visible": true, + "options": { + "color": "#5f4528" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Intake/Roller State", + "logType": "String", + "visible": true, + "options": { + "color": "#2b66a2" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "taexnipg1s9d9n3iawednkasi1c36sai", + "renderer": null, + "controlsHeight": 186 + }, + { + "type": 1, + "title": "Energy data", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/CommandSwerveDrivetrain Drive", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/CommandSwerveDrivetrain Turn", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/HandoffImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/HoodImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/IntakeImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/ShooterImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/SpindexerImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#858584", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/TurretImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#3b875a", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Total Used Energy", + "logType": "Number", + "visible": true, + "options": { + "color": "#d993aa", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/CommandSwerveDrivetrain Drive", + "logType": "Number", + "visible": true, + "options": { + "color": "#5f4528", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/CommandSwerveDrivetrain Turn", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/HandoffImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/HoodImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/IntakeImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/ShooterImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/SpindexerImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/TurretImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#858584", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "teziz9fbwjzdw99zhiquulz49m5d3pm8", + "renderer": null, + "controlsHeight": 268 + }, + { + "type": 1, + "title": "QueuedLogs", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/DogLog/QueuedLogs", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/DogLog/QueueRemainingCapacity", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "gq8c8iov24h4txn3epzzwqswvay9n04f", + "renderer": null, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "26.0.0" +} diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index df7cc990..e541a78b 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -39,7 +39,7 @@ import com.stuypulse.robot.commands.intake.IntakeTeleopDigest; import com.stuypulse.robot.commands.intake.SeedPivotDeployed; import com.stuypulse.robot.commands.intake.SeedPivotStowed; -import com.stuypulse.robot.commands.leds.LEDApplyPattern; +import com.stuypulse.robot.commands.leds.LEDApplyState; import com.stuypulse.robot.commands.leds.LEDDefaultCommand; import com.stuypulse.robot.commands.spindexer.SpindexerReverse; import com.stuypulse.robot.commands.spindexer.SpindexerRun; @@ -80,6 +80,7 @@ import com.stuypulse.robot.subsystems.intake.Intake.PivotState; import com.stuypulse.robot.subsystems.intake.Intake.RollerState; import com.stuypulse.robot.subsystems.leds.LEDController; +import com.stuypulse.robot.subsystems.leds.LEDController.LEDSTATE; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -380,19 +381,21 @@ private void configureElasticButtons() { SmartDashboard.putData("Robot/Set Pipeline High Sun", new SetPipeline(Pipeline.HIGH_SUN)); // Unjamming - SmartDashboard.putData("Robot/Handoff Reverse", + SmartDashboard.putData("Robot/Handoff Reverse", + //IMPORTANT this will not work with LEDS new ConditionalCommand( new HandoffReverse().andThen(new WaitCommand(0.25)).andThen(new HandoffRun()), new HandoffReverse().andThen(new WaitCommand(0.25).andThen(new HandoffStop())), - () -> handoff.getState() == HandoffState.FORWARD).alongWith(new LEDApplyPattern(Settings.LED.REVERSE))); + () -> handoff.getState() == HandoffState.FORWARD).alongWith(new LEDApplyState(LEDSTATE.REVERSE))); - SmartDashboard.putData("Robot/Intake Reverse", new IntakeSetState(RollerState.OUTTAKE).alongWith(new LEDApplyPattern(Settings.LED.REVERSE))); + SmartDashboard.putData("Robot/Intake Reverse", new IntakeSetState(RollerState.OUTTAKE).alongWith(new LEDApplyState(LEDSTATE.REVERSE))); SmartDashboard.putData("Robot/Spindexer Reverse", + //IMPORTANT this will not work with LEDS new ConditionalCommand( new SpindexerReverse().andThen(new WaitCommand(1)).andThen(new SpindexerRun()), new SpindexerReverse().andThen(new WaitCommand(1).andThen(new SpindexerStop())), - () -> spindexer.getState() == SpindexerState.FORWARD).alongWith(new LEDApplyPattern(Settings.LED.REVERSE))); + () -> spindexer.getState() == SpindexerState.FORWARD).alongWith(new LEDApplyState(LEDSTATE.REVERSE))); } diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java similarity index 63% rename from src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java rename to src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java index 9cc60bc6..0c89daaa 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java @@ -17,22 +17,23 @@ import com.ctre.phoenix6.controls.ControlRequest; import com.stuypulse.robot.subsystems.leds.LEDController; +import com.stuypulse.robot.subsystems.leds.LEDController.LEDSTATE; -public class LEDApplyPattern extends InstantCommand { - +public class LEDApplyState extends InstantCommand { + //This will not work as default command will override it. Either make the cached class the same as this OR (better solution) have a boolean that tells you if it is manually applied or not and if it is then default command dont change protected final LEDController leds; - protected final ControlRequest pattern; + protected final LEDSTATE state; - public LEDApplyPattern(ControlRequest pattern) { + public LEDApplyState(LEDSTATE state) { leds = LEDController.getInstance(); - this.pattern = pattern; + this.state = state; addRequirements(leds); } @Override public void execute() { - leds.applyPattern(pattern); + leds.changeState(state); } } diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java index 6f1f0583..c41565b7 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java @@ -18,6 +18,7 @@ import com.stuypulse.robot.subsystems.intake.Intake.PivotState; import com.stuypulse.robot.subsystems.intake.Intake.RollerState; import com.stuypulse.robot.subsystems.leds.LEDController; +import com.stuypulse.robot.subsystems.leds.LEDController.LEDSTATE; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -61,65 +62,53 @@ public LEDDefaultCommand() { @Override public void initialize() { - String state = "NONE"; + // String state = "NONE"; if (Robot.getMode() == RobotMode.DISABLED) { if (LimelightVision.getInstance().getMaxTagCount() >= Settings.LED.DESIRED_TAGS_WHEN_DISABLED) { - leds.applyPattern(Settings.LED.DISABLED_ALIGNED); - state = "DISABLED_ALIGNED"; + leds.changeState(LEDSTATE.DISABLED_ALIGNED); } else { - leds.applyPattern(Settings.LED.DISABLED); - state = "DISABLED_DISALLOWED"; + leds.changeState(LEDSTATE.DISABLED); } } else { if (swerve.isUnderTrench()) { - leds.applyPattern(Settings.LED.PASSING_TRENCH); - state = "UNDER_TRENCH"; + leds.changeState(LEDSTATE.PASSING_TRENCH); } else if (turret.isWrapping()) { - leds.applyPattern(Settings.LED.TURRET_WRAPPING); - state = "TURRET_WRAPPING"; + leds.changeState(LEDSTATE.TURRET_WRAPPING); } else if (superstructure.getState() == SuperstructureState.LEFT_CORNER) { - leds.applyPattern(Settings.LED.LEFT_CORNER); - state = "LEFT_CORNER"; + leds.changeState(LEDSTATE.LEFT_CORNER); } else if (superstructure.getState() == SuperstructureState.RIGHT_CORNER) { - leds.applyPattern(Settings.LED.RIGHT_CORNER); - state = "RIGHT_CORNER"; + leds.changeState(LEDSTATE.RIGHT_CORNER); } else if (superstructure.getState() == SuperstructureState.KB) { - leds.applyPattern(Settings.LED.KB_DISTANCE); - state = "KITBOT"; + leds.changeState(LEDSTATE.KB_DISTANCE); } else if (superstructure.getState() == SuperstructureState.SOTM) { - leds.applyPattern(Settings.LED.SOTM_ON); - state = "SOTM"; + leds.changeState(LEDSTATE.SOTM_ON); } else if (superstructure.getState() == SuperstructureState.FOTM) { - leds.applyPattern(Settings.LED.FOTM_ON); - state = "FOTM"; + leds.changeState(LEDSTATE.FOTM_ON); } else if (spindexer.getState() == SpindexerState.REVERSE || handoff.getState() == HandoffState.REVERSE || intake.getRollerState() == RollerState.OUTTAKE) { - leds.applyPattern(Settings.LED.REVERSE); - state = "REVERSE"; + leds.changeState(LEDSTATE.REVERSE); } else if (intake.getPivotState() == PivotState.STOW) { - leds.applyPattern(Settings.LED.INTAKE_STOW); - state = "INTAKE_STOW"; + leds.changeState(LEDSTATE.INTAKE_STOW); } else if (intake.getPivotState() == PivotState.DEPLOY) { - leds.applyPattern(Settings.LED.INTAKE_DEPLOYED); - state = "INTAKE_DEPLOYED"; + leds.changeState(LEDSTATE.INTAKE_DEPLOYED); } } - DogLog.log("Leds/State", state); + // DogLog.log("Leds/State", state); } @Override diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index e9780677..17ff1f18 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -366,45 +366,48 @@ public interface Tolerances { } public interface LED { - public static SolidColor solidColor(Color color) { - return new SolidColor(0, LED_LENGTH - 1).withColor(new RGBWColor(color)); + public SolidColor solidColorRequest = new SolidColor(0, Settings.LED.LED_LENGTH - 1).withColor(new RGBWColor(Color.kRed)); + public RainbowAnimation rainbowRequest = new RainbowAnimation(0, Settings.LED.LED_LENGTH - 1).withFrameRate(60).withSlot(0); + + public static RGBWColor rgbwConverter(Color color) { + return new RGBWColor(color); } public final int LED_LENGTH = 8 + 21; //CANdle already has 8 - SolidColor PASSING_TRENCH = solidColor(Color.kRed); - SolidColor IS_BEHIND_HUB = solidColor(Color.kRed); + RGBWColor PASSING_TRENCH = rgbwConverter(Color.kRed); + RGBWColor IS_BEHIND_HUB = rgbwConverter(Color.kRed); - // SolidColor CLIMB_ALIGNING = solidColor(Color.kYellow); - // SolidColor CLIMB_ALIGNED = solidColor(Color.kGreen); - // SolidColor CLIMBING = solidColor(Color.kRed); + // RGBWColor CLIMB_ALIGNING = rgbwConverter(Color.kYellow); + // RGBWColor CLIMB_ALIGNED = rgbwConverter(Color.kGreen); + // RGBWColor CLIMBING = rgbwConverter(Color.kRed); - SolidColor TURRET_WRAPPING = solidColor(Color.kRed); - SolidColor LEFT_WARNING = solidColor(Color.kBlack); // TBD - SolidColor RIGHT_WARNING = solidColor(Color.kBlack); // TBD + RGBWColor TURRET_WRAPPING = rgbwConverter(Color.kRed); + RGBWColor LEFT_WARNING = rgbwConverter(Color.kBlack); // TBD + RGBWColor RIGHT_WARNING = rgbwConverter(Color.kBlack); // TBD - SolidColor SHOOT_IN_PLACE = solidColor(Color.kPurple); + RGBWColor SHOOT_IN_PLACE = rgbwConverter(Color.kPurple); - SolidColor SOTM_ON = solidColor(Color.kCyan); + RGBWColor SOTM_ON = rgbwConverter(Color.kCyan); RainbowAnimation FOTM_ON = new RainbowAnimation(0, LED_LENGTH - 1).withFrameRate(60).withSlot(0); - SolidColor LEFT_CORNER = solidColor(Color.kPurple); - SolidColor RIGHT_CORNER = solidColor(Color.kBlue); + RGBWColor LEFT_CORNER = rgbwConverter(Color.kPurple); + RGBWColor RIGHT_CORNER = rgbwConverter(Color.kBlue); - SolidColor KB_DISTANCE = solidColor(Color.kPink); + RGBWColor KB_DISTANCE = rgbwConverter(Color.kPink); - SolidColor REVERSE = solidColor(Color.kWhite); - SolidColor STOP_ROLLERS = solidColor(Color.kYellow); + RGBWColor REVERSE = rgbwConverter(Color.kWhite); + RGBWColor STOP_ROLLERS = rgbwConverter(Color.kYellow); - SolidColor RESET_HEADING = solidColor(Color.kYellow); - SolidColor X_WHEELS = solidColor(Color.kRed); + RGBWColor RESET_HEADING = rgbwConverter(Color.kYellow); + RGBWColor X_WHEELS = rgbwConverter(Color.kRed); - SolidColor INTAKE_STOW = solidColor(Color.kBrown); //broken - SolidColor INTAKE_DEPLOYED = solidColor(Color.kOrange); //broken + RGBWColor INTAKE_STOW = rgbwConverter(Color.kBrown); //broken + RGBWColor INTAKE_DEPLOYED = rgbwConverter(Color.kOrange); //broken - SolidColor DISABLED_ALIGNED = solidColor(Color.kGreen); - SolidColor DISABLED = solidColor(Color.kRed); + RGBWColor DISABLED_ALIGNED = rgbwConverter(Color.kGreen); + RGBWColor DISABLED = rgbwConverter(Color.kRed); - // SolidColor.gradient(GradientType.kDiscontinuous, Color.kRed, Color.kWhite).scrollAtRelativeSpeed(Percent.per(Second).of(25)); + // RGBWColor.gradient(GradientType.kDiscontinuous, Color.kRed, Color.kWhite).scrollAtRelativeSpeed(Percent.per(Second).of(25)); public final int DESIRED_TAGS_WHEN_DISABLED = 2; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 378b3d03..f81a54d3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -34,9 +34,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDController extends SubsystemBase { - private SolidColor solidColorRequest = new SolidColor(0, Settings.LED.LED_LENGTH - 1).withColor(new RGBWColor(Color.kRed)); - private RainbowAnimation rainbowRequest = new RainbowAnimation(0, Settings.LED.LED_LENGTH - 1).withFrameRate(60).withSlot(0); - private final static LEDController instance; static { @@ -49,75 +46,69 @@ public static LEDController getInstance() { private final CANdle leds; private CANdleConfiguration candleConfigs; - private ControlRequest ledPattern = Settings.LED.DISABLED; - private boolean isChanged; + private ControlRequest ledPattern = Settings.LED.solidColorRequest.withColor(Settings.LED.DISABLED); + //different portions of the LED should be a different color to indicate whether certain limelights are dead //add the flashing aspect based on if we don't see a tag (with debounce) // one way to go further with the flashing aspect is make it flash faster over DISTANCE (since last tag was seen) rather than time public enum LEDSTATE { - PASSING_TRENCH, - IS_BEHIND_HUB, - TURRET_WRAPPING, - LEFT_WARNING, - RIGHT_WARNING, - SHOOT_IN_PLACE, - SOTM_ON, - FOTM_ON, - LEFT_CORNER, - RIGHT_CORNER, - KB_DISTANCE, - REVERSE, - STOP_ROLLERS, - RESET_HEADING, - X_WHEELS, - INTAKE_STOW, - INTAKE_DEPLOYED, - DISABLED_ALIGNED, - DISABLED + PASSING_TRENCH(Settings.LED.PASSING_TRENCH), + IS_BEHIND_HUB(Settings.LED.IS_BEHIND_HUB), + TURRET_WRAPPING(Settings.LED.TURRET_WRAPPING), + LEFT_WARNING(Settings.LED.LEFT_WARNING), + RIGHT_WARNING(Settings.LED.RIGHT_WARNING), + SHOOT_IN_PLACE(Settings.LED.SHOOT_IN_PLACE), + SOTM_ON(Settings.LED.SOTM_ON), + FOTM_ON(Settings.LED.DISABLED), //holder bcs rainbow + LEFT_CORNER(Settings.LED.LEFT_CORNER), + RIGHT_CORNER(Settings.LED.RIGHT_CORNER), + KB_DISTANCE(Settings.LED.KB_DISTANCE), + REVERSE(Settings.LED.REVERSE), + STOP_ROLLERS(Settings.LED.STOP_ROLLERS), + RESET_HEADING(Settings.LED.RESET_HEADING), + X_WHEELS(Settings.LED.X_WHEELS), + INTAKE_STOW(Settings.LED.INTAKE_STOW), + INTAKE_DEPLOYED(Settings.LED.INTAKE_DEPLOYED), + DISABLED_ALIGNED(Settings.LED.DISABLED_ALIGNED), + DISABLED(Settings.LED.DISABLED); + + private RGBWColor color; + + private LEDSTATE(RGBWColor color) { + this.color = color; + } + + public RGBWColor getColor() { + return this.color; + } + + public ControlRequest getAnimation() { + if (this == LEDSTATE.FOTM_ON) { + return Settings.LED.rainbowRequest; + } + else { + return Settings.LED.solidColorRequest.withColor(getColor()); + } + } } private LEDSTATE state = LEDSTATE.DISABLED; private LEDSTATE cachedState = LEDSTATE.DISABLED; - - public ControlRequest stateToPattern(LEDSTATE state) { - return switch (state) { - case PASSING_TRENCH -> solidColorRequest.withColor(new RGBWColor(Color.kRed)); - case IS_BEHIND_HUB -> solidColorRequest.withColor(new RGBWColor(Color.kRed)); - case TURRET_WRAPPING -> solidColorRequest.withColor(new RGBWColor(Color.kRed)); - case LEFT_WARNING -> solidColorRequest.withColor(new RGBWColor(Color.kBlack)); - case RIGHT_WARNING -> solidColorRequest.withColor(new RGBWColor(Color.kBlack)); - case SHOOT_IN_PLACE -> solidColorRequest.withColor(new RGBWColor(Color.kPurple)); - case SOTM_ON -> solidColorRequest.withColor(new RGBWColor(Color.kCyan)); - - case FOTM_ON -> rainbowRequest; //rainbow animation -> need to add cached states and a change pattern type method -> maybe apply pattern should take in the pattern and the color/frequency -> then i would need a system like the status signal where we just call them once and mutate them after - - case LEFT_CORNER -> solidColorRequest.withColor(new RGBWColor(Color.kPurple)); - case RIGHT_CORNER -> solidColorRequest.withColor(new RGBWColor(Color.kBlue)); - case KB_DISTANCE -> solidColorRequest.withColor(new RGBWColor(Color.kPink)); - case REVERSE -> solidColorRequest.withColor(new RGBWColor(Color.kWhite)); - case STOP_ROLLERS -> solidColorRequest.withColor(new RGBWColor(Color.kYellow)); - case RESET_HEADING -> solidColorRequest.withColor(new RGBWColor(Color.kYellow)); - case X_WHEELS -> solidColorRequest.withColor(new RGBWColor(Color.kRed)); - case INTAKE_STOW -> solidColorRequest.withColor(new RGBWColor(Color.kBrown)); - case INTAKE_DEPLOYED -> solidColorRequest.withColor(new RGBWColor(Color.kOrange)); - case DISABLED_ALIGNED -> solidColorRequest.withColor(new RGBWColor(Color.kGreen)); - case DISABLED -> solidColorRequest.withColor(new RGBWColor(Color.kRed)); - }; - //CHANGE apply pattern command to change state - } - public void applyPattern(ControlRequest ledPattern) { + public void applyPattern() { if (cachedState != state) { - if (stateToPattern(cachedState) != stateToPattern(state)) { - this.ledPattern = stateToPattern(state); + if (!(cachedState.getAnimation().getName().equals(state.getAnimation().getName()))) { + this.ledPattern = state.getAnimation(); } - else if (stateToPattern(state) instanceof SolidColor){ - SolidColor.class.cast(ledPattern).withColor(null); //UPDATTTEEE + else if (ledPattern instanceof SolidColor){ + SolidColor solidColor = (SolidColor) ledPattern; + solidColor.withColor(state.getColor()); + // SolidColor.class.cast(ledPattern).withColor(null); //change if neccesary } cachedState = state; } @@ -128,7 +119,7 @@ public void changeState(LEDSTATE state) { } private LEDController() { - leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); // TODO: update ports value + leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); candleConfigs = new CANdleConfiguration() .withLED( @@ -143,22 +134,24 @@ private LEDController() { leds.getConfigurator().apply(candleConfigs); leds.setControl(ledPattern); - isChanged = true; } public void periodicAfterScheduler() { if (RobotContainer.EnabledSubsystems.LEDS.get()) { - if(isChanged) { - leds.clearAllAnimations(); - leds.setControl(ledPattern); - isChanged = false; - } + leds.clearAllAnimations(); + applyPattern(); + leds.setControl(ledPattern); } else { leds.clearAllAnimations(); } - DogLog.log("LED/Pattern Name", ledPattern.getName()); + DogLog.log("LED/Applied Pattern Name", ledPattern.getName()); + + DogLog.log("LED/State Pattern Name", state.getAnimation().getName()); + DogLog.log("LED/Cached State Pattern Name", state.getAnimation().getName()); + DogLog.log("LED/State", state.toString()); + DogLog.log("LED/Cached State", state.toString()); } } \ No newline at end of file From 683bfa2a54d22b46d09b505c836a36f6a5fbf35a Mon Sep 17 00:00:00 2001 From: Apetrock Date: Wed, 20 May 2026 17:06:42 -0400 Subject: [PATCH 31/97] FEAT: new motor basic functionality --- .../com/stuypulse/robot/constants/Ports.java | 1 + .../subsystems/spindexer/SpindexerImpl.java | 28 +++++++++++++++++++ 2 files changed, 29 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 12ca7647..e08de0d5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -56,5 +56,6 @@ public interface Intake { public interface Spindexer { int MOTOR = 30; + int FOLLOWER = -1; // TODO: follower port } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index 88f93592..6a6fcc6a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -9,8 +9,10 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; @@ -35,10 +37,13 @@ public class SpindexerImpl extends Spindexer { private final Motors.TalonFXConfig spindexerLeadConfig; + private final Motors.TalonFXConfig spindexerFollowConfig; private final TalonFX leaderMotor; + private final TalonFX followerMotor; private final DutyCycleOut controller; + private final Follower followerControl; private final BStream isStalling; private boolean hasStartedStallTimer; private final Timer unjamTimer; @@ -64,16 +69,36 @@ public SpindexerImpl() { .withSensorToMechanismRatio(Settings.Spindexer.GEAR_RATIO); + spindexerFollowConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + + .withSupplyCurrentLimitAmps(45) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) + + .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) + .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) + + .withSensorToMechanismRatio(Settings.Spindexer.GEAR_RATIO); + leaderMotor = new TalonFX(Ports.Spindexer.MOTOR, Ports.CANIVORE); + followerMotor = new TalonFX(Ports.Spindexer.FOLLOWER, Ports.CANIVORE); spindexerLeadConfig.configure(leaderMotor); + spindexerFollowConfig.configure(followerMotor); controller = new DutyCycleOut(getTargetDutyCycle()).withEnableFOC(true); + followerControl = new Follower(Ports.Spindexer.MOTOR, MotorAlignmentValue.Aligned); leaderSupplyCurrent = leaderMotor.getSupplyCurrent(); leaderStatorCurrent = leaderMotor.getStatorCurrent(); leaderVelocity = leaderMotor.getVelocity(); leaderMotorVoltage = leaderMotor.getMotorVoltage(); + // followerSupplyCurrent = leaderMotor.getSupplyCurrent(); + // followerStatorCurrent = leaderMotor.getStatorCurrent(); + // followerVelocity = leaderMotor.getVelocity(); + // followerMotorVoltage = leaderMotor.getMotorVoltage(); PhoenixUtil.registerToCanivore(leaderSupplyCurrent, leaderStatorCurrent, leaderVelocity, leaderMotorVoltage); isStalling = BStream.create( () -> leaderSupplyCurrent.getValueAsDouble() > Settings.Spindexer.STALL_CURRENT_LIMIT) @@ -118,13 +143,16 @@ public void periodicAfterScheduler() { } else { if (Superstructure.getInstance().shouldStop()) { leaderMotor.stopMotor(); + followerMotor.stopMotor(); } else { leaderMotor.setControl(controller.withOutput(getTargetDutyCycle())); + followerMotor.setControl(followerControl); } } } else { leaderMotor.stopMotor(); + followerMotor.stopMotor(); } DogLog.log("Spindexer/Leader Motor RPM", getMotorRPM()); From 13774f11991578c4fc59276133cede19550a599c Mon Sep 17 00:00:00 2001 From: Apetrock Date: Wed, 20 May 2026 17:20:21 -0400 Subject: [PATCH 32/97] CELAN: cleanup new motor --- .../com/stuypulse/robot/constants/Ports.java | 2 +- .../subsystems/spindexer/SpindexerImpl.java | 40 +++++++++++++------ 2 files changed, 29 insertions(+), 13 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index e08de0d5..ee3b2254 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -55,7 +55,7 @@ public interface Intake { } public interface Spindexer { - int MOTOR = 30; + int LEADER = 30; int FOLLOWER = -1; // TODO: follower port } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index 6a6fcc6a..7b8c65b8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -54,6 +54,10 @@ public class SpindexerImpl extends Spindexer { private StatusSignal leaderStatorCurrent; private StatusSignal leaderVelocity; private StatusSignal leaderMotorVoltage; + private StatusSignal followerSupplyCurrent; + private StatusSignal followerStatorCurrent; + private StatusSignal followerVelocity; + private StatusSignal followerMotorVoltage; public SpindexerImpl() { spindexerLeadConfig = new Motors.TalonFXConfig() @@ -82,26 +86,27 @@ public SpindexerImpl() { .withSensorToMechanismRatio(Settings.Spindexer.GEAR_RATIO); - leaderMotor = new TalonFX(Ports.Spindexer.MOTOR, Ports.CANIVORE); + leaderMotor = new TalonFX(Ports.Spindexer.LEADER, Ports.CANIVORE); followerMotor = new TalonFX(Ports.Spindexer.FOLLOWER, Ports.CANIVORE); spindexerLeadConfig.configure(leaderMotor); spindexerFollowConfig.configure(followerMotor); controller = new DutyCycleOut(getTargetDutyCycle()).withEnableFOC(true); - followerControl = new Follower(Ports.Spindexer.MOTOR, MotorAlignmentValue.Aligned); + followerControl = new Follower(Ports.Spindexer.LEADER, MotorAlignmentValue.Aligned); leaderSupplyCurrent = leaderMotor.getSupplyCurrent(); leaderStatorCurrent = leaderMotor.getStatorCurrent(); leaderVelocity = leaderMotor.getVelocity(); leaderMotorVoltage = leaderMotor.getMotorVoltage(); - // followerSupplyCurrent = leaderMotor.getSupplyCurrent(); - // followerStatorCurrent = leaderMotor.getStatorCurrent(); - // followerVelocity = leaderMotor.getVelocity(); - // followerMotorVoltage = leaderMotor.getMotorVoltage(); - PhoenixUtil.registerToCanivore(leaderSupplyCurrent, leaderStatorCurrent, leaderVelocity, leaderMotorVoltage); - - isStalling = BStream.create( () -> leaderSupplyCurrent.getValueAsDouble() > Settings.Spindexer.STALL_CURRENT_LIMIT) + followerSupplyCurrent = followerMotor.getSupplyCurrent(); + followerStatorCurrent = followerMotor.getStatorCurrent(); + followerVelocity = followerMotor.getVelocity(); + followerMotorVoltage = followerMotor.getMotorVoltage(); + PhoenixUtil.registerToCanivore(leaderSupplyCurrent, leaderStatorCurrent, leaderVelocity, leaderMotorVoltage, + followerSupplyCurrent, followerStatorCurrent, followerVelocity, followerMotorVoltage); + + isStalling = BStream.create(() -> (leaderSupplyCurrent.getValueAsDouble() > Settings.Spindexer.STALL_CURRENT_LIMIT) || followerSupplyCurrent.getValueAsDouble() > Settings.Spindexer.STALL_CURRENT_LIMIT) .filtered(new BDebounce.Both(Settings.Superstructure.Hood.STALL_DEBOUNCE)); voltageOverride = Optional.empty(); @@ -109,9 +114,12 @@ public SpindexerImpl() { unjamTimer = new Timer(); } - private double getMotorRPM() { + private double getLeaderRPM() { return leaderVelocity.getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.GEAR_RATIO; } + private double getFollowerRpm() { + return followerVelocity.getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.GEAR_RATIO; + } private boolean spindexerUnjam() { @@ -140,6 +148,7 @@ public void periodicAfterScheduler() { if (EnabledSubsystems.SPINDEXER.get()) { if (voltageOverride.isPresent()) { leaderMotor.setVoltage(voltageOverride.get()); + followerMotor.setVoltage(voltageOverride.get()); } else { if (Superstructure.getInstance().shouldStop()) { leaderMotor.stopMotor(); @@ -155,12 +164,15 @@ public void periodicAfterScheduler() { followerMotor.stopMotor(); } - DogLog.log("Spindexer/Leader Motor RPM", getMotorRPM()); + DogLog.log("Spindexer/Leader Motor RPM", getLeaderRPM()); // SmartDashboard.putBoolean("Spindexer/Unjamming", unJamming); DogLog.log("Spindexer/Leader Voltage (volts)", leaderMotorVoltage.getValueAsDouble()); DogLog.log("Spindexer/Leader Supply Current (amps)", leaderSupplyCurrent.getValueAsDouble()); DogLog.log("Spindexer/Leader Stator Current (amps)", leaderStatorCurrent.getValueAsDouble()); + DogLog.log("Spindexer/Follower Voltage (volts)", followerMotorVoltage.getValueAsDouble()); + DogLog.log("Spindexer/Follower Supply Current (amps)", followerSupplyCurrent.getValueAsDouble()); + DogLog.log("Spindexer/Follower Stator Current (amps)", followerStatorCurrent.getValueAsDouble()); // SmartDashboard.putBoolean("Spindexer/Should Stop?", shouldStop()); @@ -168,8 +180,12 @@ public void periodicAfterScheduler() { if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log( "Robot/CAN/Canivore/Spindexer Leader Motor Connected? (ID " - + String.valueOf(Ports.Spindexer.MOTOR) + ")", + + String.valueOf(Ports.Spindexer.LEADER) + ")", leaderMotor.isConnected()); + DogLog.log( + "Robot/CAN/Canivore/Spindexer Follower Motor Connected? (ID " + + String.valueOf(Ports.Spindexer.FOLLOWER) + ")", + followerMotor.isConnected()); } Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); } From 65f172d7fb46c308920d1bf64a399b29cf88916c Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 20 May 2026 17:32:08 -0400 Subject: [PATCH 33/97] feat: states work and tested. Added dead limelightds and distance from last seen april tag logic. Tweaked code further i think. Removed old stuff. Rest of todo list is in LED interface in Settings. --- .../com/stuypulse/robot/RobotContainer.java | 30 +++---- .../commands/leds/LEDDefaultCommand.java | 10 +-- .../stuypulse/robot/constants/Cameras.java | 20 +++++ .../stuypulse/robot/constants/Settings.java | 35 +++++++- .../robot/subsystems/leds/LEDController.java | 85 +++++++++++++------ .../subsystems/vision/LimelightVision.java | 37 +++++++- 6 files changed, 166 insertions(+), 51 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index e541a78b..666e124f 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -381,21 +381,21 @@ private void configureElasticButtons() { SmartDashboard.putData("Robot/Set Pipeline High Sun", new SetPipeline(Pipeline.HIGH_SUN)); // Unjamming - SmartDashboard.putData("Robot/Handoff Reverse", - //IMPORTANT this will not work with LEDS - new ConditionalCommand( - new HandoffReverse().andThen(new WaitCommand(0.25)).andThen(new HandoffRun()), - new HandoffReverse().andThen(new WaitCommand(0.25).andThen(new HandoffStop())), - () -> handoff.getState() == HandoffState.FORWARD).alongWith(new LEDApplyState(LEDSTATE.REVERSE))); - - SmartDashboard.putData("Robot/Intake Reverse", new IntakeSetState(RollerState.OUTTAKE).alongWith(new LEDApplyState(LEDSTATE.REVERSE))); - - SmartDashboard.putData("Robot/Spindexer Reverse", - //IMPORTANT this will not work with LEDS - new ConditionalCommand( - new SpindexerReverse().andThen(new WaitCommand(1)).andThen(new SpindexerRun()), - new SpindexerReverse().andThen(new WaitCommand(1).andThen(new SpindexerStop())), - () -> spindexer.getState() == SpindexerState.FORWARD).alongWith(new LEDApplyState(LEDSTATE.REVERSE))); + // SmartDashboard.putData("Robot/Handoff Reverse", + // //IMPORTANT this will not work with LEDS + // new ConditionalCommand( + // new HandoffReverse().andThen(new WaitCommand(0.25)).andThen(new HandoffRun()), + // new HandoffReverse().andThen(new WaitCommand(0.25).andThen(new HandoffStop())), + // () -> handoff.getState() == HandoffState.FORWARD).alongWith(new LEDApplyState(LEDSTATE.REVERSE))); + + // SmartDashboard.putData("Robot/Intake Reverse", new IntakeSetState(RollerState.OUTTAKE).alongWith(new LEDApplyState(LEDSTATE.REVERSE))); + + // SmartDashboard.putData("Robot/Spindexer Reverse", + // //IMPORTANT this will not work with LEDS + // new ConditionalCommand( + // new SpindexerReverse().andThen(new WaitCommand(1)).andThen(new SpindexerRun()), + // new SpindexerReverse().andThen(new WaitCommand(1).andThen(new SpindexerStop())), + // () -> spindexer.getState() == SpindexerState.FORWARD).alongWith(new LEDApplyState(LEDSTATE.REVERSE))); } diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java index c41565b7..458ed95e 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java @@ -95,11 +95,11 @@ else if (superstructure.getState() == SuperstructureState.SOTM) { else if (superstructure.getState() == SuperstructureState.FOTM) { leds.changeState(LEDSTATE.FOTM_ON); } - else if (spindexer.getState() == SpindexerState.REVERSE || - handoff.getState() == HandoffState.REVERSE || - intake.getRollerState() == RollerState.OUTTAKE) { - leds.changeState(LEDSTATE.REVERSE); - } + // else if (spindexer.getState() == SpindexerState.REVERSE || + // handoff.getState() == HandoffState.REVERSE || + // intake.getRollerState() == RollerState.OUTTAKE) { + // leds.changeState(LEDSTATE.REVERSE); + // } else if (intake.getPivotState() == PivotState.STOW) { leds.changeState(LEDSTATE.INTAKE_STOW); } diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index d9daa6b4..b9668aed 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -49,6 +49,10 @@ public static class Camera { private int rejectedCounterAngularVelocity; private int rejectedCounterInvalidPosition; private int rejectedCounterTargetArea; + + // private boolean isDead; + // private double heartBeat; + private LimelightResults result; private Pipeline currentPipeline; @@ -116,6 +120,22 @@ public void incrementRejection(RejectionValue rejectionValue) { } } + public int getNumberOfTagsSeen() { + return LimelightHelpers.getRawFiducials(this.getName()).length; + } + + // public boolean seesTag() { + // return getNumberOfTagsSeen() == 0; + // } + + // public void checkHeartBeats() { + + // } + + // public boolean isDead(){ + // return this.isDead; + // } + public void log() { DogLog.log(keyName + "# Rejected Not Null", rejectedCounterNotNull); DogLog.log(keyName + "# Rejected Target Area", rejectedCounterTargetArea); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 17ff1f18..9a94fcf5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -366,6 +366,26 @@ public interface Tolerances { } public interface LED { + //TODO: + //add back states (reverse and etc (low priority)) + + //(DONE) remove stuff we dont use + //(DONE) space out dead limelight indicators + + //FIX rainbow (flickering) + //make dead limelight colors flash (GIVEN THEY WORK) + //TUNE constant for heart beat + + //(DONE BUT CONFIRM WITH BLAY IF HE WANTS TIME (like 2 seconds outside of zone)) + //make debounce for the distance away from last pose with april tags in sight + + //(DONE) make the getter for the last pose with april tags in sight + // (PARTIAL IMPLEMENTATION) add the variables to the Camera objects as fields (heartbeats) + + //Add flashing based on the distance thing. + + //SEPERATE THING: ADD JSONS TO LL from the PractiCAL + public SolidColor solidColorRequest = new SolidColor(0, Settings.LED.LED_LENGTH - 1).withColor(new RGBWColor(Color.kRed)); public RainbowAnimation rainbowRequest = new RainbowAnimation(0, Settings.LED.LED_LENGTH - 1).withFrameRate(60).withSlot(0); @@ -382,12 +402,12 @@ public static RGBWColor rgbwConverter(Color color) { // RGBWColor CLIMBING = rgbwConverter(Color.kRed); RGBWColor TURRET_WRAPPING = rgbwConverter(Color.kRed); - RGBWColor LEFT_WARNING = rgbwConverter(Color.kBlack); // TBD - RGBWColor RIGHT_WARNING = rgbwConverter(Color.kBlack); // TBD + // RGBWColor LEFT_WARNING = rgbwConverter(Color.kBlack); // TBD + // RGBWColor RIGHT_WARNING = rgbwConverter(Color.kBlack); // TBD RGBWColor SHOOT_IN_PLACE = rgbwConverter(Color.kPurple); - RGBWColor SOTM_ON = rgbwConverter(Color.kCyan); + RGBWColor SOTM_ON = rgbwConverter(Color.kGreen); RainbowAnimation FOTM_ON = new RainbowAnimation(0, LED_LENGTH - 1).withFrameRate(60).withSlot(0); RGBWColor LEFT_CORNER = rgbwConverter(Color.kPurple); @@ -395,7 +415,7 @@ public static RGBWColor rgbwConverter(Color color) { RGBWColor KB_DISTANCE = rgbwConverter(Color.kPink); - RGBWColor REVERSE = rgbwConverter(Color.kWhite); + // RGBWColor REVERSE = rgbwConverter(Color.kWhite); RGBWColor STOP_ROLLERS = rgbwConverter(Color.kYellow); RGBWColor RESET_HEADING = rgbwConverter(Color.kYellow); @@ -407,9 +427,15 @@ public static RGBWColor rgbwConverter(Color color) { RGBWColor DISABLED_ALIGNED = rgbwConverter(Color.kGreen); RGBWColor DISABLED = rgbwConverter(Color.kRed); + RGBWColor LEFTDEAD = rgbwConverter(Color.kWhite); + RGBWColor RIGHTDEAD = rgbwConverter(Color.kWhite); + RGBWColor BACKDEAD = rgbwConverter(Color.kWhite); + // RGBWColor.gradient(GradientType.kDiscontinuous, Color.kRed, Color.kWhite).scrollAtRelativeSpeed(Percent.per(Second).of(25)); public final int DESIRED_TAGS_WHEN_DISABLED = 2; + + public double APRIL_TAG_DISTANCE_THRESHOLD = Units.feetToMeters(2); //TODO: update because comparing Translation2d, so make sure it is 2 feet } public interface Vision { @@ -421,6 +447,7 @@ public interface Vision { public final double INVALID_POSITION_TOLERANCE_M = 0.05; public final double MAX_ANGULAR_VELOCITY_RAD_SEC = 2 * Math.PI; double MIN_TAG_AREA = 5; //TODO: MAKE SURE THIS IS A GOOD VALUE!!! + public final double MIN_CYCLE_LL_HB = 1000; // TODO: tune SmartBoolean HDR_ENABLED = new SmartBoolean("Vision/HDR Enabled?", false); double HDR_TIMEOUT_SEC = 0.25; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index f81a54d3..a0a0ca1b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -8,7 +8,6 @@ import java.util.Optional; - import com.ctre.phoenix6.configs.CANdleConfiguration; import com.ctre.phoenix6.configs.CANdleFeaturesConfigs; import com.ctre.phoenix6.configs.CustomParamsConfigs; @@ -24,18 +23,26 @@ import com.ctre.phoenix6.signals.StatusLedWhenActiveValue; import com.ctre.phoenix6.signals.StripTypeValue; import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Cameras.Camera; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import dev.doglog.DogLog; -import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDController extends SubsystemBase { private final static LEDController instance; + public static boolean isLeftLLDead = false; + public static boolean isBackLLDead = false; + public static boolean isRightLLDead = false; + + private Pose2d lastPoseOnAprilTag; + private boolean initialPoseUpdated = false; + static { instance = new LEDController(); } @@ -47,25 +54,23 @@ public static LEDController getInstance() { private final CANdle leds; private CANdleConfiguration candleConfigs; private ControlRequest ledPattern = Settings.LED.solidColorRequest.withColor(Settings.LED.DISABLED); - - //different portions of the LED should be a different color to indicate whether certain limelights are dead - //add the flashing aspect based on if we don't see a tag (with debounce) - // one way to go further with the flashing aspect is make it flash faster over DISTANCE (since last tag was seen) rather than time + // different portions of the LED should be a different color to indicate whether + // certain limelights are dead + // add the flashing aspect based on if we don't see a tag (with debounce) + // one way to go further with the flashing aspect is make it flash faster over + // DISTANCE (since last tag was seen) rather than time public enum LEDSTATE { PASSING_TRENCH(Settings.LED.PASSING_TRENCH), IS_BEHIND_HUB(Settings.LED.IS_BEHIND_HUB), TURRET_WRAPPING(Settings.LED.TURRET_WRAPPING), - LEFT_WARNING(Settings.LED.LEFT_WARNING), - RIGHT_WARNING(Settings.LED.RIGHT_WARNING), SHOOT_IN_PLACE(Settings.LED.SHOOT_IN_PLACE), SOTM_ON(Settings.LED.SOTM_ON), - FOTM_ON(Settings.LED.DISABLED), //holder bcs rainbow + FOTM_ON(Settings.LED.DISABLED), // holder bcs rainbow LEFT_CORNER(Settings.LED.LEFT_CORNER), RIGHT_CORNER(Settings.LED.RIGHT_CORNER), KB_DISTANCE(Settings.LED.KB_DISTANCE), - REVERSE(Settings.LED.REVERSE), STOP_ROLLERS(Settings.LED.STOP_ROLLERS), RESET_HEADING(Settings.LED.RESET_HEADING), X_WHEELS(Settings.LED.X_WHEELS), @@ -80,16 +85,15 @@ private LEDSTATE(RGBWColor color) { this.color = color; } - public RGBWColor getColor() { + private RGBWColor getColor() { return this.color; } public ControlRequest getAnimation() { if (this == LEDSTATE.FOTM_ON) { return Settings.LED.rainbowRequest; - } - else { - return Settings.LED.solidColorRequest.withColor(getColor()); + } else { + return Settings.LED.solidColorRequest.withColor(this.color); } } } @@ -97,15 +101,19 @@ public ControlRequest getAnimation() { private LEDSTATE state = LEDSTATE.DISABLED; private LEDSTATE cachedState = LEDSTATE.DISABLED; - //CHANGE apply pattern command to change state + // CHANGE apply pattern command to change state public void applyPattern() { - if (cachedState != state) { + if (cachedState != state) { + if (cachedState.getAnimation().getName() != "SolidColor") { + leds.clearAllAnimations(); + } // TODO: daniel's change, double check if works. If not keep calling + // clearAllAnimations every loop if (!(cachedState.getAnimation().getName().equals(state.getAnimation().getName()))) { this.ledPattern = state.getAnimation(); - } - - else if (ledPattern instanceof SolidColor){ + } + + else if (ledPattern instanceof SolidColor) { SolidColor solidColor = (SolidColor) ledPattern; solidColor.withColor(state.getColor()); // SolidColor.class.cast(ledPattern).withColor(null); //change if neccesary @@ -120,6 +128,7 @@ public void changeState(LEDSTATE state) { private LEDController() { leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); + lastPoseOnAprilTag = new Pose2d(); candleConfigs = new CANdleConfiguration() .withLED( @@ -127,10 +136,10 @@ private LEDController() { .withBrightnessScalar(1.0) .withStripType(StripTypeValue.GRB) .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)) - + .withCANdleFeatures( new CANdleFeaturesConfigs().withStatusLedWhenActive(StatusLedWhenActiveValue.Enabled)); - + leds.getConfigurator().apply(candleConfigs); leds.setControl(ledPattern); @@ -138,13 +147,39 @@ private LEDController() { public void periodicAfterScheduler() { if (RobotContainer.EnabledSubsystems.LEDS.get()) { - leds.clearAllAnimations(); applyPattern(); leds.setControl(ledPattern); } else { leds.clearAllAnimations(); } + // reflective of the 3 LED gap between them + if (isRightLLDead) { + leds.setControl(new SolidColor(Settings.LED.LED_LENGTH - 4, Settings.LED.LED_LENGTH - 1) + .withColor(Settings.LED.RIGHTDEAD)); + } + if (isLeftLLDead) { + leds.setControl(new SolidColor(Settings.LED.LED_LENGTH - 11, Settings.LED.LED_LENGTH - 8) + .withColor(Settings.LED.LEFTDEAD)); + } + if (isBackLLDead) { + leds.setControl(new SolidColor(Settings.LED.LED_LENGTH - 18, Settings.LED.LED_LENGTH - 15) + .withColor(Settings.LED.BACKDEAD)); + } + + if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || + Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || + Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { + lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); + initialPoseUpdated = true; + } + + if (initialPoseUpdated && + lastPoseOnAprilTag.getTranslation() + .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()) > Settings.LED.APRIL_TAG_DISTANCE_THRESHOLD) { + //TODO: add flashing + } + DogLog.log("LED/Applied Pattern Name", ledPattern.getName()); DogLog.log("LED/State Pattern Name", state.getAnimation().getName()); @@ -152,6 +187,6 @@ public void periodicAfterScheduler() { DogLog.log("LED/State", state.toString()); DogLog.log("LED/Cached State", state.toString()); - + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 8b286bf4..3fd1e227 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -15,6 +15,7 @@ import com.stuypulse.robot.constants.Cameras.Camera.RejectionValue; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.robot.util.vision.LimelightHelpers.IMUData; @@ -48,6 +49,10 @@ public static LimelightVision getInstance() { private int maxTagCount; private MegaTagMode megaTagMode; + private double leftLLHeartbeat = 0.0; //change to -1 is we need to switch the way we do this + private double rightLLHeartbeat = 0.0; + private double backLLHeartbeat = 0.0; + private Pose2d[] limelightPoseArray; // private StructPublisher leftLimelightPosePublisher; @@ -153,7 +158,7 @@ public void setIMUAssistValue(double assistValue) { for (String name : names) { LimelightHelpers.SetIMUAssistAlpha(name, assistValue); } - } + } /** * Allows all tags except the specified ones by setting blacklisted tag @@ -210,6 +215,10 @@ public boolean hasData() { return debouncedHasData.get(); } + public boolean checkDead(double current, double prev) { + return Math.abs(prev - current) < Settings.Vision.MIN_CYCLE_LL_HB; + } + public void periodicAfterScheduler() { if (enabled.get()) { hasData = false; @@ -220,6 +229,29 @@ public void periodicAfterScheduler() { if (Cameras.LimelightCameras[i].isEnabled()) { String limelightName = names[i]; + if (limelightName.equals("limelight-right")) { + //prev + // if (rightLLHeartbeat == LimelightHelpers.getHeartbeat(limelightName) && rightLLHeartbeat != -1) + if (checkDead(LimelightHelpers.getHeartbeat(limelightName), rightLLHeartbeat)) { + LEDController.isRightLLDead = true; + } + rightLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); + } + else if (limelightName.equals("limelight-left") && leftLLHeartbeat != -1) { + if (checkDead(LimelightHelpers.getHeartbeat(limelightName), leftLLHeartbeat)) { + LEDController.isLeftLLDead = true; + } + leftLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); + } + else if (limelightName.equals("limelight-back") && backLLHeartbeat != -1) { + if (checkDead(LimelightHelpers.getHeartbeat(limelightName), backLLHeartbeat)) { + LEDController.isBackLLDead = true; + } + backLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); + } + + + // Seed robot heading (used by MT2) LimelightHelpers.SetRobotOrientation( limelightName, @@ -244,6 +276,7 @@ public void periodicAfterScheduler() { : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(limelightName); } + // Adding to pose estimator boolean notNull = false; boolean withinAngularVelocityTolerance = false; @@ -312,8 +345,8 @@ public void periodicAfterScheduler() { // this is just the yaw of the internal imu DogLog.log("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); + } - Cameras.LimelightCameras[i].log(); } // Alternating pipelines for hdr From 6ade32b85a219e6c81d13d5458e0faa8690bc061 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 21 May 2026 15:06:59 -0400 Subject: [PATCH 34/97] update port --- src/main/java/com/stuypulse/robot/constants/Ports.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index ee3b2254..9bdcad21 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -56,6 +56,6 @@ public interface Intake { public interface Spindexer { int LEADER = 30; - int FOLLOWER = -1; // TODO: follower port + int FOLLOWER = 31; // TODO: follower port } } From c6f45a3831a74f1550b5069fbad58693b411484c Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 21 May 2026 15:10:41 -0400 Subject: [PATCH 35/97] fix FMS util constantly printing error, and limited it to 5 attempts --- src/main/java/com/stuypulse/robot/util/FMSUtil.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/util/FMSUtil.java b/src/main/java/com/stuypulse/robot/util/FMSUtil.java index d28a6535..e4459ba3 100644 --- a/src/main/java/com/stuypulse/robot/util/FMSUtil.java +++ b/src/main/java/com/stuypulse/robot/util/FMSUtil.java @@ -19,6 +19,7 @@ public class FMSUtil { private final Timer timer = new Timer(); private boolean autoMode; private boolean autoOverride = false; + private int maxErrorPrint = 0; public enum FieldState { AUTO(0.0, 20.0), @@ -115,9 +116,10 @@ public boolean didWinAuto() { String winner = DriverStation.getGameSpecificMessage(); Optional allianceOpt = DriverStation.getAlliance(); - if (winner == null || winner.isEmpty() || allianceOpt.isEmpty()) { + if (winner == null || winner.isEmpty() || allianceOpt.isEmpty() && maxErrorPrint < 5) { DriverStation.reportWarning("No FMS auto winner data available", false); DogLog.log("FMSUtil/No Auto Winner Data", true); + maxErrorPrint += 1; return autoOverride; } From 23e11a50f175c54c10ac4e23f3699017c882b84b Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 21 May 2026 16:03:38 -0400 Subject: [PATCH 36/97] feat: heartbeat works. LL died in practice match and was caught. TODO at bottom of LED Controller - very important --- .../stuypulse/robot/constants/Settings.java | 5 +- .../robot/subsystems/leds/LEDController.java | 25 +++---- .../subsystems/vision/LimelightVision.java | 73 +++++++++++++------ 3 files changed, 65 insertions(+), 38 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 9a94fcf5..398c7484 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -408,8 +408,7 @@ public static RGBWColor rgbwConverter(Color color) { RGBWColor SHOOT_IN_PLACE = rgbwConverter(Color.kPurple); RGBWColor SOTM_ON = rgbwConverter(Color.kGreen); - RainbowAnimation FOTM_ON = new RainbowAnimation(0, LED_LENGTH - 1).withFrameRate(60).withSlot(0); - + RGBWColor FOTM_ON = rgbwConverter(Color.kDarkBlue); RGBWColor LEFT_CORNER = rgbwConverter(Color.kPurple); RGBWColor RIGHT_CORNER = rgbwConverter(Color.kBlue); @@ -447,7 +446,7 @@ public interface Vision { public final double INVALID_POSITION_TOLERANCE_M = 0.05; public final double MAX_ANGULAR_VELOCITY_RAD_SEC = 2 * Math.PI; double MIN_TAG_AREA = 5; //TODO: MAKE SURE THIS IS A GOOD VALUE!!! - public final double MIN_CYCLE_LL_HB = 1000; // TODO: tune + public final double MIN_CYCLE_LL_HB = 1; // TODO: tune SmartBoolean HDR_ENABLED = new SmartBoolean("Vision/HDR Enabled?", false); double HDR_TIMEOUT_SEC = 0.25; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index a0a0ca1b..48770ee3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -67,7 +67,7 @@ public enum LEDSTATE { TURRET_WRAPPING(Settings.LED.TURRET_WRAPPING), SHOOT_IN_PLACE(Settings.LED.SHOOT_IN_PLACE), SOTM_ON(Settings.LED.SOTM_ON), - FOTM_ON(Settings.LED.DISABLED), // holder bcs rainbow + FOTM_ON(Settings.LED.FOTM_ON), LEFT_CORNER(Settings.LED.LEFT_CORNER), RIGHT_CORNER(Settings.LED.RIGHT_CORNER), KB_DISTANCE(Settings.LED.KB_DISTANCE), @@ -90,11 +90,7 @@ private RGBWColor getColor() { } public ControlRequest getAnimation() { - if (this == LEDSTATE.FOTM_ON) { - return Settings.LED.rainbowRequest; - } else { - return Settings.LED.solidColorRequest.withColor(this.color); - } + return Settings.LED.solidColorRequest.withColor(this.color); } } @@ -105,19 +101,19 @@ public ControlRequest getAnimation() { public void applyPattern() { if (cachedState != state) { - if (cachedState.getAnimation().getName() != "SolidColor") { - leds.clearAllAnimations(); - } // TODO: daniel's change, double check if works. If not keep calling + // if (cachedState.getAnimation().getName() != "SolidColor") { + // leds.clearAllAnimations(); + // } // clearAllAnimations every loop - if (!(cachedState.getAnimation().getName().equals(state.getAnimation().getName()))) { - this.ledPattern = state.getAnimation(); - } + // if (!(cachedState.getAnimation().getName().equals(state.getAnimation().getName()))) { + // this.ledPattern = state.getAnimation(); + // } - else if (ledPattern instanceof SolidColor) { + // else if (ledPattern instanceof SolidColor) { SolidColor solidColor = (SolidColor) ledPattern; solidColor.withColor(state.getColor()); // SolidColor.class.cast(ledPattern).withColor(null); //change if neccesary - } + // } cachedState = state; } } @@ -155,6 +151,7 @@ public void periodicAfterScheduler() { // reflective of the 3 LED gap between them if (isRightLLDead) { + //TODO: when it goes back on CLEAR ANIMATIONS !! leds.setControl(new SolidColor(Settings.LED.LED_LENGTH - 4, Settings.LED.LED_LENGTH - 1) .withColor(Settings.LED.RIGHTDEAD)); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 3fd1e227..a456a23f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -30,6 +30,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LimelightVision extends SubsystemBase { @@ -49,9 +50,13 @@ public static LimelightVision getInstance() { private int maxTagCount; private MegaTagMode megaTagMode; - private double leftLLHeartbeat = 0.0; //change to -1 is we need to switch the way we do this - private double rightLLHeartbeat = 0.0; - private double backLLHeartbeat = 0.0; + private double leftLLHeartbeat = -1; //change to -1 is we need to switch the way we do this + private double rightLLHeartbeat = -1; + private double backLLHeartbeat = -1; + + private int leftLoopCounter = 0; + private int rightLoopCounter = 0; + private int backLoopCounter = 0; private Pose2d[] limelightPoseArray; @@ -215,10 +220,6 @@ public boolean hasData() { return debouncedHasData.get(); } - public boolean checkDead(double current, double prev) { - return Math.abs(prev - current) < Settings.Vision.MIN_CYCLE_LL_HB; - } - public void periodicAfterScheduler() { if (enabled.get()) { hasData = false; @@ -229,25 +230,55 @@ public void periodicAfterScheduler() { if (Cameras.LimelightCameras[i].isEnabled()) { String limelightName = names[i]; - if (limelightName.equals("limelight-right")) { - //prev - // if (rightLLHeartbeat == LimelightHelpers.getHeartbeat(limelightName) && rightLLHeartbeat != -1) - if (checkDead(LimelightHelpers.getHeartbeat(limelightName), rightLLHeartbeat)) { - LEDController.isRightLLDead = true; + DogLog.log("LED/heartbeat" + limelightName, LimelightHelpers.getHeartbeat(limelightName)); + + if (limelightName.equals(Cameras.LimelightCameras[0].getName())) { + DogLog.log("LED/Right Loop Counter", rightLoopCounter); + DogLog.log("LED/variable heartbeat " + limelightName, leftLLHeartbeat); + rightLoopCounter += 1; + if (rightLoopCounter == 50) { + DogLog.log("LED/Right Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat); + if (LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && rightLLHeartbeat != -1) { + LEDController.isRightLLDead = true; + } + else { + LEDController.isBackLLDead = false; + } + rightLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); + rightLoopCounter = 0; } - rightLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); } - else if (limelightName.equals("limelight-left") && leftLLHeartbeat != -1) { - if (checkDead(LimelightHelpers.getHeartbeat(limelightName), leftLLHeartbeat)) { - LEDController.isLeftLLDead = true; + if (limelightName.equals(Cameras.LimelightCameras[1].getName())) { + DogLog.log("LED/Left Loop Counter", leftLoopCounter); + DogLog.log("LED/variable heartbeat " + limelightName, leftLLHeartbeat); + leftLoopCounter += 1; + if (leftLoopCounter == 50) { + DogLog.log("LED/Left Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat); + if (LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && rightLLHeartbeat != -1) { + LEDController.isLeftLLDead = true; + } + else { + LEDController.isBackLLDead = false; + } + leftLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); + leftLoopCounter = 0; } - leftLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); } - else if (limelightName.equals("limelight-back") && backLLHeartbeat != -1) { - if (checkDead(LimelightHelpers.getHeartbeat(limelightName), backLLHeartbeat)) { - LEDController.isBackLLDead = true; + if (limelightName.equals(Cameras.LimelightCameras[2].getName())) { + DogLog.log("LED/Back Loop Counter", backLoopCounter); + DogLog.log("LED/variable heartbeat " + limelightName, backLLHeartbeat); + backLoopCounter += 1; + if (backLoopCounter == 50) { + DogLog.log("LED/Back Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat); + if (LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && rightLLHeartbeat != -1) { + LEDController.isBackLLDead = true; + } + else { + LEDController.isBackLLDead = false; + } + backLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); + backLoopCounter = 0; } - backLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); } From 7238747f6df56df32dcb49db6b7ffec0e89b1bf6 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 22 May 2026 12:40:31 -0400 Subject: [PATCH 37/97] feat: dead LL LED signals work. Removed a boolean flag. --- .../stuypulse/robot/constants/Settings.java | 4 +++ .../robot/subsystems/leds/LEDController.java | 30 +++++++++++++++++-- .../subsystems/vision/LimelightVision.java | 12 ++++---- 3 files changed, 37 insertions(+), 9 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 398c7484..557a14fb 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -430,6 +430,10 @@ public static RGBWColor rgbwConverter(Color color) { RGBWColor RIGHTDEAD = rgbwConverter(Color.kWhite); RGBWColor BACKDEAD = rgbwConverter(Color.kWhite); + SolidColor RIGHT_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 4, Settings.LED.LED_LENGTH - 1); + SolidColor BACK_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 11, Settings.LED.LED_LENGTH - 8); + SolidColor LEFT_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 18, Settings.LED.LED_LENGTH - 15); + // RGBWColor.gradient(GradientType.kDiscontinuous, Color.kRed, Color.kWhite).scrollAtRelativeSpeed(Percent.per(Second).of(25)); public final int DESIRED_TAGS_WHEN_DISABLED = 2; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 48770ee3..eac38927 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -39,6 +39,9 @@ public class LEDController extends SubsystemBase { public static boolean isLeftLLDead = false; public static boolean isBackLLDead = false; public static boolean isRightLLDead = false; + // public static boolean isLeftLLDeadControlApplied; + // public static boolean isBackLLDeadControlApplied; + // public static boolean isRightLLDeadControlApplied; private Pose2d lastPoseOnAprilTag; private boolean initialPoseUpdated = false; @@ -123,6 +126,11 @@ public void changeState(LEDSTATE state) { } private LEDController() { + + // isLeftLLDeadControlApplied= false; + // isBackLLDeadControlApplied= false; + // isRightLLDeadControlApplied = false; + leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); lastPoseOnAprilTag = new Pose2d(); @@ -152,16 +160,28 @@ public void periodicAfterScheduler() { // reflective of the 3 LED gap between them if (isRightLLDead) { //TODO: when it goes back on CLEAR ANIMATIONS !! - leds.setControl(new SolidColor(Settings.LED.LED_LENGTH - 4, Settings.LED.LED_LENGTH - 1) + leds.setControl(Settings.LED.RIGHT_DEAD_STRIP .withColor(Settings.LED.RIGHTDEAD)); + // isRightLLDeadControlApplied = true; + } else if (!isRightLLDead /*&& isRightLLDeadControlApplied*/) { + leds.clearAllAnimations(); + //isRightLLDeadControlApplied = false; } if (isLeftLLDead) { - leds.setControl(new SolidColor(Settings.LED.LED_LENGTH - 11, Settings.LED.LED_LENGTH - 8) + leds.setControl(Settings.LED.LEFT_DEAD_STRIP .withColor(Settings.LED.LEFTDEAD)); + //isLeftLLDeadControlApplied = true; + } else if (!isLeftLLDead /*&& isLeftLLDeadControlApplied */) { + leds.clearAllAnimations(); + //isLeftLLDeadControlApplied = false; } if (isBackLLDead) { - leds.setControl(new SolidColor(Settings.LED.LED_LENGTH - 18, Settings.LED.LED_LENGTH - 15) + leds.setControl(Settings.LED.BACK_DEAD_STRIP .withColor(Settings.LED.BACKDEAD)); + //isBackLLDeadControlApplied = true; + } else if (!isBackLLDead /*&& isBackLLDeadControlApplied*/) { + leds.clearAllAnimations(); + //isBackLLDeadControlApplied = false; } if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || @@ -184,6 +204,10 @@ public void periodicAfterScheduler() { DogLog.log("LED/State", state.toString()); DogLog.log("LED/Cached State", state.toString()); + + DogLog.log("LED/Is Back LL dead", isBackLLDead); + DogLog.log("LED/Is Right LL dead", isRightLLDead); + DogLog.log("LED/Is Left LL dead", isLeftLLDead); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index a456a23f..63d31f75 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -234,15 +234,15 @@ public void periodicAfterScheduler() { if (limelightName.equals(Cameras.LimelightCameras[0].getName())) { DogLog.log("LED/Right Loop Counter", rightLoopCounter); - DogLog.log("LED/variable heartbeat " + limelightName, leftLLHeartbeat); + DogLog.log("LED/variable heartbeat " + limelightName, rightLLHeartbeat); rightLoopCounter += 1; if (rightLoopCounter == 50) { DogLog.log("LED/Right Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat); - if (LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && rightLLHeartbeat != -1) { + if (LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1) { LEDController.isRightLLDead = true; } else { - LEDController.isBackLLDead = false; + LEDController.isRightLLDead = false; } rightLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); rightLoopCounter = 0; @@ -254,11 +254,11 @@ public void periodicAfterScheduler() { leftLoopCounter += 1; if (leftLoopCounter == 50) { DogLog.log("LED/Left Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat); - if (LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && rightLLHeartbeat != -1) { + if (LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1) { LEDController.isLeftLLDead = true; } else { - LEDController.isBackLLDead = false; + LEDController.isLeftLLDead = false; } leftLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); leftLoopCounter = 0; @@ -270,7 +270,7 @@ public void periodicAfterScheduler() { backLoopCounter += 1; if (backLoopCounter == 50) { DogLog.log("LED/Back Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat); - if (LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && rightLLHeartbeat != -1) { + if (LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && backLLHeartbeat != -1) { LEDController.isBackLLDead = true; } else { From 404f516b10cd80a460c77305903800c5815115de Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 22 May 2026 12:40:31 -0400 Subject: [PATCH 38/97] feat: dead LL LED signals work. Removed a boolean flag. --- .../stuypulse/robot/constants/Settings.java | 4 +++ .../robot/subsystems/leds/LEDController.java | 30 +++++++++++++++++-- .../subsystems/vision/LimelightVision.java | 10 +++---- 3 files changed, 36 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 398c7484..557a14fb 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -430,6 +430,10 @@ public static RGBWColor rgbwConverter(Color color) { RGBWColor RIGHTDEAD = rgbwConverter(Color.kWhite); RGBWColor BACKDEAD = rgbwConverter(Color.kWhite); + SolidColor RIGHT_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 4, Settings.LED.LED_LENGTH - 1); + SolidColor BACK_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 11, Settings.LED.LED_LENGTH - 8); + SolidColor LEFT_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 18, Settings.LED.LED_LENGTH - 15); + // RGBWColor.gradient(GradientType.kDiscontinuous, Color.kRed, Color.kWhite).scrollAtRelativeSpeed(Percent.per(Second).of(25)); public final int DESIRED_TAGS_WHEN_DISABLED = 2; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 48770ee3..eac38927 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -39,6 +39,9 @@ public class LEDController extends SubsystemBase { public static boolean isLeftLLDead = false; public static boolean isBackLLDead = false; public static boolean isRightLLDead = false; + // public static boolean isLeftLLDeadControlApplied; + // public static boolean isBackLLDeadControlApplied; + // public static boolean isRightLLDeadControlApplied; private Pose2d lastPoseOnAprilTag; private boolean initialPoseUpdated = false; @@ -123,6 +126,11 @@ public void changeState(LEDSTATE state) { } private LEDController() { + + // isLeftLLDeadControlApplied= false; + // isBackLLDeadControlApplied= false; + // isRightLLDeadControlApplied = false; + leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); lastPoseOnAprilTag = new Pose2d(); @@ -152,16 +160,28 @@ public void periodicAfterScheduler() { // reflective of the 3 LED gap between them if (isRightLLDead) { //TODO: when it goes back on CLEAR ANIMATIONS !! - leds.setControl(new SolidColor(Settings.LED.LED_LENGTH - 4, Settings.LED.LED_LENGTH - 1) + leds.setControl(Settings.LED.RIGHT_DEAD_STRIP .withColor(Settings.LED.RIGHTDEAD)); + // isRightLLDeadControlApplied = true; + } else if (!isRightLLDead /*&& isRightLLDeadControlApplied*/) { + leds.clearAllAnimations(); + //isRightLLDeadControlApplied = false; } if (isLeftLLDead) { - leds.setControl(new SolidColor(Settings.LED.LED_LENGTH - 11, Settings.LED.LED_LENGTH - 8) + leds.setControl(Settings.LED.LEFT_DEAD_STRIP .withColor(Settings.LED.LEFTDEAD)); + //isLeftLLDeadControlApplied = true; + } else if (!isLeftLLDead /*&& isLeftLLDeadControlApplied */) { + leds.clearAllAnimations(); + //isLeftLLDeadControlApplied = false; } if (isBackLLDead) { - leds.setControl(new SolidColor(Settings.LED.LED_LENGTH - 18, Settings.LED.LED_LENGTH - 15) + leds.setControl(Settings.LED.BACK_DEAD_STRIP .withColor(Settings.LED.BACKDEAD)); + //isBackLLDeadControlApplied = true; + } else if (!isBackLLDead /*&& isBackLLDeadControlApplied*/) { + leds.clearAllAnimations(); + //isBackLLDeadControlApplied = false; } if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || @@ -184,6 +204,10 @@ public void periodicAfterScheduler() { DogLog.log("LED/State", state.toString()); DogLog.log("LED/Cached State", state.toString()); + + DogLog.log("LED/Is Back LL dead", isBackLLDead); + DogLog.log("LED/Is Right LL dead", isRightLLDead); + DogLog.log("LED/Is Left LL dead", isLeftLLDead); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index a456a23f..9e0640f3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -234,7 +234,7 @@ public void periodicAfterScheduler() { if (limelightName.equals(Cameras.LimelightCameras[0].getName())) { DogLog.log("LED/Right Loop Counter", rightLoopCounter); - DogLog.log("LED/variable heartbeat " + limelightName, leftLLHeartbeat); + DogLog.log("LED/variable heartbeat " + limelightName, rightLLHeartbeat); rightLoopCounter += 1; if (rightLoopCounter == 50) { DogLog.log("LED/Right Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat); @@ -242,7 +242,7 @@ public void periodicAfterScheduler() { LEDController.isRightLLDead = true; } else { - LEDController.isBackLLDead = false; + LEDController.isRightLLDead = false; } rightLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); rightLoopCounter = 0; @@ -254,11 +254,11 @@ public void periodicAfterScheduler() { leftLoopCounter += 1; if (leftLoopCounter == 50) { DogLog.log("LED/Left Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat); - if (LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && rightLLHeartbeat != -1) { + if (LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1) { LEDController.isLeftLLDead = true; } else { - LEDController.isBackLLDead = false; + LEDController.isLeftLLDead = false; } leftLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); leftLoopCounter = 0; @@ -270,7 +270,7 @@ public void periodicAfterScheduler() { backLoopCounter += 1; if (backLoopCounter == 50) { DogLog.log("LED/Back Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat); - if (LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && rightLLHeartbeat != -1) { + if (LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && backLLHeartbeat != -1) { LEDController.isBackLLDead = true; } else { From da36de99d40c93511790d093c97383452e1c7230 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 22 May 2026 15:39:21 -0400 Subject: [PATCH 39/97] FEAT: Led reset state --- .../com/stuypulse/robot/RobotContainer.java | 7 ++-- .../robot/commands/leds/LEDApplyState.java | 6 ++-- .../commands/leds/LEDDefaultCommand.java | 34 ++++++++----------- .../stuypulse/robot/constants/Settings.java | 2 +- .../robot/subsystems/leds/LEDController.java | 12 +++---- 5 files changed, 30 insertions(+), 31 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 666e124f..1d7697cd 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -80,7 +80,7 @@ import com.stuypulse.robot.subsystems.intake.Intake.PivotState; import com.stuypulse.robot.subsystems.intake.Intake.RollerState; import com.stuypulse.robot.subsystems.leds.LEDController; -import com.stuypulse.robot.subsystems.leds.LEDController.LEDSTATE; +import com.stuypulse.robot.subsystems.leds.LEDController.LedState; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -99,12 +99,14 @@ import com.stuypulse.stuylib.network.SmartNumber; import dev.doglog.DogLog; +import dev.doglog.internal.TimedCommand; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -304,7 +306,8 @@ private void configureButtonBindings() { driver.getDPadRight() .onTrue(new SuperstructureStow() .alongWith(new HandoffStop()) - .alongWith(new SpindexerStop())); + .alongWith(new SpindexerStop())) + .onTrue(new LEDApplyState(LedState.RESET).repeatedly().withTimeout(2.0)); // Manual Left Corner Scoring driver.getLeftButton() diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java index 0c89daaa..74f1ba0f 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java @@ -17,14 +17,14 @@ import com.ctre.phoenix6.controls.ControlRequest; import com.stuypulse.robot.subsystems.leds.LEDController; -import com.stuypulse.robot.subsystems.leds.LEDController.LEDSTATE; +import com.stuypulse.robot.subsystems.leds.LEDController.LedState; public class LEDApplyState extends InstantCommand { //This will not work as default command will override it. Either make the cached class the same as this OR (better solution) have a boolean that tells you if it is manually applied or not and if it is then default command dont change protected final LEDController leds; - protected final LEDSTATE state; + protected final LedState state; - public LEDApplyState(LEDSTATE state) { + public LEDApplyState(LedState state) { leds = LEDController.getInstance(); this.state = state; diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java index 458ed95e..db551aa1 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java @@ -18,7 +18,7 @@ import com.stuypulse.robot.subsystems.intake.Intake.PivotState; import com.stuypulse.robot.subsystems.intake.Intake.RollerState; import com.stuypulse.robot.subsystems.leds.LEDController; -import com.stuypulse.robot.subsystems.leds.LEDController.LEDSTATE; +import com.stuypulse.robot.subsystems.leds.LEDController.LedState; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -62,49 +62,45 @@ public LEDDefaultCommand() { @Override public void initialize() { - // String state = "NONE"; - if (Robot.getMode() == RobotMode.DISABLED) { if (LimelightVision.getInstance().getMaxTagCount() >= Settings.LED.DESIRED_TAGS_WHEN_DISABLED) { - leds.changeState(LEDSTATE.DISABLED_ALIGNED); + leds.changeState(LedState.DISABLED_ALIGNED); } else { - leds.changeState(LEDSTATE.DISABLED); + leds.changeState(LedState.DISABLED); } } else { if (swerve.isUnderTrench()) { - leds.changeState(LEDSTATE.PASSING_TRENCH); + leds.changeState(LedState.PASSING_TRENCH); } else if (turret.isWrapping()) { - leds.changeState(LEDSTATE.TURRET_WRAPPING); + leds.changeState(LedState.TURRET_WRAPPING); } else if (superstructure.getState() == SuperstructureState.LEFT_CORNER) { - leds.changeState(LEDSTATE.LEFT_CORNER); + leds.changeState(LedState.LEFT_CORNER); } else if (superstructure.getState() == SuperstructureState.RIGHT_CORNER) { - leds.changeState(LEDSTATE.RIGHT_CORNER); + leds.changeState(LedState.RIGHT_CORNER); } else if (superstructure.getState() == SuperstructureState.KB) { - leds.changeState(LEDSTATE.KB_DISTANCE); + leds.changeState(LedState.KB_DISTANCE); } else if (superstructure.getState() == SuperstructureState.SOTM) { - leds.changeState(LEDSTATE.SOTM_ON); + leds.changeState(LedState.SOTM_ON); } else if (superstructure.getState() == SuperstructureState.FOTM) { - leds.changeState(LEDSTATE.FOTM_ON); + leds.changeState(LedState.FOTM_ON); } - // else if (spindexer.getState() == SpindexerState.REVERSE || - // handoff.getState() == HandoffState.REVERSE || - // intake.getRollerState() == RollerState.OUTTAKE) { - // leds.changeState(LEDSTATE.REVERSE); - // } + // else if (superstructure.getState() == SuperstructureState.STOW) { + // leds.changeState(LedState.RESET); + // } else if (intake.getPivotState() == PivotState.STOW) { - leds.changeState(LEDSTATE.INTAKE_STOW); + leds.changeState(LedState.INTAKE_STOW); } else if (intake.getPivotState() == PivotState.DEPLOY) { - leds.changeState(LEDSTATE.INTAKE_DEPLOYED); + leds.changeState(LedState.INTAKE_DEPLOYED); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 557a14fb..449d9cab 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -421,7 +421,7 @@ public static RGBWColor rgbwConverter(Color color) { RGBWColor X_WHEELS = rgbwConverter(Color.kRed); RGBWColor INTAKE_STOW = rgbwConverter(Color.kBrown); //broken - RGBWColor INTAKE_DEPLOYED = rgbwConverter(Color.kOrange); //broken + RGBWColor INTAKE_DEPLOYED = rgbwConverter(Color.kGray); //broken RGBWColor DISABLED_ALIGNED = rgbwConverter(Color.kGreen); RGBWColor DISABLED = rgbwConverter(Color.kRed); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index eac38927..46f217af 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -64,7 +64,7 @@ public static LEDController getInstance() { // one way to go further with the flashing aspect is make it flash faster over // DISTANCE (since last tag was seen) rather than time - public enum LEDSTATE { + public enum LedState { PASSING_TRENCH(Settings.LED.PASSING_TRENCH), IS_BEHIND_HUB(Settings.LED.IS_BEHIND_HUB), TURRET_WRAPPING(Settings.LED.TURRET_WRAPPING), @@ -75,7 +75,7 @@ public enum LEDSTATE { RIGHT_CORNER(Settings.LED.RIGHT_CORNER), KB_DISTANCE(Settings.LED.KB_DISTANCE), STOP_ROLLERS(Settings.LED.STOP_ROLLERS), - RESET_HEADING(Settings.LED.RESET_HEADING), + RESET(Settings.LED.RESET_HEADING), X_WHEELS(Settings.LED.X_WHEELS), INTAKE_STOW(Settings.LED.INTAKE_STOW), INTAKE_DEPLOYED(Settings.LED.INTAKE_DEPLOYED), @@ -84,7 +84,7 @@ public enum LEDSTATE { private RGBWColor color; - private LEDSTATE(RGBWColor color) { + private LedState(RGBWColor color) { this.color = color; } @@ -97,8 +97,8 @@ public ControlRequest getAnimation() { } } - private LEDSTATE state = LEDSTATE.DISABLED; - private LEDSTATE cachedState = LEDSTATE.DISABLED; + private LedState state = LedState.DISABLED; + private LedState cachedState = LedState.DISABLED; // CHANGE apply pattern command to change state @@ -121,7 +121,7 @@ public void applyPattern() { } } - public void changeState(LEDSTATE state) { + public void changeState(LedState state) { this.state = state; } From 753492e1287e8389f14288d4b1645cdaeb53a04b Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Tue, 26 May 2026 12:27:27 -0400 Subject: [PATCH 40/97] feat: Left One Cycle auton (no code yet) --- .../pathplanner/autos/BC Left One Cycle.auto | 43 ++++++ .../pathplanner/autos/BC Left Two Cycle.auto | 55 +++++++ .../pathplanner/paths/Left Corner to Dot.path | 77 ++++++++++ .../pathplanner/paths/Left Dot to Middle.path | 81 +++++++++++ .../pathplanner/paths/Omit Second Shot.path | 137 ++++++++++++++++++ .../paths/Short Left Score To Corner.path | 54 +++++++ 6 files changed, 447 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/BC Left One Cycle.auto create mode 100644 src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto create mode 100644 src/main/deploy/pathplanner/paths/Left Corner to Dot.path create mode 100644 src/main/deploy/pathplanner/paths/Left Dot to Middle.path create mode 100644 src/main/deploy/pathplanner/paths/Omit Second Shot.path create mode 100644 src/main/deploy/pathplanner/paths/Short Left Score To Corner.path diff --git a/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto new file mode 100644 index 00000000..8029eb98 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Short Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Omit Second Shot" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Dot to Middle" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto new file mode 100644 index 00000000..1308a1cb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Short Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Short Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Corner to Dot" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Dot to Middle" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Battle Cry", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner to Dot.path b/src/main/deploy/pathplanner/paths/Left Corner to Dot.path new file mode 100644 index 00000000..0e9f8c29 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner to Dot.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.308, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.128246869413065, + "y": 7.53157423971269 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 7.441 + }, + "prevControl": { + "x": 7.053320214672454, + "y": 6.979910554560632 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.7914438502673795, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7525423728813557, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "BC paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Dot to Middle.path b/src/main/deploy/pathplanner/paths/Left Dot to Middle.path new file mode 100644 index 00000000..c3f271d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Dot to Middle.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.254, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.468955322425778, + "y": 7.441 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 4.07556350626118 + }, + "prevControl": { + "x": 4.535578251580112, + "y": 3.6328315855227107 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2994652406417112, + "rotationDegrees": -117.87026328105652 + }, + { + "waypointRelativePos": 0.6925133689839572, + "rotationDegrees": -51.403799535528066 + }, + { + "waypointRelativePos": 0.9147121535181245, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.845374746792703, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "BC paths", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Omit Second Shot.path b/src/main/deploy/pathplanner/paths/Omit Second Shot.path new file mode 100644 index 00000000..447196ec --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Omit Second Shot.path @@ -0,0 +1,137 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.3075858778625955, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 7.2996636803917525, + "y": 7.440906488549619 + }, + "isLocked": false, + "linkedName": "Left Corner" + }, + { + "anchor": { + "x": 6.006, + "y": 5.154194008559203 + }, + "prevControl": { + "x": 6.006, + "y": 7.340827389443653 + }, + "nextControl": { + "x": 6.006, + "y": 4.22180224083617 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.207855555555556, + "y": 4.9314888888888895 + }, + "prevControl": { + "x": 6.941432423475657, + "y": 4.554048939637246 + }, + "nextControl": { + "x": 7.428987161198288, + "y": 5.244764621968616 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.946533523537802, + "y": 6.460998573466477 + }, + "prevControl": { + "x": 7.899066186613268, + "y": 6.028698234305561 + }, + "nextControl": { + "x": 7.994000860462336, + "y": 6.893298912627393 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.26, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 8.126490727532095, + "y": 7.198502139800285 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1891117478510029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.3601659751037194, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.658921161825727, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.219851116625306, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -114.069 + }, + "reversed": false, + "folder": "BC paths", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path b/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path new file mode 100644 index 00000000..2849c12f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6120827389443653, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 3.2000538693416494, + "y": 7.4888359978960555 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 3.2824809160305346, + "y": 7.441 + }, + "prevControl": { + "x": 3.516912705949025, + "y": 7.354156831727459 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Corner" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.15, + "maxAcceleration": 4.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file From 4365eb91e1e7af885a0776c82e8eab1811a1335d Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Tue, 26 May 2026 12:27:27 -0400 Subject: [PATCH 41/97] feat: Left One Cycle auton (no code yet) --- .../autos/BC Left Corner Bite.auto | 49 +++++++ .../pathplanner/autos/BC Left One Cycle.auto | 43 ++++++ .../pathplanner/autos/BC Left Two Cycle.auto | 55 +++++++ .../pathplanner/paths/Left Corner to Dot.path | 77 ++++++++++ .../pathplanner/paths/Left Dot to Middle.path | 81 +++++++++++ .../pathplanner/paths/Omit Second Shot.path | 137 ++++++++++++++++++ .../paths/Short Left Score To Corner.path | 54 +++++++ 7 files changed, 496 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto create mode 100644 src/main/deploy/pathplanner/autos/BC Left One Cycle.auto create mode 100644 src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto create mode 100644 src/main/deploy/pathplanner/paths/Left Corner to Dot.path create mode 100644 src/main/deploy/pathplanner/paths/Left Dot to Middle.path create mode 100644 src/main/deploy/pathplanner/paths/Omit Second Shot.path create mode 100644 src/main/deploy/pathplanner/paths/Short Left Score To Corner.path diff --git a/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto new file mode 100644 index 00000000..691348fc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Short Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Corner to Dot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto new file mode 100644 index 00000000..8029eb98 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Short Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Omit Second Shot" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Dot to Middle" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto new file mode 100644 index 00000000..1308a1cb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Short Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Short Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Corner to Dot" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Dot to Middle" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Battle Cry", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner to Dot.path b/src/main/deploy/pathplanner/paths/Left Corner to Dot.path new file mode 100644 index 00000000..0e9f8c29 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner to Dot.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.308, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.128246869413065, + "y": 7.53157423971269 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 7.441 + }, + "prevControl": { + "x": 7.053320214672454, + "y": 6.979910554560632 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.7914438502673795, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7525423728813557, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "BC paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Dot to Middle.path b/src/main/deploy/pathplanner/paths/Left Dot to Middle.path new file mode 100644 index 00000000..c3f271d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Dot to Middle.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.254, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.468955322425778, + "y": 7.441 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 4.07556350626118 + }, + "prevControl": { + "x": 4.535578251580112, + "y": 3.6328315855227107 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2994652406417112, + "rotationDegrees": -117.87026328105652 + }, + { + "waypointRelativePos": 0.6925133689839572, + "rotationDegrees": -51.403799535528066 + }, + { + "waypointRelativePos": 0.9147121535181245, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.845374746792703, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "BC paths", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Omit Second Shot.path b/src/main/deploy/pathplanner/paths/Omit Second Shot.path new file mode 100644 index 00000000..447196ec --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Omit Second Shot.path @@ -0,0 +1,137 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.3075858778625955, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 7.2996636803917525, + "y": 7.440906488549619 + }, + "isLocked": false, + "linkedName": "Left Corner" + }, + { + "anchor": { + "x": 6.006, + "y": 5.154194008559203 + }, + "prevControl": { + "x": 6.006, + "y": 7.340827389443653 + }, + "nextControl": { + "x": 6.006, + "y": 4.22180224083617 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.207855555555556, + "y": 4.9314888888888895 + }, + "prevControl": { + "x": 6.941432423475657, + "y": 4.554048939637246 + }, + "nextControl": { + "x": 7.428987161198288, + "y": 5.244764621968616 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.946533523537802, + "y": 6.460998573466477 + }, + "prevControl": { + "x": 7.899066186613268, + "y": 6.028698234305561 + }, + "nextControl": { + "x": 7.994000860462336, + "y": 6.893298912627393 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.26, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 8.126490727532095, + "y": 7.198502139800285 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1891117478510029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.3601659751037194, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.658921161825727, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.219851116625306, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -114.069 + }, + "reversed": false, + "folder": "BC paths", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path b/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path new file mode 100644 index 00000000..2849c12f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6120827389443653, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 3.2000538693416494, + "y": 7.4888359978960555 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 3.2824809160305346, + "y": 7.441 + }, + "prevControl": { + "x": 3.516912705949025, + "y": 7.354156831727459 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Corner" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.15, + "maxAcceleration": 4.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file From 75f70bbbd2e60cde1bef7bff31e61413c67b7951 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Tue, 26 May 2026 14:42:34 -0400 Subject: [PATCH 42/97] feat: BC right corner bite auton --- .../pathplanner/autos/BC Left Two Cycle.auto | 2 +- .../autos/BC Right Corner Bite.auto | 49 ++++++++++++ .../pathplanner/paths/Omit Second Shot.path | 20 ++--- .../paths/Right Bite Score To Score.path | 10 +-- .../paths/Right Corner To Dot.path | 77 +++++++++++++++++++ .../pathplanner/paths/Right NZ To Score.path | 4 +- .../paths/Right Score To Corner.path | 8 +- .../paths/Right Score To NZ (F).path | 4 +- .../paths/Right Score To Score.path | 10 +-- .../paths/Right Shallow To Score.path | 6 +- .../paths/Short Left Score To Corner.path | 6 +- 11 files changed, 161 insertions(+), 35 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto create mode 100644 src/main/deploy/pathplanner/paths/Right Corner To Dot.path diff --git a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto index 1308a1cb..7746e9f2 100644 --- a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto @@ -50,6 +50,6 @@ } }, "resetOdom": true, - "folder": "Battle Cry", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto new file mode 100644 index 00000000..ae7fee0b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Bite Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Corner To Dot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Omit Second Shot.path b/src/main/deploy/pathplanner/paths/Omit Second Shot.path index 447196ec..60be0894 100644 --- a/src/main/deploy/pathplanner/paths/Omit Second Shot.path +++ b/src/main/deploy/pathplanner/paths/Omit Second Shot.path @@ -4,15 +4,15 @@ { "anchor": { "x": 3.3075858778625955, - "y": 7.440906488549619 + "y": 7.441 }, "prevControl": null, "nextControl": { "x": 7.2996636803917525, - "y": 7.440906488549619 + "y": 7.441 }, "isLocked": false, - "linkedName": "Left Corner" + "linkedName": null }, { "anchor": { @@ -64,16 +64,16 @@ }, { "anchor": { - "x": 8.26, - "y": 7.440906488549619 + "x": 8.2, + "y": 7.441 }, "prevControl": { - "x": 8.126490727532095, - "y": 7.198502139800285 + "x": 8.066490727532095, + "y": 7.198595651250666 }, "nextControl": null, "isLocked": false, - "linkedName": "Left Trench Score" + "linkedName": null } ], "rotationTargets": [ @@ -125,10 +125,10 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": -114.069 + "rotation": 0.0 }, "reversed": false, - "folder": "BC paths", + "folder": null, "idealStartingState": { "velocity": 0.0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path index a3ace306..631cbbfe 100644 --- a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path @@ -4,12 +4,12 @@ { "anchor": { "x": 3.2824809160305346, - "y": 0.5868473609129818 + "y": 0.559 }, "prevControl": null, "nextControl": { "x": 5.236218433862203, - "y": 0.5997860199714706 + "y": 0.571938659058489 }, "isLocked": false, "linkedName": "Right Corner" @@ -57,7 +57,7 @@ }, "nextControl": { "x": 6.070427960057061, - "y": 0.15987161198288047 + "y": 0.15987161198288025 }, "isLocked": false, "linkedName": null @@ -65,11 +65,11 @@ { "anchor": { "x": 3.6120827389443653, - "y": 0.5868473609129818 + "y": 0.559 }, "prevControl": { "x": 6.1480599144079875, - "y": 0.5739087018544944 + "y": 0.5460613409415132 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path new file mode 100644 index 00000000..db6dc40d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.308, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.109243937232525, + "y": 0.6774179743223976 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 0.559 + }, + "prevControl": { + "x": 8.500236542443773, + "y": 0.602215334840027 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3795309168443479, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.75, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.75, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index d6d029a1..2da238c0 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -33,11 +33,11 @@ { "anchor": { "x": 3.6120827389443653, - "y": 0.5868473609129818 + "y": 0.559 }, "prevControl": { "x": 6.311366666666668, - "y": 0.5562333333333322 + "y": 0.5283859724203506 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Corner.path b/src/main/deploy/pathplanner/paths/Right Score To Corner.path index 385b6020..f5ea9d15 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Corner.path @@ -4,12 +4,12 @@ { "anchor": { "x": 3.6120827389443653, - "y": 0.5868473609129818 + "y": 0.559 }, "prevControl": null, "nextControl": { "x": 3.2000538693416494, - "y": 0.6346833588090371 + "y": 0.6068359978960558 }, "isLocked": false, "linkedName": "Right Trench Score" @@ -17,11 +17,11 @@ { "anchor": { "x": 3.2824809160305346, - "y": 0.5868473609129818 + "y": 0.559 }, "prevControl": { "x": 3.516912705949025, - "y": 0.5000041926404413 + "y": 0.47215683172745937 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 9882af47..96401506 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -4,12 +4,12 @@ { "anchor": { "x": 3.2824809160305346, - "y": 0.5868473609129818 + "y": 0.559 }, "prevControl": null, "nextControl": { "x": 6.853550816173187, - "y": 0.5739087018544944 + "y": 0.5460613409415132 }, "isLocked": false, "linkedName": "Right Corner" diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index d2b56676..7cbce93a 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -4,12 +4,12 @@ { "anchor": { "x": 3.2824809160305346, - "y": 0.5868473609129818 + "y": 0.559 }, "prevControl": null, "nextControl": { "x": 6.885563480741797, - "y": 0.5480313837375184 + "y": 0.5201840228245365 }, "isLocked": false, "linkedName": "Right Corner" @@ -49,11 +49,11 @@ { "anchor": { "x": 3.6120827389443653, - "y": 0.5868473609129818 + "y": 0.559 }, "prevControl": { - "x": 7.673089129599261, - "y": 0.5487960706826538 + "x": 7.67308912959926, + "y": 0.5209487097696721 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path index ea1e749d..be0232ff 100644 --- a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 6.419771754636235, - "y": 0.3410128388017126 + "y": 0.34101283880171307 }, "isLocked": false, "linkedName": "Right Shallow NZ" @@ -17,11 +17,11 @@ { "anchor": { "x": 3.6120827389443653, - "y": 0.5868473609129818 + "y": 0.559 }, "prevControl": { "x": 6.018673323823109, - "y": 0.392767475035663 + "y": 0.36492011412268166 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path b/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path index 2849c12f..f2df6d4a 100644 --- a/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path @@ -12,7 +12,7 @@ "y": 7.4888359978960555 }, "isLocked": false, - "linkedName": "Right Trench Score" + "linkedName": null }, { "anchor": { @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Right Corner" + "linkedName": null } ], "rotationTargets": [], @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "BC paths", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 From bcda77810f22f056dfc25a76eda1948d467ee578 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Tue, 26 May 2026 15:23:02 -0400 Subject: [PATCH 43/97] feat: burger autons --- .../com/stuypulse/robot/RobotContainer.java | 26 +++++++------------ 1 file changed, 10 insertions(+), 16 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1d7697cd..ae719bf1 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -11,29 +11,26 @@ import com.stuypulse.robot.commands.auton.regular.LeftBump; import com.stuypulse.robot.commands.auton.regular.LeftFollow; import com.stuypulse.robot.commands.auton.regular.LeftTwoCorner; +import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerBC; import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerShallow; import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; import com.stuypulse.robot.commands.auton.regular.RightBump; import com.stuypulse.robot.commands.auton.regular.RightFollow; import com.stuypulse.robot.commands.auton.regular.RightTwoCorner; +import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBC; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerShallow; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; -import com.stuypulse.robot.commands.auton.test.BoxTest; -import com.stuypulse.robot.commands.auton.test.EmptyTest; -import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hood.HomingRoutineLower; import com.stuypulse.robot.commands.hood.HomingRoutineUpper; import com.stuypulse.robot.commands.hood.SeedHoodRelativeEncoderAtLowerHardstop; import com.stuypulse.robot.commands.hood.SeedHoodRelativeEncoderAtUpperHardstop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeOuttake; import com.stuypulse.robot.commands.intake.IntakeRunRollers; -import com.stuypulse.robot.commands.intake.IntakeSetState; import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; import com.stuypulse.robot.commands.intake.IntakeTeleopDigest; @@ -41,14 +38,12 @@ import com.stuypulse.robot.commands.intake.SeedPivotStowed; import com.stuypulse.robot.commands.leds.LEDApplyState; import com.stuypulse.robot.commands.leds.LEDDefaultCommand; -import com.stuypulse.robot.commands.spindexer.SpindexerReverse; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureCacheState; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; import com.stuypulse.robot.commands.superstructure.SuperstructureLeftCorner; -import com.stuypulse.robot.commands.superstructure.SuperstructureManualOverride; import com.stuypulse.robot.commands.superstructure.SuperstructureRightCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.superstructure.SuperstructureStow; @@ -56,7 +51,6 @@ import com.stuypulse.robot.commands.swerve.SwerveDriveFOTM; import com.stuypulse.robot.commands.swerve.SwerveDriveSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; import com.stuypulse.robot.commands.swerve.SwerveResetPoseKBShot; import com.stuypulse.robot.commands.swerve.SwerveResetPoseLeftCorner; import com.stuypulse.robot.commands.swerve.SwerveResetPoseRightCorner; @@ -73,12 +67,9 @@ import com.stuypulse.robot.constants.Cameras.Camera.Pipeline; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.intake.Intake.PivotState; -import com.stuypulse.robot.subsystems.intake.Intake.RollerState; import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.leds.LEDController.LedState; import com.stuypulse.robot.subsystems.spindexer.Spindexer; @@ -92,27 +83,22 @@ import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; import com.stuypulse.robot.util.PathUtil.AutonConfig; -import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; import dev.doglog.DogLog; -import dev.doglog.internal.TimedCommand; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; public class RobotContainer { @@ -439,10 +425,18 @@ public void configureAutons() { "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); LEFT_TWO_CORNER.register(autonChooser); + AutonConfig LEFT_TWO_CORNER_BC = new AutonConfig("BC Left Two Corner", LeftTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)", "Left Corner To Dot", "Short Left Score to Corner"); + LEFT_TWO_CORNER_BC.register(autonChooser); + AutonConfig RIGHT_TWO_CORNER = new AutonConfig("Right Two Corner", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CORNER.register(autonChooser); + AutonConfig RIGHT_TWO_CORNER_BC = new AutonConfig("BC Right Two Corner", RightTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)", "Right Corner To Dot", "Short Right Score To Corner"); + RIGHT_TWO_CORNER_BC.register(autonChooser); + AutonConfig LEFT_TWO_CORNER_SHALLOW = new AutonConfig("Left Two Corner Shallow", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, "Left To Shallow", "Left Shallow To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); LEFT_TWO_CORNER_SHALLOW.register(autonChooser); From c4914e9b56900da4cd9108b7fc48547a56ab99ad Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Tue, 26 May 2026 15:57:45 -0400 Subject: [PATCH 44/97] daniel forgot these --- .../paths/Short Right Score To Corner.path | 54 ++++++++++++ .../auton/regular/LeftTwoCornerBC.java | 86 +++++++++++++++++++ .../auton/regular/RightTwoCornerBC.java | 86 +++++++++++++++++++ 3 files changed, 226 insertions(+) create mode 100644 src/main/deploy/pathplanner/paths/Short Right Score To Corner.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java diff --git a/src/main/deploy/pathplanner/paths/Short Right Score To Corner.path b/src/main/deploy/pathplanner/paths/Short Right Score To Corner.path new file mode 100644 index 00000000..48298e16 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Short Right Score To Corner.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 3.2000538693416494, + "y": 0.6068359978960558 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.2824809160305346, + "y": 0.559 + }, + "prevControl": { + "x": 3.516912705949025, + "y": 0.47215683172745937 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.15, + "maxAcceleration": 4.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java new file mode 100644 index 00000000..83d071e8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java @@ -0,0 +1,86 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class LeftTwoCornerBC extends SequentialCommandGroup { + + public LeftTwoCornerBC(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), //changed to 4 bcs of delay (from 5) + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().withTimeout(1.5)), //cut this down + new WaitCommand(1.0) //cut this down + ), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java new file mode 100644 index 00000000..0630ddf6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java @@ -0,0 +1,86 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightTwoCornerBC extends SequentialCommandGroup { + + public RightTwoCornerBC(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), //changed to 4 bcs of delay (from 5) + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().withTimeout(4.0)), //cut this down + new WaitCommand(1.0) //cut this down + ), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]) + + ); + + } + +} From 64f07822d243ae421326c442f3571def98852728 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 26 May 2026 16:43:04 -0400 Subject: [PATCH 45/97] feat: corner to side dots + test auton --- .../autos/BC Left Corner Bite.auto | 2 +- .../pathplanner/autos/BC Left Two Cycle.auto | 6 -- .../pathplanner/autos/BC to Dot test.auto | 19 ++++ .../paths/Left Corner to Center Dot.path | 97 +++++++++++++++++++ .../paths/Left Corner to Dot v2.path | 97 +++++++++++++++++++ .../paths/Right Corner to Dot v2.path | 97 +++++++++++++++++++ .../paths/Short Left Score To Corner.path | 14 +-- .../com/stuypulse/robot/RobotContainer.java | 4 + .../robot/commands/auton/test/TestBC.java | 27 ++++++ 9 files changed, 349 insertions(+), 14 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/BC to Dot test.auto create mode 100644 src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path create mode 100644 src/main/deploy/pathplanner/paths/Left Corner to Dot v2.path create mode 100644 src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/test/TestBC.java diff --git a/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto index 691348fc..cf53fc5d 100644 --- a/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto @@ -31,7 +31,7 @@ { "type": "path", "data": { - "pathName": "Short Left Score To Corner" + "pathName": "Left Score To Corner" } }, { diff --git a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto index 7746e9f2..35dae601 100644 --- a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto @@ -39,12 +39,6 @@ "data": { "pathName": "Left Corner to Dot" } - }, - { - "type": "path", - "data": { - "pathName": "Left Dot to Middle" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/BC to Dot test.auto b/src/main/deploy/pathplanner/autos/BC to Dot test.auto new file mode 100644 index 00000000..f351c702 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC to Dot test.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Corner to Dot v2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path b/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path new file mode 100644 index 00000000..8366ea77 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path @@ -0,0 +1,97 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.308, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.911440798858774, + "y": 8.117146932952926 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 5.542353780313838 + }, + "prevControl": { + "x": 8.20530670470756, + "y": 6.448059914407988 + }, + "nextControl": { + "x": 8.27821986421368, + "y": 5.091858913340555 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 4.00265335235378 + }, + "prevControl": { + "x": 8.254, + "y": 3.7526533523537795 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3837953091684457, + "rotationDegrees": -35.0 + }, + { + "waypointRelativePos": 0.6652452025586366, + "rotationDegrees": -33.0 + }, + { + "waypointRelativePos": 1.35, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.5, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "BC Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner to Dot v2.path b/src/main/deploy/pathplanner/paths/Left Corner to Dot v2.path new file mode 100644 index 00000000..3711b812 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner to Dot v2.path @@ -0,0 +1,97 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.308, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.911440798858774, + "y": 8.117146932952926 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 6.072838801711842 + }, + "prevControl": { + "x": 6.86861310177917, + "y": 5.172517037410415 + }, + "nextControl": { + "x": 8.632282453637659, + "y": 6.31867332382311 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 7.441 + }, + "prevControl": { + "x": 8.254, + "y": 7.190999999999999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3837953091684457, + "rotationDegrees": -35.0 + }, + { + "waypointRelativePos": 0.6652452025586366, + "rotationDegrees": -33.0 + }, + { + "waypointRelativePos": 1.35, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.5, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "BC Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path b/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path new file mode 100644 index 00000000..dbc288ce --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path @@ -0,0 +1,97 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.308, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.911445078005017, + "y": -0.11712412737826428 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 1.927 + }, + "prevControl": { + "x": 6.868618724968162, + "y": 2.827330417029173 + }, + "nextControl": { + "x": 8.63228091821551, + "y": 1.6811631152454245 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 0.759 + }, + "prevControl": { + "x": 8.254, + "y": 1.0090000000000008 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3837953091684457, + "rotationDegrees": 35.0 + }, + { + "waypointRelativePos": 0.6652452025586366, + "rotationDegrees": 33.0 + }, + { + "waypointRelativePos": 1.35, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.5, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "BC Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path b/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path index f2df6d4a..4e494dff 100644 --- a/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 3.2000538693416494, - "y": 7.4888359978960555 + "x": 3.1972863164901653, + "y": 7.441 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.2824809160305346, + "x": 3.482, "y": 7.441 }, "prevControl": { - "x": 3.516912705949025, - "y": 7.354156831727459 + "x": 3.732, + "y": 7.441 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.15, + "maxVelocity": 0.1, "maxAcceleration": 4.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": null, + "folder": "BC Dot", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index ae719bf1..3ae927f5 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -22,6 +22,7 @@ import com.stuypulse.robot.commands.auton.regular.RightTwoCornerShallow; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; +import com.stuypulse.robot.commands.auton.test.TestBC; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hood.HomingRoutineLower; @@ -420,6 +421,9 @@ public void configureAutons() { "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CYCLE.register(autonChooser); + AutonConfig BC_TEST = new AutonConfig("BC Test", TestBC::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner to Dot v2"); + BC_TEST.register(autonChooser); + // TWO CYCLES (CORNER) AutonConfig LEFT_TWO_CORNER = new AutonConfig("Left Two Corner", LeftTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/TestBC.java b/src/main/java/com/stuypulse/robot/commands/auton/test/TestBC.java new file mode 100644 index 00000000..ab9dc102 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/TestBC.java @@ -0,0 +1,27 @@ +package com.stuypulse.robot.commands.auton.test; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class TestBC extends SequentialCommandGroup { + + public TestBC(PathPlannerPath... paths) { + + addCommands( + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0].name).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy())) + ); + + } +} \ No newline at end of file From 2efd98ed4a2083464b860fd3da9771eb78a3c8e4 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 26 May 2026 16:51:55 -0400 Subject: [PATCH 46/97] feat: partial changes --- .../paths/Left Corner to Center Dot.path | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path b/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path index 8366ea77..d4feffc3 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path +++ b/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 6.911440798858774, - "y": 8.117146932952926 + "x": 8.593466476462194, + "y": 7.690171184022825 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.254, - "y": 5.542353780313838 + "x": 6.212753209700427, + "y": 5.231825962910129 }, "prevControl": { - "x": 8.20530670470756, - "y": 6.448059914407988 + "x": 5.552881597717547, + "y": 5.930513552068473 }, "nextControl": { - "x": 8.27821986421368, - "y": 5.091858913340555 + "x": 7.277238840530277, + "y": 4.1047235302667575 }, "isLocked": false, "linkedName": null From 40f98235c158e68a19d877e49669465e79596d68 Mon Sep 17 00:00:00 2001 From: Apetrock Date: Tue, 26 May 2026 17:26:33 -0400 Subject: [PATCH 47/97] FEAT: Center auto --- build.gradle | 2 +- .../paths/Left Corner to Center Dot.path | 50 +++-------- .../paths/Right Corner To Center Dot.path | 70 +++++++++++++++ src/main/java/com/stuypulse/robot/Robot.java | 1 - .../com/stuypulse/robot/RobotContainer.java | 14 ++- .../auton/regular/LeftTwoCornerBCCenter.java | 86 +++++++++++++++++++ .../auton/regular/RightTwoCornerBCCenter.java | 86 +++++++++++++++++++ 7 files changed, 269 insertions(+), 40 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java diff --git a/build.gradle b/build.gradle index 057c2477..0ea9f06f 100644 --- a/build.gradle +++ b/build.gradle @@ -12,7 +12,7 @@ allprojects { // Set this to the latest version of StuyLib. // You can check here: https://github.com/StuyPulse/StuyLib/releases. -final String STUYLIB_VERSION = '2026.1.1-SNAPSHOT' +final String STUYLIB_VERSION = '61cedd1b41' def ROBOT_MAIN_CLASS = "com.stuypulse.robot.Main" diff --git a/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path b/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path index d4feffc3..9d1c07a7 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path +++ b/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 8.593466476462194, - "y": 7.690171184022825 + "x": 3.838144577913229, + "y": 7.324341986522995 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.212753209700427, - "y": 5.231825962910129 + "x": 5.669329529243937, + "y": 7.441 }, "prevControl": { - "x": 5.552881597717547, - "y": 5.930513552068473 + "x": 5.26188446583575, + "y": 7.48179080723414 }, "nextControl": { - "x": 7.277238840530277, - "y": 4.1047235302667575 + "x": 6.767235397729793, + "y": 7.331084650264197 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 4.00265335235378 }, "prevControl": { - "x": 8.254, - "y": 3.7526533523537795 + "x": 7.4625239361875195, + "y": 5.141632372754151 }, "nextControl": null, "isLocked": false, @@ -46,33 +46,11 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.3837953091684457, - "rotationDegrees": -35.0 - }, - { - "waypointRelativePos": 0.6652452025586366, - "rotationDegrees": -33.0 - }, - { - "waypointRelativePos": 1.35, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.5, - "maxWaypointRelativePos": 2.0, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.0, - "unlimited": false - } + "waypointRelativePos": 0.72, + "rotationDegrees": 0.0 } ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -88,7 +66,7 @@ "rotation": -90.0 }, "reversed": false, - "folder": "BC Dot", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path new file mode 100644 index 00000000..f531a994 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.308, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 4.508899325763389, + "y": 0.4843175015516353 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.358801711840228, + "y": 0.559 + }, + "prevControl": { + "x": 4.815378031383736, + "y": 0.5480313837375183 + }, + "nextControl": { + "x": 6.222474731027868, + "y": 0.5764326189020147 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.347631954350925, + "y": 4.080285306704707 + }, + "prevControl": { + "x": 7.748480008528932, + "y": 3.4192923215240993 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 72ead732..be63fdcd 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -9,7 +9,6 @@ import java.lang.management.ManagementFactory; import java.lang.reflect.Field; import java.util.List; -import java.util.function.BiConsumer; import com.pathplanner.lib.commands.FollowPathCommand; import com.pathplanner.lib.commands.PathfindingCommand; diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 3ae927f5..13845684 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -12,6 +12,7 @@ import com.stuypulse.robot.commands.auton.regular.LeftFollow; import com.stuypulse.robot.commands.auton.regular.LeftTwoCorner; import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerBC; +import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerBCCenter; import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerShallow; import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; @@ -19,6 +20,7 @@ import com.stuypulse.robot.commands.auton.regular.RightFollow; import com.stuypulse.robot.commands.auton.regular.RightTwoCorner; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBC; +import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCCenter; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerShallow; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; @@ -429,18 +431,26 @@ public void configureAutons() { "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); LEFT_TWO_CORNER.register(autonChooser); - AutonConfig LEFT_TWO_CORNER_BC = new AutonConfig("BC Left Two Corner", LeftTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig LEFT_TWO_CORNER_BC = new AutonConfig("BC Left Two Corner BC", LeftTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)", "Left Corner To Dot", "Short Left Score to Corner"); LEFT_TWO_CORNER_BC.register(autonChooser); + AutonConfig LEFT_TWO_CORNER_BC_CENTER = new AutonConfig("BC Left Two Corner BC Center", LeftTwoCornerBCCenter::new, prevWaitTimeOne, prevWaitTimeTwo, + "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)", "Left Corner to Center Dot", "Short Left Score to Corner"); + LEFT_TWO_CORNER_BC_CENTER.register(autonChooser); + AutonConfig RIGHT_TWO_CORNER = new AutonConfig("Right Two Corner", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CORNER.register(autonChooser); - AutonConfig RIGHT_TWO_CORNER_BC = new AutonConfig("BC Right Two Corner", RightTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig RIGHT_TWO_CORNER_BC = new AutonConfig("BC Right Two Corner BC", RightTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)", "Right Corner To Dot", "Short Right Score To Corner"); RIGHT_TWO_CORNER_BC.register(autonChooser); + AutonConfig RIGHT_TWO_CORNER_BC_CENTER = new AutonConfig("BC Right Two Corner BC Center", RightTwoCornerBCCenter::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)", "Right Corner To Center Dot", "Short Right Score To Corner"); + RIGHT_TWO_CORNER_BC_CENTER.register(autonChooser); + AutonConfig LEFT_TWO_CORNER_SHALLOW = new AutonConfig("Left Two Corner Shallow", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, "Left To Shallow", "Left Shallow To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); LEFT_TWO_CORNER_SHALLOW.register(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java new file mode 100644 index 00000000..25b7e0a1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java @@ -0,0 +1,86 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class LeftTwoCornerBCCenter extends SequentialCommandGroup { + + public LeftTwoCornerBCCenter(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), //changed to 4 bcs of delay (from 5) + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().withTimeout(1.5)), //cut this down + new WaitCommand(1.0) //cut this down + ), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java new file mode 100644 index 00000000..c2b95d87 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java @@ -0,0 +1,86 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightTwoCornerBCCenter extends SequentialCommandGroup { + + public RightTwoCornerBCCenter(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), //changed to 4 bcs of delay (from 5) + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().withTimeout(4.0)), //cut this down + new WaitCommand(1.0) //cut this down + ), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]) + + ); + + } + +} From 191c28cba66a25f1bedfa019b62078da97d757dc Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 26 May 2026 18:10:16 -0400 Subject: [PATCH 48/97] feat: testing changes --- .../pathplanner/autos/BC Left One Cycle.auto | 2 +- .../pathplanner/autos/BC Left Two Cycle.auto | 4 +-- .../autos/BC Right Corner Bite.auto | 2 +- .../paths/Right Corner To Dot.path | 34 ++++++------------- .../paths/Right Corner to Dot v2.path | 4 +-- .../com/stuypulse/robot/RobotContainer.java | 12 +++---- .../auton/regular/LeftTwoCornerBC.java | 7 ++-- .../auton/regular/RightTwoCornerBC.java | 6 ++-- 8 files changed, 30 insertions(+), 41 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto index 8029eb98..19f3aa30 100644 --- a/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto +++ b/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": "Short Left Score To Corner" + "pathName": "Short Left Score to Corner" } }, { diff --git a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto index 35dae601..c4569351 100644 --- a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": "Short Left Score To Corner" + "pathName": "Short Left Score to Corner" } }, { @@ -31,7 +31,7 @@ { "type": "path", "data": { - "pathName": "Short Left Score To Corner" + "pathName": "Short Left Score to Corner" } }, { diff --git a/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto index ae7fee0b..45ab87a1 100644 --- a/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto @@ -37,7 +37,7 @@ { "type": "path", "data": { - "pathName": "Right Corner To Dot" + "pathName": "Right Corner to Dot" } } ] diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path index db6dc40d..c03ab667 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path @@ -4,12 +4,12 @@ { "anchor": { "x": 3.308, - "y": 0.559 + "y": 0.65 }, "prevControl": null, "nextControl": { - "x": 6.109243937232525, - "y": 0.6774179743223976 + "x": 6.09644007818155, + "y": 0.65 }, "isLocked": false, "linkedName": null @@ -17,11 +17,11 @@ { "anchor": { "x": 8.254, - "y": 0.559 + "y": 0.65 }, "prevControl": { - "x": 8.500236542443773, - "y": 0.602215334840027 + "x": 8.504, + "y": 0.65 }, "nextControl": null, "isLocked": false, @@ -35,24 +35,10 @@ }, { "waypointRelativePos": 0.75, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.75, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } + "rotationDegrees": 90.0 } ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -65,10 +51,10 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 90.0 }, "reversed": false, - "folder": null, + "folder": "BC Dot", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path b/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path index dbc288ce..1ade5c2e 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path +++ b/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path @@ -55,7 +55,7 @@ }, { "waypointRelativePos": 1.35, - "rotationDegrees": -90.0 + "rotationDegrees": 90.0 } ], "constraintZones": [ @@ -85,7 +85,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -90.0 + "rotation": 90.0 }, "reversed": false, "folder": "BC Dot", diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 13845684..c11b6b66 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -113,7 +113,7 @@ public interface EnabledSubsystems { SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", true); SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", true); SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); - SmartBoolean LEDS = new SmartBoolean("Enabled Subsystems/LEDs Is Enabled", false); + SmartBoolean LEDS = new SmartBoolean("Enabled Subsystems/LEDs Is Enabled", true); SmartBoolean BACK_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Back Limelight Is Enabled", true); SmartBoolean LEFT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Left Limelight Is Enabled", true); @@ -423,7 +423,7 @@ public void configureAutons() { "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CYCLE.register(autonChooser); - AutonConfig BC_TEST = new AutonConfig("BC Test", TestBC::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner to Dot v2"); + AutonConfig BC_TEST = new AutonConfig("BC Test", TestBC::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner to Dot"); BC_TEST.register(autonChooser); // TWO CYCLES (CORNER) @@ -431,8 +431,8 @@ public void configureAutons() { "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); LEFT_TWO_CORNER.register(autonChooser); - AutonConfig LEFT_TWO_CORNER_BC = new AutonConfig("BC Left Two Corner BC", LeftTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)", "Left Corner To Dot", "Short Left Score to Corner"); + AutonConfig LEFT_TWO_CORNER_BC = new AutonConfig("BC Left Two Corner", LeftTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)", "Left Corner to Dot", "Short Left Score to Corner"); LEFT_TWO_CORNER_BC.register(autonChooser); AutonConfig LEFT_TWO_CORNER_BC_CENTER = new AutonConfig("BC Left Two Corner BC Center", LeftTwoCornerBCCenter::new, prevWaitTimeOne, prevWaitTimeTwo, @@ -443,8 +443,8 @@ public void configureAutons() { "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CORNER.register(autonChooser); - AutonConfig RIGHT_TWO_CORNER_BC = new AutonConfig("BC Right Two Corner BC", RightTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)", "Right Corner To Dot", "Short Right Score To Corner"); + AutonConfig RIGHT_TWO_CORNER_BC = new AutonConfig("BC Right Two Corner", RightTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)", "Right Corner to Dot"); RIGHT_TWO_CORNER_BC.register(autonChooser); AutonConfig RIGHT_TWO_CORNER_BC_CENTER = new AutonConfig("BC Right Two Corner BC Center", RightTwoCornerBCCenter::new, prevWaitTimeOne, prevWaitTimeTwo, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java index 83d071e8..32f9bda8 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java @@ -18,6 +18,7 @@ import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -73,12 +74,12 @@ public LeftTwoCornerBC(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().withTimeout(1.5)), //cut this down + .andThen(new IntakeAutoDigest().withTimeout(2.5)), //cut this down new WaitCommand(1.0) //cut this down ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]) - + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), + new SwerveXMode() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java index 0630ddf6..abbe8e9f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java @@ -18,6 +18,7 @@ import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -73,11 +74,12 @@ public RightTwoCornerBC(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().withTimeout(4.0)), //cut this down + .andThen(new IntakeAutoDigest().withTimeout(4.0)), //cut this down CHANGE HERE new WaitCommand(1.0) //cut this down ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), + new SwerveXMode() ); From 65097cc5a9b81a6b546b76943631dd9532fc5c6c Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 26 May 2026 18:38:47 -0400 Subject: [PATCH 49/97] feat: pathfinding implemented + testing changes. Pathfinding bugged needs fixing --- .../autos/BC Right Corner Bite.auto | 4 +- .../paths/BC Right Corner Bite.path | 81 +++++++++++++++++++ .../paths/BC Right NZ To Score.path | 79 ++++++++++++++++++ .../paths/Right Corner To Dot.path | 2 +- .../com/stuypulse/robot/RobotContainer.java | 2 +- .../auton/regular/RightTwoCornerBC.java | 16 ++-- .../swerve/CommandSwerveDrivetrain.java | 13 +++ 7 files changed, 186 insertions(+), 11 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/BC Right Corner Bite.path create mode 100644 src/main/deploy/pathplanner/paths/BC Right NZ To Score.path diff --git a/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto index 45ab87a1..b9b32959 100644 --- a/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto @@ -7,13 +7,13 @@ { "type": "path", "data": { - "pathName": "Right Corner Bite" + "pathName": "BC Right Corner Bite" } }, { "type": "path", "data": { - "pathName": "Right NZ To Score" + "pathName": "BC Right NZ To Score" } }, { diff --git a/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path b/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path new file mode 100644 index 00000000..9ada29ad --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 7.584251069900143, + "y": 0.48333808844507875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.909455555555555, + "y": 3.0 + }, + "prevControl": { + "x": 7.948433333333332, + "y": 1.6942444444444456 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17621776504297812, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.41791044776119385, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.7782515991471214, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5077650236326793, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "BC Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path b/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path new file mode 100644 index 00000000..31da71d6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.909455555555555, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 6.204177777777778, + "y": 2.902555555555556 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.419771754636234, + "y": 1.4925534950071324 + }, + "prevControl": { + "x": 6.44766178400938, + "y": 2.924425883014991 + }, + "nextControl": { + "x": 6.399066666666666, + "y": 0.42955555555555525 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.559 + }, + "prevControl": { + "x": 6.311366666666668, + "y": 0.5283859724203506 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.23445825932504563, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.488272921108742, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC Dot", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path index c03ab667..0cf06fa8 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path @@ -59,5 +59,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c11b6b66..bc6272c1 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -444,7 +444,7 @@ public void configureAutons() { RIGHT_TWO_CORNER.register(autonChooser); AutonConfig RIGHT_TWO_CORNER_BC = new AutonConfig("BC Right Two Corner", RightTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)", "Right Corner to Dot"); + "BC Right Corner Bite", "BC Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)", "Right Corner to Dot"); RIGHT_TWO_CORNER_BC.register(autonChooser); AutonConfig RIGHT_TWO_CORNER_BC_CENTER = new AutonConfig("BC Right Two Corner BC Center", RightTwoCornerBCCenter::new, prevWaitTimeOne, prevWaitTimeTwo, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java index abbe8e9f..25dc4a87 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java @@ -7,6 +7,7 @@ import java.util.Set; +import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.commands.handoff.HandoffRun; @@ -54,15 +55,16 @@ public RightTwoCornerBC(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), //changed to 4 bcs of delay (from 5) - new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.0)) //changed to 4 bcs of delay (from 5) + // new WaitCommand(1.0).andThen( + // new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[2], paths[0].getGlobalConstraints()), new HandoffStop(), new SpindexerStop() ), @@ -74,11 +76,11 @@ public RightTwoCornerBC(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().withTimeout(4.0)), //cut this down CHANGE HERE - new WaitCommand(1.0) //cut this down + .andThen(new IntakeAutoDigest().withTimeout(1.0)) //cut this down CHANGE HERE + // new WaitCommand(1.0) //cut this down ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[5], paths[0].getGlobalConstraints()), new SwerveXMode() ); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index b3abc799..95bd9041 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -25,6 +25,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.PathPlannerLogging; import com.stuypulse.robot.Robot; @@ -477,10 +478,22 @@ public Command followPathCommand(String pathName) { } } + public Command pathfindThenFollowPath(String pathName, PathConstraints pathFindingConstraints) { + try { + return pathfindThenFollowPath(PathPlannerPath.fromPathFile(pathName), pathFindingConstraints); + } catch (Exception e) { + throw new IllegalArgumentException(pathName + " does not exist"); + } + } + public Command followPathCommand(PathPlannerPath path) { return AutoBuilder.followPath(path); } + public Command pathfindThenFollowPath(PathPlannerPath path, PathConstraints pathFindingConstraints) { + return AutoBuilder.pathfindThenFollowPath(path, pathFindingConstraints); + } + public SwerveModuleState[] getModuleStates() { return getState().ModuleStates; } From 4a0f49da9f60250b7dfecf4625140be593d6b823 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 26 May 2026 18:47:49 -0400 Subject: [PATCH 50/97] feat: fix for patfinding --- .../robot/commands/auton/regular/RightTwoCornerBC.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java index 25dc4a87..f31f2644 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java @@ -51,7 +51,7 @@ public RightTwoCornerBC(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).withTimeout(1.0), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) @@ -64,7 +64,7 @@ public RightTwoCornerBC(PathPlannerPath... paths) { // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[2], paths[0].getGlobalConstraints()), + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[2], PathConstraints.unlimitedConstraints(12)), new HandoffStop(), new SpindexerStop() ), @@ -72,7 +72,7 @@ public RightTwoCornerBC(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).withTimeout(1.0), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) @@ -80,7 +80,7 @@ public RightTwoCornerBC(PathPlannerPath... paths) { // new WaitCommand(1.0) //cut this down ), - CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[5], paths[0].getGlobalConstraints()), + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[5], PathConstraints.unlimitedConstraints(12)), new SwerveXMode() ); From ac46397d58229eb092695414e6984ad6d0b29ed8 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Wed, 27 May 2026 18:39:26 -0400 Subject: [PATCH 51/97] feat: center autos, other paths, updated all BC path logics. Needs testing. --- .../autos/BC Left Corner Bite CD.auto | 49 +++++++++++ .../autos/BC Left Corner Bite.auto | 8 +- .../pathplanner/autos/BC Left One Cycle.auto | 4 +- .../pathplanner/autos/BC Left Two Cycle.auto | 14 +++- .../autos/BC Right Corner Bite CD.auto | 49 +++++++++++ .../autos/BC Right Corner Bite.auto | 6 +- .../pathplanner/autos/BC to Dot test.auto | 4 +- .../paths/BC Left Corner Bite.path | 81 +++++++++++++++++++ .../paths/BC Left NZ To Score.path | 79 ++++++++++++++++++ .../paths/BC Right Corner Bite.path | 4 +- .../paths/BC Right NZ To Score.path | 4 +- .../pathplanner/paths/Left Corner Bite.path | 2 +- .../paths/Left Corner To Dot v3.path | 67 +++++++++++++++ .../paths/Left Corner to Center Dot.path | 46 ++++++----- .../paths/Left Corner to Dot v2.path | 2 +- .../pathplanner/paths/Left Corner to Dot.path | 30 +++---- .../paths/Right Corner To Center Dot.path | 51 +++++++----- .../paths/Right Corner To Dot v3.path | 77 ++++++++++++++++++ .../paths/Right Corner To Dot.path | 36 ++++++--- .../paths/Right Corner to Dot v2.path | 2 +- .../paths/Right Score To Corner.path | 8 +- ...Score To Corner.path => Straight One.path} | 28 +++---- ...Score To Corner.path => Straight Two.path} | 26 +++--- src/main/deploy/pathplanner/settings.json | 8 +- .../com/stuypulse/robot/RobotContainer.java | 25 +++++- .../auton/regular/LeftTwoCornerBC.java | 27 ++++--- .../auton/regular/LeftTwoCornerBCCenter.java | 29 ++++--- .../auton/regular/RightTwoCornerBC.java | 24 +++--- .../auton/regular/RightTwoCornerBCCenter.java | 31 +++---- .../commands/auton/test/PathfindTest.java | 19 +++++ 30 files changed, 659 insertions(+), 181 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto create mode 100644 src/main/deploy/pathplanner/autos/BC Right Corner Bite CD.auto create mode 100644 src/main/deploy/pathplanner/paths/BC Left Corner Bite.path create mode 100644 src/main/deploy/pathplanner/paths/BC Left NZ To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Left Corner To Dot v3.path create mode 100644 src/main/deploy/pathplanner/paths/Right Corner To Dot v3.path rename src/main/deploy/pathplanner/paths/{Short Right Score To Corner.path => Straight One.path} (63%) rename src/main/deploy/pathplanner/paths/{Short Left Score To Corner.path => Straight Two.path} (67%) create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/test/PathfindTest.java diff --git a/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto b/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto new file mode 100644 index 00000000..beddcdaa --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BC Left Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Corner To Center Dot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Main", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto index cf53fc5d..1ea9a019 100644 --- a/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto @@ -7,13 +7,13 @@ { "type": "path", "data": { - "pathName": "Left Corner Bite" + "pathName": "BC Left Corner Bite" } }, { "type": "path", "data": { - "pathName": "Left NZ To Score" + "pathName": "BC Left NZ To Score" } }, { @@ -37,13 +37,13 @@ { "type": "path", "data": { - "pathName": "Left Corner to Dot" + "pathName": "Left Corner To Dot" } } ] } }, "resetOdom": true, - "folder": null, + "folder": "BC Main", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto index 19f3aa30..7b676264 100644 --- a/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto +++ b/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": "Short Left Score to Corner" + "pathName": "Left Score To Corner" } }, { @@ -38,6 +38,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "BC Extras", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto index c4569351..552723bd 100644 --- a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": "Short Left Score to Corner" + "pathName": "Left Score To Corner" } }, { @@ -31,19 +31,25 @@ { "type": "path", "data": { - "pathName": "Short Left Score to Corner" + "pathName": "Left Score To Corner" } }, { "type": "path", "data": { - "pathName": "Left Corner to Dot" + "pathName": "Left Corner To Dot" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Dot to Middle" } } ] } }, "resetOdom": true, - "folder": null, + "folder": "BC Extras", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Right Corner Bite CD.auto b/src/main/deploy/pathplanner/autos/BC Right Corner Bite CD.auto new file mode 100644 index 00000000..af4828c4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Right Corner Bite CD.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BC Right Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Corner To Center Dot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Main", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto index b9b32959..8bcb3d18 100644 --- a/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Right Bite Score To Score" + "pathName": "Right Score To Score" } }, { @@ -37,13 +37,13 @@ { "type": "path", "data": { - "pathName": "Right Corner to Dot" + "pathName": "Right Corner To Dot" } } ] } }, "resetOdom": true, - "folder": null, + "folder": "BC Main", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC to Dot test.auto b/src/main/deploy/pathplanner/autos/BC to Dot test.auto index f351c702..74dda1e2 100644 --- a/src/main/deploy/pathplanner/autos/BC to Dot test.auto +++ b/src/main/deploy/pathplanner/autos/BC to Dot test.auto @@ -7,13 +7,13 @@ { "type": "path", "data": { - "pathName": "Right Corner to Dot v2" + "pathName": "Right Corner To Dot v2" } } ] } }, "resetOdom": true, - "folder": null, + "folder": "BC Extras", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left Corner Bite.path b/src/main/deploy/pathplanner/paths/BC Left Corner Bite.path new file mode 100644 index 00000000..f79f7401 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left Corner Bite.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 7.605 + }, + "prevControl": null, + "nextControl": { + "x": 7.584251069900143, + "y": 7.6935575540939345 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.909455555555555, + "y": 5.0 + }, + "prevControl": { + "x": 7.9484375712176165, + "y": 6.305755429044663 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17621776504297812, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.41791044776119385, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.7782515991471214, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5077650236326793, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.2, + "rotation": -90.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path b/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path new file mode 100644 index 00000000..ddcae2ff --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.909455555555555, + "y": 5.0 + }, + "prevControl": null, + "nextControl": { + "x": 6.204177777777778, + "y": 4.902555555555557 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.419771754636234, + "y": 6.507 + }, + "prevControl": { + "x": 6.447665111532734, + "y": 5.075127676809553 + }, + "nextControl": { + "x": 6.399064196369385, + "y": 7.569997891332222 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 7.441 + }, + "prevControl": { + "x": 6.311366666666668, + "y": 7.41038597242035 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.23445825932504563, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.488272921108742, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 1.2, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path b/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path index 9ada29ad..01d71b55 100644 --- a/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path @@ -68,11 +68,11 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 1.2, "rotation": 90.0 }, "reversed": false, - "folder": "BC Dot", + "folder": "BC modified", "idealStartingState": { "velocity": 0, "rotation": 90.0 diff --git a/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path b/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path index 31da71d6..a542a80b 100644 --- a/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path @@ -70,9 +70,9 @@ "rotation": 0.0 }, "reversed": false, - "folder": "BC Dot", + "folder": "BC modified", "idealStartingState": { - "velocity": 0.0, + "velocity": 1.2, "rotation": 90.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite.path b/src/main/deploy/pathplanner/paths/Left Corner Bite.path index c33f487b..e29ed1fc 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite.path @@ -68,7 +68,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 0.0, "rotation": -90.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Dot v3.path b/src/main/deploy/pathplanner/paths/Left Corner To Dot v3.path new file mode 100644 index 00000000..0e69c7db --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner To Dot v3.path @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.612, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 5.137616333629864, + "y": 7.448897901816431 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 7.441 + }, + "prevControl": { + "x": 7.931262293036603, + "y": 7.447711041684223 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.36281588447653357, + "rotationDegrees": -2.0 + }, + { + "waypointRelativePos": 0.7914438502673795, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path b/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path index 9d1c07a7..194ab52d 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path +++ b/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 3.308, + "x": 3.282, "y": 7.441 }, "prevControl": null, "nextControl": { - "x": 3.838144577913229, - "y": 7.324341986522995 + "x": 5.209691738594327, + "y": 7.581929716399507 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.669329529243937, - "y": 7.441 + "x": 6.20456226879871, + "y": 6.203 }, "prevControl": { - "x": 5.26188446583575, - "y": 7.48179080723414 + "x": 6.237003699136868, + "y": 7.419722564734895 }, "nextControl": { - "x": 6.767235397729793, - "y": 7.331084650264197 + "x": 6.174004896303544, + "y": 5.056939414929412 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.254, - "y": 4.00265335235378 + "x": 8.27, + "y": 4.067 }, "prevControl": { - "x": 7.4625239361875195, - "y": 5.141632372754151 + "x": 5.685499383471953, + "y": 4.089069050550844 }, "nextControl": null, "isLocked": false, @@ -46,8 +46,16 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.72, + "waypointRelativePos": 0.23, "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.109909909909912, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.754954954954955, + "rotationDegrees": 0.4456470247826539 } ], "constraintZones": [], @@ -58,17 +66,17 @@ "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": -90.0 + "velocity": 0.0, + "rotation": 90.0 }, "reversed": false, - "folder": null, + "folder": "BC To Dot", "idealStartingState": { - "velocity": 0, + "velocity": 0.0, "rotation": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left Corner to Dot v2.path b/src/main/deploy/pathplanner/paths/Left Corner to Dot v2.path index 3711b812..9b02e7c4 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner to Dot v2.path +++ b/src/main/deploy/pathplanner/paths/Left Corner to Dot v2.path @@ -88,7 +88,7 @@ "rotation": -90.0 }, "reversed": false, - "folder": "BC Dot", + "folder": "BC To Dot", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Left Corner to Dot.path b/src/main/deploy/pathplanner/paths/Left Corner to Dot.path index 0e9f8c29..37ebd0ed 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner to Dot.path +++ b/src/main/deploy/pathplanner/paths/Left Corner to Dot.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.128246869413065, - "y": 7.53157423971269 + "x": 4.833616333629863, + "y": 7.448897901816431 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 7.441 }, "prevControl": { - "x": 7.053320214672454, - "y": 6.979910554560632 + "x": 7.931262293036603, + "y": 7.447711041684223 }, "nextControl": null, "isLocked": false, @@ -33,26 +33,16 @@ "waypointRelativePos": 0.35, "rotationDegrees": 0.0 }, + { + "waypointRelativePos": 0.36281588447653357, + "rotationDegrees": -2.0 + }, { "waypointRelativePos": 0.7914438502673795, "rotationDegrees": 180.0 } ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7525423728813557, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -68,7 +58,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": "BC paths", + "folder": "BC To Dot", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path index f531a994..bbea90e7 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path @@ -3,48 +3,61 @@ "waypoints": [ { "anchor": { - "x": 3.308, + "x": 3.282, "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 4.508899325763389, - "y": 0.4843175015516353 + "x": 5.6206165228113445, + "y": 0.531325524044389 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.358801711840228, - "y": 0.559 + "x": 6.20456226879871, + "y": 1.7965413070243335 }, "prevControl": { - "x": 4.815378031383736, - "y": 0.5480313837375183 + "x": 6.193748458692971, + "y": 0.6502774352651044 }, "nextControl": { - "x": 6.222474731027868, - "y": 0.5764326189020147 + "x": 6.2118058772904625, + "y": 2.564363807519222 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.347631954350925, - "y": 4.080285306704707 + "x": 8.27, + "y": 4.067 }, "prevControl": { - "x": 7.748480008528932, - "y": 3.4192923215240993 + "x": 5.685499383471953, + "y": 4.089069050550844 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.23, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.109909909909912, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.6432432432432433, + "rotationDegrees": 0.4456470247826539 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -53,17 +66,17 @@ "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": 0.0 + "velocity": 0.0, + "rotation": -90.0 }, "reversed": false, - "folder": null, + "folder": "BC To Dot", "idealStartingState": { - "velocity": 0, + "velocity": 0.0, "rotation": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot v3.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot v3.path new file mode 100644 index 00000000..578d5ceb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot v3.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.413243937232525, + "y": 0.6774179743223976 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 0.559 + }, + "prevControl": { + "x": 8.500236542443773, + "y": 0.6022153348400272 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3795309168443479, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.75, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.75, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path index 0cf06fa8..560a1799 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path @@ -4,12 +4,12 @@ { "anchor": { "x": 3.308, - "y": 0.65 + "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 6.09644007818155, - "y": 0.65 + "x": 6.109243937232525, + "y": 0.6774179743223976 }, "isLocked": false, "linkedName": null @@ -17,11 +17,11 @@ { "anchor": { "x": 8.254, - "y": 0.65 + "y": 0.559 }, "prevControl": { - "x": 8.504, - "y": 0.65 + "x": 8.500236542443773, + "y": 0.6022153348400271 }, "nextControl": null, "isLocked": false, @@ -35,10 +35,24 @@ }, { "waypointRelativePos": 0.75, - "rotationDegrees": 90.0 + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.75, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } } ], - "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -51,13 +65,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 90.0 + "rotation": 180.0 }, "reversed": false, - "folder": "BC Dot", + "folder": "BC To Dot", "idealStartingState": { "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path b/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path index 1ade5c2e..723eaf99 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path +++ b/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path @@ -88,7 +88,7 @@ "rotation": 90.0 }, "reversed": false, - "folder": "BC Dot", + "folder": "BC To Dot", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Right Score To Corner.path b/src/main/deploy/pathplanner/paths/Right Score To Corner.path index f5ea9d15..46dd4a62 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Corner.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, + "x": 3.612, "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 3.2000538693416494, + "x": 3.199971130397284, "y": 0.6068359978960558 }, "isLocked": false, @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.2824809160305346, + "x": 3.282, "y": 0.559 }, "prevControl": { - "x": 3.516912705949025, + "x": 3.5164317899184905, "y": 0.47215683172745937 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Short Right Score To Corner.path b/src/main/deploy/pathplanner/paths/Straight One.path similarity index 63% rename from src/main/deploy/pathplanner/paths/Short Right Score To Corner.path rename to src/main/deploy/pathplanner/paths/Straight One.path index 48298e16..2220a3ec 100644 --- a/src/main/deploy/pathplanner/paths/Short Right Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Straight One.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, - "y": 0.559 + "x": 4.440805008944544, + "y": 7.547799642218246 }, "prevControl": null, "nextControl": { - "x": 3.2000538693416494, - "y": 0.6068359978960558 + "x": 5.440805008944544, + "y": 7.547799642218246 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.2824809160305346, - "y": 0.559 + "x": 5.441, + "y": 7.547799642218246 }, "prevControl": { - "x": 3.516912705949025, - "y": 0.47215683172745937 + "x": 4.441, + "y": 7.547799642218246 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Pathfinder" } ], "rotationTargets": [], @@ -33,11 +33,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.15, - "maxAcceleration": 4.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { @@ -45,10 +45,10 @@ "rotation": 0.0 }, "reversed": false, - "folder": null, + "folder": "PathFinder Test", "idealStartingState": { "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path b/src/main/deploy/pathplanner/paths/Straight Two.path similarity index 67% rename from src/main/deploy/pathplanner/paths/Short Left Score To Corner.path rename to src/main/deploy/pathplanner/paths/Straight Two.path index 4e494dff..d9e57f30 100644 --- a/src/main/deploy/pathplanner/paths/Short Left Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Straight Two.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, - "y": 7.441 + "x": 6.0, + "y": 7.547799642218246 }, "prevControl": null, "nextControl": { - "x": 3.1972863164901653, - "y": 7.441 + "x": 7.0, + "y": 7.547799642218246 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.482, - "y": 7.441 + "x": 7.0, + "y": 7.547799642218246 }, "prevControl": { - "x": 3.732, - "y": 7.441 + "x": 6.0, + "y": 7.547799642218246 }, "nextControl": null, "isLocked": false, @@ -33,11 +33,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.1, - "maxAcceleration": 4.0, + "maxVelocity": 4.19, + "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { @@ -45,10 +45,10 @@ "rotation": 0.0 }, "reversed": false, - "folder": "BC Dot", + "folder": "PathFinder Test", "idealStartingState": { "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 5871e05a..48443623 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,14 +3,20 @@ "robotLength": 0.762, "holonomicMode": true, "pathFolders": [ + "BC modified", "Bump Stuff", "Follow", "Non-Collision", + "PathFinder Test", "To Depot", + "BC To Dot", "To NZ", "To Score" ], - "autoFolders": [], + "autoFolders": [ + "BC Main", + "BC Extras" + ], "defaultMaxVel": 4.19, "defaultMaxAccel": 10.0, "defaultMaxAngVel": 300.0, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index bc6272c1..bb44be70 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -24,6 +24,7 @@ import com.stuypulse.robot.commands.auton.regular.RightTwoCornerShallow; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; +import com.stuypulse.robot.commands.auton.test.PathfindTest; import com.stuypulse.robot.commands.auton.test.TestBC; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; @@ -432,11 +433,17 @@ public void configureAutons() { LEFT_TWO_CORNER.register(autonChooser); AutonConfig LEFT_TWO_CORNER_BC = new AutonConfig("BC Left Two Corner", LeftTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)", "Left Corner to Dot", "Short Left Score to Corner"); + "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "Left Score To Score", "Left Corner To Dot"); LEFT_TWO_CORNER_BC.register(autonChooser); + //before i get flamed ITS NOT EXPERIMENTAL! + //i just needed a distinct name. All it does is instead of pathfinding to the dot path at the end of shooting, it path finds forward to save time + AutonConfig LEFT_TWO_CORNER_V3 = new AutonConfig("V3 Left BC Two Corner", LeftTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "Left Score To Score", "Left Corner To Dot v3"); + LEFT_TWO_CORNER_V3.register(autonChooser); + AutonConfig LEFT_TWO_CORNER_BC_CENTER = new AutonConfig("BC Left Two Corner BC Center", LeftTwoCornerBCCenter::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)", "Left Corner to Center Dot", "Short Left Score to Corner"); + "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "Left Score To Score", "Left Corner To Center Dot"); LEFT_TWO_CORNER_BC_CENTER.register(autonChooser); AutonConfig RIGHT_TWO_CORNER = new AutonConfig("Right Two Corner", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, @@ -444,11 +451,17 @@ public void configureAutons() { RIGHT_TWO_CORNER.register(autonChooser); AutonConfig RIGHT_TWO_CORNER_BC = new AutonConfig("BC Right Two Corner", RightTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)", "Right Corner to Dot"); + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "Right Score To Score", "Right Corner To Dot"); RIGHT_TWO_CORNER_BC.register(autonChooser); + //before i get flamed ITS NOT EXPERIMENTAL! + //i just needed a distinct name. All it does is instead of pathfinding to the dot path at the end of shooting, it path finds forward to save time + AutonConfig RIGHT_TWO_CORNER_V3 = new AutonConfig("V3 Right BC Two Corner", RightTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "Right Score To Score", "Right Corner To Dot v3"); + RIGHT_TWO_CORNER_V3.register(autonChooser); + AutonConfig RIGHT_TWO_CORNER_BC_CENTER = new AutonConfig("BC Right Two Corner BC Center", RightTwoCornerBCCenter::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)", "Right Corner To Center Dot", "Short Right Score To Corner"); + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "Right Score To Score", "Right Corner To Center Dot"); RIGHT_TWO_CORNER_BC_CENTER.register(autonChooser); AutonConfig LEFT_TWO_CORNER_SHALLOW = new AutonConfig("Left Two Corner Shallow", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, @@ -480,6 +493,10 @@ public void configureAutons() { // "Right Trench Score To Corner"); // EMPTY_TEST.register(autonChooser); + AutonConfig PATH_FIND_TEST = new AutonConfig("Path Find Test", PathfindTest::new, prevWaitTimeOne, prevWaitTimeTwo, + "Straight One", "Straight Two"); + PATH_FIND_TEST.register(autonChooser); + SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java index 32f9bda8..698365fe 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java @@ -7,6 +7,7 @@ import java.util.Set; +import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.commands.handoff.HandoffRun; @@ -38,7 +39,6 @@ public LeftTwoCornerBC(PathPlannerPath... paths) { Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( new WaitCommand(0.2).andThen(new IntakeDeploy()) ), @@ -50,19 +50,19 @@ public LeftTwoCornerBC(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), //changed to 4 bcs of delay (from 5) - new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) - ), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //changed to 4 bcs of delay (from 5) + // new WaitCommand(1.0).andThen( + // new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) + ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[3], PathConstraints.unlimitedConstraints(12)), new HandoffStop(), new SpindexerStop() ), @@ -70,15 +70,16 @@ public LeftTwoCornerBC(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().withTimeout(2.5)), //cut this down - new WaitCommand(1.0) //cut this down - ), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down + // new WaitCommand(1.0) + ).withTimeout(1.0), + new IntakeDeploy(), //in case digestion doesn't finish neatly - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), new SwerveXMode() ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java index 25b7e0a1..0fc8c006 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java @@ -7,6 +7,7 @@ import java.util.Set; +import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.commands.handoff.HandoffRun; @@ -18,6 +19,7 @@ import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -37,7 +39,6 @@ public LeftTwoCornerBCCenter(PathPlannerPath... paths) { Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( new WaitCommand(0.2).andThen(new IntakeDeploy()) ), @@ -49,19 +50,19 @@ public LeftTwoCornerBCCenter(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), //changed to 4 bcs of delay (from 5) - new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) - ), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //changed to 4 bcs of delay (from 5) + // new WaitCommand(1.0).andThen( + // new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) + ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[3], PathConstraints.unlimitedConstraints(12)), new HandoffStop(), new SpindexerStop() ), @@ -69,15 +70,17 @@ public LeftTwoCornerBCCenter(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().withTimeout(1.5)), //cut this down - new WaitCommand(1.0) //cut this down - ), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down + // new WaitCommand(1.0) + ).withTimeout(1.0), + new IntakeDeploy(), //in case digestion doesn't finish neatly - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]) + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), + new SwerveXMode() ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java index f31f2644..862efcc1 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java @@ -39,7 +39,6 @@ public RightTwoCornerBC(PathPlannerPath... paths) { Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( new WaitCommand(0.2).andThen(new IntakeDeploy()) ), @@ -51,20 +50,19 @@ public RightTwoCornerBC(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).withTimeout(1.0), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.0)) //changed to 4 bcs of delay (from 5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //changed to 4 bcs of delay (from 5) // new WaitCommand(1.0).andThen( // new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) - ), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), - + ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[2], PathConstraints.unlimitedConstraints(12)), + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[3], PathConstraints.unlimitedConstraints(12)), new HandoffStop(), new SpindexerStop() ), @@ -72,17 +70,17 @@ public RightTwoCornerBC(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).withTimeout(1.0), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().withTimeout(1.0)) //cut this down CHANGE HERE - // new WaitCommand(1.0) //cut this down - ), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down + // new WaitCommand(1.0) + ).withTimeout(1.0), + new IntakeDeploy(), //in case digestion doesn't finish neatly - CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[5], PathConstraints.unlimitedConstraints(12)), + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), new SwerveXMode() - ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java index c2b95d87..5252705c 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java @@ -7,6 +7,7 @@ import java.util.Set; +import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.commands.handoff.HandoffRun; @@ -18,6 +19,7 @@ import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -32,12 +34,10 @@ public class RightTwoCornerBCCenter extends SequentialCommandGroup { public RightTwoCornerBCCenter(PathPlannerPath... paths) { addCommands( - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( new WaitCommand(0.2).andThen(new IntakeDeploy()) ), @@ -49,19 +49,19 @@ public RightTwoCornerBCCenter(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), //changed to 4 bcs of delay (from 5) - new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) - ), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //changed to 4 bcs of delay (from 5) + // new WaitCommand(1.0).andThen( + // new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) + ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[3], PathConstraints.unlimitedConstraints(12)), new HandoffStop(), new SpindexerStop() ), @@ -69,16 +69,17 @@ public RightTwoCornerBCCenter(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().withTimeout(4.0)), //cut this down - new WaitCommand(1.0) //cut this down - ), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down + // new WaitCommand(1.0) + ).withTimeout(1.0), + new IntakeDeploy(), //in case digestion doesn't finish neatly - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]) - + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), + new SwerveXMode() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/PathfindTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/PathfindTest.java new file mode 100644 index 00000000..1653aae5 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/PathfindTest.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.auton.test; + +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class PathfindTest extends SequentialCommandGroup { + public PathfindTest(PathPlannerPath... paths) { + addCommands( + new SwerveResetPose(paths[0].getStartingDifferentialPose()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[1], PathConstraints.unlimitedConstraints(12.0)) + ); + } +} From 7e6168b06e670387b2761ad18eee17fad11b3d6a Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 28 May 2026 13:05:31 -0400 Subject: [PATCH 52/97] Testing Changes --- .../deploy/pathplanner/autos/BC Left Two Cycle.auto | 2 +- src/main/deploy/pathplanner/autos/BC to Dot test.auto | 2 +- src/main/deploy/pathplanner/paths/Straight One.path | 10 +++++----- src/main/deploy/pathplanner/paths/Straight Two.path | 8 ++++---- src/main/java/com/stuypulse/robot/RobotContainer.java | 5 +---- .../robot/commands/auton/regular/LeftTwoCornerBC.java | 2 +- .../commands/auton/regular/LeftTwoCornerBCCenter.java | 2 +- .../robot/commands/auton/regular/RightTwoCornerBC.java | 10 +++++----- .../commands/auton/regular/RightTwoCornerBCCenter.java | 4 ++-- .../robot/commands/auton/test/PathfindTest.java | 4 +++- 10 files changed, 24 insertions(+), 25 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto index 552723bd..3b482828 100644 --- a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto @@ -37,7 +37,7 @@ { "type": "path", "data": { - "pathName": "Left Corner To Dot" + "pathName": null } }, { diff --git a/src/main/deploy/pathplanner/autos/BC to Dot test.auto b/src/main/deploy/pathplanner/autos/BC to Dot test.auto index 74dda1e2..89695d9e 100644 --- a/src/main/deploy/pathplanner/autos/BC to Dot test.auto +++ b/src/main/deploy/pathplanner/autos/BC to Dot test.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "Right Corner To Dot v2" + "pathName": null } } ] diff --git a/src/main/deploy/pathplanner/paths/Straight One.path b/src/main/deploy/pathplanner/paths/Straight One.path index 2220a3ec..f3f18442 100644 --- a/src/main/deploy/pathplanner/paths/Straight One.path +++ b/src/main/deploy/pathplanner/paths/Straight One.path @@ -4,12 +4,12 @@ { "anchor": { "x": 4.440805008944544, - "y": 7.547799642218246 + "y": 0.559 }, "prevControl": null, "nextControl": { "x": 5.440805008944544, - "y": 7.547799642218246 + "y": 0.5589999999999999 }, "isLocked": false, "linkedName": null @@ -17,15 +17,15 @@ { "anchor": { "x": 5.441, - "y": 7.547799642218246 + "y": 0.559 }, "prevControl": { "x": 4.441, - "y": 7.547799642218246 + "y": 0.5589999999999999 }, "nextControl": null, "isLocked": false, - "linkedName": "Pathfinder" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Straight Two.path b/src/main/deploy/pathplanner/paths/Straight Two.path index d9e57f30..0c65a299 100644 --- a/src/main/deploy/pathplanner/paths/Straight Two.path +++ b/src/main/deploy/pathplanner/paths/Straight Two.path @@ -4,12 +4,12 @@ { "anchor": { "x": 6.0, - "y": 7.547799642218246 + "y": 0.559 }, "prevControl": null, "nextControl": { "x": 7.0, - "y": 7.547799642218246 + "y": 0.5590000000000002 }, "isLocked": false, "linkedName": null @@ -17,11 +17,11 @@ { "anchor": { "x": 7.0, - "y": 7.547799642218246 + "y": 0.559 }, "prevControl": { "x": 6.0, - "y": 7.547799642218246 + "y": 0.5590000000000002 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index bb44be70..3a2a7a0c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -436,8 +436,7 @@ public void configureAutons() { "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "Left Score To Score", "Left Corner To Dot"); LEFT_TWO_CORNER_BC.register(autonChooser); - //before i get flamed ITS NOT EXPERIMENTAL! - //i just needed a distinct name. All it does is instead of pathfinding to the dot path at the end of shooting, it path finds forward to save time + AutonConfig LEFT_TWO_CORNER_V3 = new AutonConfig("V3 Left BC Two Corner", LeftTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "Left Score To Score", "Left Corner To Dot v3"); LEFT_TWO_CORNER_V3.register(autonChooser); @@ -454,8 +453,6 @@ public void configureAutons() { "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "Right Score To Score", "Right Corner To Dot"); RIGHT_TWO_CORNER_BC.register(autonChooser); - //before i get flamed ITS NOT EXPERIMENTAL! - //i just needed a distinct name. All it does is instead of pathfinding to the dot path at the end of shooting, it path finds forward to save time AutonConfig RIGHT_TWO_CORNER_V3 = new AutonConfig("V3 Right BC Two Corner", RightTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "Right Score To Score", "Right Corner To Dot v3"); RIGHT_TWO_CORNER_V3.register(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java index 698365fe..7cb1aba5 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java @@ -77,7 +77,7 @@ public LeftTwoCornerBC(PathPlannerPath... paths) { .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down // new WaitCommand(1.0) ).withTimeout(1.0), - new IntakeDeploy(), //in case digestion doesn't finish neatly + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure not in SOTM and digestion works CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), new SwerveXMode() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java index 0fc8c006..a6d1c533 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java @@ -77,7 +77,7 @@ public LeftTwoCornerBCCenter(PathPlannerPath... paths) { .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down // new WaitCommand(1.0) ).withTimeout(1.0), - new IntakeDeploy(), //in case digestion doesn't finish neatly + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure not in SOTM and digestion works CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), new SwerveXMode() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java index 862efcc1..5ec6866a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java @@ -50,11 +50,11 @@ public RightTwoCornerBC(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).withTimeout(1.0), //uncomment if we have issues new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //changed to 4 bcs of delay (from 5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.0)) //changed to 4 bcs of delay (from 5) // new WaitCommand(1.0).andThen( // new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs @@ -70,14 +70,14 @@ public RightTwoCornerBC(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).withTimeout(1.0), //uncomment if we have issues new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.0)) //cut this down // new WaitCommand(1.0) ).withTimeout(1.0), - new IntakeDeploy(), //in case digestion doesn't finish neatly + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure not in SOTM and digestion works CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), new SwerveXMode() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java index 5252705c..9be3fd2f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java @@ -57,7 +57,7 @@ public RightTwoCornerBCCenter(PathPlannerPath... paths) { // new WaitCommand(1.0).andThen( // new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure not in SOTM and digestion works // NZ Trip 2 new ParallelCommandGroup( @@ -76,7 +76,7 @@ public RightTwoCornerBCCenter(PathPlannerPath... paths) { .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down // new WaitCommand(1.0) ).withTimeout(1.0), - new IntakeDeploy(), //in case digestion doesn't finish neatly + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure not in SOTM and digestion works CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), new SwerveXMode() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/PathfindTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/PathfindTest.java index 1653aae5..cb2726b9 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/test/PathfindTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/PathfindTest.java @@ -6,13 +6,15 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; public class PathfindTest extends SequentialCommandGroup { public PathfindTest(PathPlannerPath... paths) { addCommands( - new SwerveResetPose(paths[0].getStartingDifferentialPose()), + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new WaitCommand(10), CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[1], PathConstraints.unlimitedConstraints(12.0)) ); } From 7db51c76d7ec37f3778d12c398ef50167e8e3b39 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 28 May 2026 14:32:34 -0400 Subject: [PATCH 53/97] FEAT: x on idle in teleop --- .../commands/swerve/SwerveDriveDrive.java | 31 ++++++++++++++----- .../commands/swerve/SwerveDriveFOTM.java | 23 ++++++++++---- .../commands/swerve/SwerveDriveSOTM.java | 4 --- 3 files changed, 41 insertions(+), 17 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index 33fd1e47..6a89b035 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -8,13 +8,15 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.stuylib.streams.numbers.IStream; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.stuypulse.stuylib.streams.vectors.VStream; import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; - +import com.ctre.phoenix6.swerve.SwerveRequest; import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; import com.stuypulse.robot.constants.Settings.Swerve; @@ -31,6 +33,9 @@ public class SwerveDriveDrive extends Command { private final VStream speed; private final IStream turn; + private final BStream isIdle; + private boolean isIdleInit; + public SwerveDriveDrive(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); @@ -53,7 +58,10 @@ public SwerveDriveDrive(Gamepad driver) { ); this.driver = driver; - + isIdle = BStream.create( + () -> getDriverInputAsVelocity().magnitude() <= Drive.DEADBAND && Math.abs(driver.getRightX()) <= Turn.DEADBAND) + .filtered(new BDebounce.Rising(0.5), new BDebounce.Falling(0.1)); + isIdleInit = false; addRequirements(swerve); } @@ -63,10 +71,19 @@ private Vector2D getDriverInputAsVelocity() { @Override public void execute() { - swerve.setControl(swerve.getFieldCentricSwerveRequest() - .withVelocityX(speed.get().x) - .withVelocityY(speed.get().y) - .withRotationalRate(-turn.getAsDouble()) - ); + if (isIdle.get()) { + // if (!isIdleInit) { + // CommandScheduler.getInstance().schedule(new IntakeAutoDigest().repeatedly().onlyWhile(() -> isIdle.get()).andThen(new IntakeDeploy())); + swerve.setControl(new SwerveRequest.SwerveDriveBrake()); + // } + isIdleInit = true; + } else { + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(speed.get().x) + .withVelocityY(speed.get().y) + .withRotationalRate(-turn.getAsDouble()) + ); + isIdleInit = false; + } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java index e8ea6c32..56427a79 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java @@ -5,6 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.swerve; +import com.ctre.phoenix6.swerve.SwerveRequest; import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; import com.stuypulse.robot.constants.Settings; @@ -15,6 +16,8 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.stuylib.streams.numbers.IStream; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.stuypulse.stuylib.streams.vectors.VStream; @@ -35,7 +38,8 @@ public class SwerveDriveFOTM extends Command{ private final VStream speed; private final IStream turn; - + private final BStream isIdle; + public SwerveDriveFOTM(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); superstructure = Superstructure.getInstance(); @@ -58,6 +62,10 @@ public SwerveDriveFOTM(Gamepad driver) { new LowPassFilter(Turn.RC) ); + isIdle = BStream.create( + () -> getDriverInputAsVelocity().magnitude() <= Drive.DEADBAND && Math.abs(driver.getRightX()) <= Turn.DEADBAND) + .filtered(new BDebounce.Rising(0.5), new BDebounce.Falling(0.1)); + this.driver = driver; addRequirements(swerve); @@ -70,11 +78,14 @@ private Vector2D getDriverInputAsVelocity() { @Override public void execute() { - swerve.setControl(swerve.getFieldCentricSwerveRequest() - .withVelocityX(speed.get().x) - .withVelocityY(speed.get().y) - .withRotationalRate(-turn.get())); - + if (isIdle.get()) { + swerve.setControl(new SwerveRequest.SwerveDriveBrake()); + } else { + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(speed.get().x) + .withVelocityY(speed.get().y) + .withRotationalRate(-turn.get())); + } DogLog.log("Swerve/Speed x", speed.get().x); DogLog.log("Swerve/Speed y", speed.get().y); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java index 89303369..d772b251 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -46,7 +46,6 @@ public class SwerveDriveSOTM extends Command { private final IStream turn; private final BStream isIdle; - private boolean isIdleInit; public SwerveDriveSOTM(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); @@ -75,7 +74,6 @@ public SwerveDriveSOTM(Gamepad driver) { .filtered(new BDebounce.Rising(0.5), new BDebounce.Falling(0.1)); this.driver = driver; - isIdleInit = false; addRequirements(swerve); } @@ -93,14 +91,12 @@ public void execute() { // CommandScheduler.getInstance().schedule(new IntakeAutoDigest().repeatedly().onlyWhile(() -> isIdle.get()).andThen(new IntakeDeploy())); swerve.setControl(new SwerveRequest.SwerveDriveBrake()); // } - isIdleInit = true; } else { Vector2D velocity = speed.get(); swerve.setControl(swerve.getFieldCentricSwerveRequest() .withVelocityX(velocity.x) .withVelocityY(velocity.y) .withRotationalRate(-turn.get())); - isIdleInit = false; } } From 3d46c2556d3d8dcffa663b04f04d17ec6829131b Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Thu, 28 May 2026 14:36:55 -0400 Subject: [PATCH 54/97] feat last minute auton changes. Comments made at the top of every single "NEW" auton. Last testing session on pd6 didnt change out of SOTM, this is fixed. --- .../paths/BC Right Score To Score.path | 111 ++++++++++++++++++ .../paths/Left Corner To Dot v4.path | 67 +++++++++++ .../paths/Right Bite Score To Score.path | 4 +- .../paths/Right Corner To Dot v4.path | 63 ++++++++++ .../paths/Right Score To NZ (F).path | 4 +- .../paths/Right Score To Score.path | 6 +- .../com/stuypulse/robot/RobotContainer.java | 31 +++++ .../auton/regular/RightTwoCornerBC.java | 10 +- .../auton/regular/RightTwoCornerBCCenter.java | 5 +- .../auton/regular/RightTwoCornerBCNew.java | 91 ++++++++++++++ .../regular/RightTwoCornerBCNewFive.java | 82 +++++++++++++ .../regular/RightTwoCornerBCNewFour.java | 83 +++++++++++++ .../auton/regular/RightTwoCornerBCNewSix.java | 84 +++++++++++++ .../regular/RightTwoCornerBCNewThree.java | 84 +++++++++++++ .../auton/regular/RightTwoCornerBCNewTwo.java | 93 +++++++++++++++ .../robot/subsystems/leds/LEDController.java | 3 + 16 files changed, 806 insertions(+), 15 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/BC Right Score To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Left Corner To Dot v4.path create mode 100644 src/main/deploy/pathplanner/paths/Right Corner To Dot v4.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNew.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFive.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFour.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewSix.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewThree.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewTwo.java diff --git a/src/main/deploy/pathplanner/paths/BC Right Score To Score.path b/src/main/deploy/pathplanner/paths/BC Right Score To Score.path new file mode 100644 index 00000000..34fed6be --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right Score To Score.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.48, + "y": 0.57 + }, + "prevControl": null, + "nextControl": { + "x": 7.083082715798332, + "y": 0.5311980499756427 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.850470756062768, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 5.850470756062768, + "y": 0.3280741797432247 + }, + "nextControl": { + "x": 5.850470756062768, + "y": 4.150659142168011 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.302881844380403, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 7.215816731986725, + "y": 3.83143356936277 + }, + "nextControl": { + "x": 7.525057636887608, + "y": 0.34949567723342856 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.559 + }, + "prevControl": { + "x": 7.6730891295992585, + "y": 0.5209487097696721 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17051509769094172, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.644760213143872, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.1812366737739914, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.4179104477611943, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Dot v4.path b/src/main/deploy/pathplanner/paths/Left Corner To Dot v4.path new file mode 100644 index 00000000..479fa787 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner To Dot v4.path @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.52, + "y": 7.42 + }, + "prevControl": null, + "nextControl": { + "x": 5.045616333629863, + "y": 7.427897901816431 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 7.441 + }, + "prevControl": { + "x": 7.931262293036603, + "y": 7.447711041684223 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.36281588447653357, + "rotationDegrees": -2.0 + }, + { + "waypointRelativePos": 0.7914438502673795, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path index 631cbbfe..909087b9 100644 --- a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.2824809160305346, + "x": 3.282, "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 5.236218433862203, + "x": 5.2357375178316685, "y": 0.571938659058489 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot v4.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot v4.path new file mode 100644 index 00000000..2d05be59 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot v4.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.48, + "y": 0.57 + }, + "prevControl": null, + "nextControl": { + "x": 6.281243937232524, + "y": 0.6884179743223975 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 0.559 + }, + "prevControl": { + "x": 8.007798061746946, + "y": 0.5155879555832673 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2547717842323645, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.75, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 96401506..5be40485 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.2824809160305346, + "x": 3.282, "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 6.853550816173187, + "x": 6.8530699001426525, "y": 0.5460613409415132 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 7cbce93a..db85e6e5 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.2824809160305346, + "x": 3.282, "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 6.885563480741797, + "x": 6.885082564711262, "y": 0.5201840228245365 }, "isLocked": false, @@ -52,7 +52,7 @@ "y": 0.559 }, "prevControl": { - "x": 7.67308912959926, + "x": 7.673089129599259, "y": 0.5209487097696721 }, "nextControl": null, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index bb44be70..8a2118eb 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -21,6 +21,12 @@ import com.stuypulse.robot.commands.auton.regular.RightTwoCorner; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBC; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCCenter; +import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCNew; +import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCNewFive; +import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCNewFour; +import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCNewSix; +import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCNewThree; +import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCNewTwo; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerShallow; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; @@ -460,6 +466,31 @@ public void configureAutons() { "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "Right Score To Score", "Right Corner To Dot v3"); RIGHT_TWO_CORNER_V3.register(autonChooser); + AutonConfig RIGHT_TWO_CORNER_NEW = new AutonConfig("NEW Right BC Two Corner", RightTwoCornerBCNew::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); + RIGHT_TWO_CORNER_V3.register(autonChooser); + + AutonConfig RIGHT_TWO_CORNER_NEW_TWO = new AutonConfig("NEW TWO Right BC Two Corner", RightTwoCornerBCNewTwo::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); + RIGHT_TWO_CORNER_V3.register(autonChooser); + + AutonConfig RIGHT_TWO_CORNER_NEW_THREE = new AutonConfig("NEW THREE Right BC Two Corner", RightTwoCornerBCNewThree::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); + RIGHT_TWO_CORNER_V3.register(autonChooser); + + AutonConfig RIGHT_TWO_CORNER_NEW_FOUR = new AutonConfig("NEW FOUR Right BC Two Corner", RightTwoCornerBCNewFour::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); + RIGHT_TWO_CORNER_V3.register(autonChooser); + + AutonConfig RIGHT_TWO_CORNER_NEW_FIVE = new AutonConfig("NEW FIVE Right BC Two Corner", RightTwoCornerBCNewFive::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); + RIGHT_TWO_CORNER_V3.register(autonChooser); + + AutonConfig RIGHT_TWO_CORNER_NEW_SIX = new AutonConfig("NEW SIX Right BC Two Corner", RightTwoCornerBCNewSix::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); + RIGHT_TWO_CORNER_V3.register(autonChooser); + + AutonConfig RIGHT_TWO_CORNER_BC_CENTER = new AutonConfig("BC Right Two Corner BC Center", RightTwoCornerBCCenter::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "Right Score To Score", "Right Corner To Center Dot"); RIGHT_TWO_CORNER_BC_CENTER.register(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java index 862efcc1..9c177c0a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java @@ -54,10 +54,10 @@ public RightTwoCornerBC(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //changed to 4 bcs of delay (from 5) - // new WaitCommand(1.0).andThen( - // new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) - ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(2.0)), //changed to 4 bcs of delay (from 5) + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(2.5)) + ).withTimeout(2.0), //update to 3.0 for actual, this just ensures pathfinding occurs new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating // NZ Trip 2 @@ -77,7 +77,7 @@ public RightTwoCornerBC(PathPlannerPath... paths) { .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down // new WaitCommand(1.0) ).withTimeout(1.0), - new IntakeDeploy(), //in case digestion doesn't finish neatly + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), new SwerveXMode() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java index 5252705c..632e4a4f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java @@ -57,8 +57,7 @@ public RightTwoCornerBCCenter(PathPlannerPath... paths) { // new WaitCommand(1.0).andThen( // new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[3], PathConstraints.unlimitedConstraints(12)), @@ -76,7 +75,7 @@ public RightTwoCornerBCCenter(PathPlannerPath... paths) { .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down // new WaitCommand(1.0) ).withTimeout(1.0), - new IntakeDeploy(), //in case digestion doesn't finish neatly + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), new SwerveXMode() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNew.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNew.java new file mode 100644 index 00000000..61d617bd --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNew.java @@ -0,0 +1,91 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightTwoCornerBCNew extends SequentialCommandGroup { + //this one puts a timeout only on parallel command + //optimizations, remove wait commands + + public RightTwoCornerBCNew(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty())), //changed to 4 bcs of delay (from 5) + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty())) + ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty())), //changed to 4 bcs of delay (from 5) + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty())) + ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + + new SwerveXMode() + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFive.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFive.java new file mode 100644 index 00000000..81a41d38 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFive.java @@ -0,0 +1,82 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightTwoCornerBCNewFive extends SequentialCommandGroup { + //this one removes pathfinder + optimized timeouts, and removes is hopper empty, and puts timeout only on digestion + public RightTwoCornerBCNewFive(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest().withTimeout(3) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest().withTimeout(3) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new SwerveXMode() + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFour.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFour.java new file mode 100644 index 00000000..2dd43f23 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFour.java @@ -0,0 +1,83 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightTwoCornerBCNewFour extends SequentialCommandGroup { + //this one removes pathfinder, and removes is hopper empty, and puts timeout only on group, not even on digestion + public RightTwoCornerBCNewFour(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest() + ).withTimeout(2.0), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest() + ).withTimeout(2.0), + + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new SwerveXMode() + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewSix.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewSix.java new file mode 100644 index 00000000..9ce88dc4 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewSix.java @@ -0,0 +1,84 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightTwoCornerBCNewSix extends SequentialCommandGroup { + //this one removes pathfinder, removes is hopper empty, and puts timeout only on the path + public RightTwoCornerBCNewSix(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).withTimeout(2.0),// uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest() + ), //update to 3.0 for actual, this just ensures pathfinding occurs + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).withTimeout(1.0), //uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest().withTimeout(2) + // new WaitCommand(1.0) + ).withTimeout(2.0), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new SwerveXMode() + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewThree.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewThree.java new file mode 100644 index 00000000..499c6f13 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewThree.java @@ -0,0 +1,84 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightTwoCornerBCNewThree extends SequentialCommandGroup { + //this one removes pathfinder + optimized timeouts, and removes is hopper empty, with timeout on group + public RightTwoCornerBCNewThree(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest().withTimeout(2) + ).withTimeout(2.0), //update to 3.0 for actual, this just ensures pathfinding occurs + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest().withTimeout(2) + // new WaitCommand(1.0) + ).withTimeout(2.0), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new SwerveXMode() + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewTwo.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewTwo.java new file mode 100644 index 00000000..f623760b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewTwo.java @@ -0,0 +1,93 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightTwoCornerBCNewTwo extends SequentialCommandGroup { + + public RightTwoCornerBCNewTwo(PathPlannerPath... paths) { + //SKETCHY + //this one removes pathfinder + optimized timeouts + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.5)), //changed to 4 bcs of delay (from 5) + new WaitCommand(0.5).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.5)) + ), //update to 3.0 for actual, this just ensures pathfinding occurs + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.5)), //changed to 4 bcs of delay (from 5) + new WaitCommand(0.5).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.0)) + ), //update to 3.0 for actual, this just ensures pathfinding occurs + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + + new SwerveXMode() + //TODO: not accounting for the first loop!! - in terms of paths!!! + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 46f217af..a328ab36 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -190,6 +190,9 @@ public void periodicAfterScheduler() { lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); initialPoseUpdated = true; } + else { + + } if (initialPoseUpdated && lastPoseOnAprilTag.getTranslation() From 0ac8f5f6a7526a9a2ac9f538e2d097eadf39e1e8 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 28 May 2026 15:39:48 -0400 Subject: [PATCH 55/97] WORK: right side autos (center and right dot) --- .../paths/BC Right Bite Score To Score.path | 145 ++++++++++++++++++ .../paths/Right Corner To Center Dot v2.path | 83 ++++++++++ .../paths/Right Corner To Dot v5.path | 63 ++++++++ .../com/stuypulse/robot/RobotContainer.java | 19 +-- .../auton/regular/RightTwoCornerBCNew.java | 14 +- .../regular/RightTwoCornerBCNewCenter.java | 91 +++++++++++ 6 files changed, 396 insertions(+), 19 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Right Corner To Center Dot v2.path create mode 100644 src/main/deploy/pathplanner/paths/Right Corner To Dot v5.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewCenter.java diff --git a/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path new file mode 100644 index 00000000..9240bf7f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.48, + "y": 0.57 + }, + "prevControl": null, + "nextControl": { + "x": 5.433737517831668, + "y": 0.5829386590584889 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.739514978601996, + "y": 1.0008844507845935 + }, + "prevControl": { + "x": 7.7167792788333145, + "y": 0.31502417442937847 + }, + "nextControl": { + "x": 7.817146932952923, + "y": 3.3427817403708993 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.002011412268189, + "y": 3.925021398002854 + }, + "prevControl": { + "x": 8.114526380651917, + "y": 3.9034191656070543 + }, + "nextControl": { + "x": 5.669329529243937, + "y": 3.9508987161198283 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.031611982881596, + "y": 2.0877318116975756 + }, + "prevControl": { + "x": 6.009732033987853, + "y": 3.174435940086786 + }, + "nextControl": { + "x": 6.070427960057061, + "y": 0.15987161198288025 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.559 + }, + "prevControl": { + "x": 6.1480599144079875, + "y": 0.5460613409415132 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5714285714285716, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.9722814498933815, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.3475479744136523, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.936034115138597, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.4818763326225852, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.0618336886993425, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.4968017057569094, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0353753235547816, + "maxWaypointRelativePos": 3.1406384814495194, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot v2.path b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot v2.path new file mode 100644 index 00000000..6d34c835 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot v2.path @@ -0,0 +1,83 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.48, + "y": 0.57 + }, + "prevControl": null, + "nextControl": { + "x": 5.818616522811345, + "y": 0.5423255240443889 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.20456226879871, + "y": 1.7965413070243335 + }, + "prevControl": { + "x": 6.193748458692971, + "y": 0.6502774352651044 + }, + "nextControl": { + "x": 6.2118058772904625, + "y": 2.564363807519222 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.27, + "y": 4.067 + }, + "prevControl": { + "x": 5.685499383471953, + "y": 4.089069050550844 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.23, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.109909909909912, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.6432432432432433, + "rotationDegrees": 0.4456470247826539 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot v5.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot v5.path new file mode 100644 index 00000000..f82812f4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot v5.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.3, + "y": 0.55 + }, + "prevControl": null, + "nextControl": { + "x": 6.101243937232525, + "y": 0.6684179743223976 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 0.559 + }, + "prevControl": { + "x": 8.007798061746946, + "y": 0.5155879555832673 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2547717842323645, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.75, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 35c917ff..6389ba10 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -464,28 +464,29 @@ public void configureAutons() { RIGHT_TWO_CORNER_V3.register(autonChooser); AutonConfig RIGHT_TWO_CORNER_NEW = new AutonConfig("NEW Right BC Two Corner", RightTwoCornerBCNew::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); - RIGHT_TWO_CORNER_V3.register(autonChooser); + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot v5"); + RIGHT_TWO_CORNER_NEW.register(autonChooser); - AutonConfig RIGHT_TWO_CORNER_NEW_TWO = new AutonConfig("NEW TWO Right BC Two Corner", RightTwoCornerBCNewTwo::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); - RIGHT_TWO_CORNER_V3.register(autonChooser); + //NEW TWO IS TO CENTER DOT AS OF RIGHT NOW + AutonConfig RIGHT_TWO_CORNER_NEW_TWO = new AutonConfig("NEW TWO Right BC Two Corner", RightTwoCornerBCNew::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Center Dot v2"); + RIGHT_TWO_CORNER_NEW_TWO.register(autonChooser); AutonConfig RIGHT_TWO_CORNER_NEW_THREE = new AutonConfig("NEW THREE Right BC Two Corner", RightTwoCornerBCNewThree::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); - RIGHT_TWO_CORNER_V3.register(autonChooser); + RIGHT_TWO_CORNER_NEW_THREE.register(autonChooser); AutonConfig RIGHT_TWO_CORNER_NEW_FOUR = new AutonConfig("NEW FOUR Right BC Two Corner", RightTwoCornerBCNewFour::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); - RIGHT_TWO_CORNER_V3.register(autonChooser); + RIGHT_TWO_CORNER_NEW_FOUR.register(autonChooser); AutonConfig RIGHT_TWO_CORNER_NEW_FIVE = new AutonConfig("NEW FIVE Right BC Two Corner", RightTwoCornerBCNewFive::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); - RIGHT_TWO_CORNER_V3.register(autonChooser); + RIGHT_TWO_CORNER_NEW_FIVE.register(autonChooser); AutonConfig RIGHT_TWO_CORNER_NEW_SIX = new AutonConfig("NEW SIX Right BC Two Corner", RightTwoCornerBCNewSix::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); - RIGHT_TWO_CORNER_V3.register(autonChooser); + RIGHT_TWO_CORNER_NEW_SIX.register(autonChooser); AutonConfig RIGHT_TWO_CORNER_BC_CENTER = new AutonConfig("BC Right Two Corner BC Center", RightTwoCornerBCCenter::new, prevWaitTimeOne, prevWaitTimeTwo, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNew.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNew.java index 61d617bd..72fa5595 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNew.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNew.java @@ -54,11 +54,8 @@ public RightTwoCornerBCNew(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty())), //changed to 4 bcs of delay (from 5) - new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty())) - ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs + new IntakeAutoDigest() + ).withTimeout(1.25), //update to 3.0 for actual, this just ensures pathfinding occurs new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating // NZ Trip 2 @@ -74,11 +71,8 @@ public RightTwoCornerBCNew(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty())), //changed to 4 bcs of delay (from 5) - new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty())) - ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs + new IntakeAutoDigest() + ).withTimeout(4.2), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewCenter.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewCenter.java new file mode 100644 index 00000000..8dffaa98 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewCenter.java @@ -0,0 +1,91 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightTwoCornerBCNewCenter extends SequentialCommandGroup { + //this one puts a timeout only on parallel command + //optimizations, remove wait commands + + public RightTwoCornerBCNewCenter(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty())), //changed to 4 bcs of delay (from 5) + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty())) + ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty())), //changed to 4 bcs of delay (from 5) + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty())) + ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + + new SwerveXMode() + ); + + } + +} From 02c69d5f7cbb97fce34d48dc0b1e68546ae3a89e Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 28 May 2026 17:26:33 -0400 Subject: [PATCH 56/97] feat: testing changes. everything works :D - left is not done, will do it all at home --- .../autos/BC Left Corner Bite CD.auto | 2 +- .../autos/BC Left Corner Bite.auto | 2 +- .../autos/BC Right Corner Bite CD.auto | 2 +- .../autos/BC Right Corner Bite.auto | 2 +- .../paths/BC Left Corner Bite.path | 6 +- .../paths/BC Left NZ To Score.path | 18 ++-- .../paths/BC Right Bite Score To Score.path | 4 +- .../paths/BC Right Corner Bite.path | 4 +- .../paths/BC Right NZ To Score.path | 18 ++-- ...ath => Left Corner To Center Dot XXX.path} | 0 ...ot v4.path => Left Corner To Dot XXX.path} | 0 .../paths/Left Corner To Dot v3.path | 67 ------------- .../pathplanner/paths/Left Corner to Dot.path | 67 ------------- ...th => Right Corner To Center Dot pt1.path} | 35 +++---- ...th => Right Corner To Center Dot pt2.path} | 31 +++---- .../paths/Right Corner To Center Dot v2.path | 83 ----------------- .../paths/Right Corner To Center Dot.path | 14 ++- .../paths/Right Corner To Dot v3.path | 77 --------------- .../paths/Right Corner To Dot.path | 30 ++---- src/main/java/com/stuypulse/robot/Robot.java | 2 + .../com/stuypulse/robot/RobotContainer.java | 72 ++++---------- ...BCNewThree.java => CenterTwoCornerBC.java} | 22 +++-- .../auton/regular/LeftTwoCornerBC.java | 88 ------------------ .../auton/regular/LeftTwoCornerBCCenter.java | 89 ------------------ .../auton/regular/RightTwoCornerBC.java | 88 ------------------ .../auton/regular/RightTwoCornerBCCenter.java | 86 ----------------- .../regular/RightTwoCornerBCNewCenter.java | 91 ------------------ .../regular/RightTwoCornerBCNewFive.java | 82 ---------------- .../regular/RightTwoCornerBCNewFour.java | 83 ----------------- .../auton/regular/RightTwoCornerBCNewSix.java | 84 ----------------- .../auton/regular/RightTwoCornerBCNewTwo.java | 93 ------------------- ...htTwoCornerBCNew.java => TwoCornerBC.java} | 11 ++- 32 files changed, 107 insertions(+), 1246 deletions(-) rename src/main/deploy/pathplanner/paths/{Left Corner to Center Dot.path => Left Corner To Center Dot XXX.path} (100%) rename src/main/deploy/pathplanner/paths/{Left Corner To Dot v4.path => Left Corner To Dot XXX.path} (100%) delete mode 100644 src/main/deploy/pathplanner/paths/Left Corner To Dot v3.path delete mode 100644 src/main/deploy/pathplanner/paths/Left Corner to Dot.path rename src/main/deploy/pathplanner/paths/{Right Corner To Dot v5.path => Right Corner To Center Dot pt1.path} (60%) rename src/main/deploy/pathplanner/paths/{Right Corner To Dot v4.path => Right Corner To Center Dot pt2.path} (63%) delete mode 100644 src/main/deploy/pathplanner/paths/Right Corner To Center Dot v2.path delete mode 100644 src/main/deploy/pathplanner/paths/Right Corner To Dot v3.path rename src/main/java/com/stuypulse/robot/commands/auton/regular/{RightTwoCornerBCNewThree.java => CenterTwoCornerBC.java} (86%) delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewCenter.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFive.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFour.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewSix.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewTwo.java rename src/main/java/com/stuypulse/robot/commands/auton/regular/{RightTwoCornerBCNew.java => TwoCornerBC.java} (92%) diff --git a/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto b/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto index beddcdaa..a1a050dd 100644 --- a/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto +++ b/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto @@ -37,7 +37,7 @@ { "type": "path", "data": { - "pathName": "Left Corner To Center Dot" + "pathName": "Left Corner To Center Dot XXX" } } ] diff --git a/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto index 1ea9a019..06fa4b7c 100644 --- a/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto @@ -37,7 +37,7 @@ { "type": "path", "data": { - "pathName": "Left Corner To Dot" + "pathName": null } } ] diff --git a/src/main/deploy/pathplanner/autos/BC Right Corner Bite CD.auto b/src/main/deploy/pathplanner/autos/BC Right Corner Bite CD.auto index af4828c4..e31b801b 100644 --- a/src/main/deploy/pathplanner/autos/BC Right Corner Bite CD.auto +++ b/src/main/deploy/pathplanner/autos/BC Right Corner Bite CD.auto @@ -37,7 +37,7 @@ { "type": "path", "data": { - "pathName": "Right Corner To Center Dot" + "pathName": null } } ] diff --git a/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto index 8bcb3d18..e31b801b 100644 --- a/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto @@ -37,7 +37,7 @@ { "type": "path", "data": { - "pathName": "Right Corner To Dot" + "pathName": null } } ] diff --git a/src/main/deploy/pathplanner/paths/BC Left Corner Bite.path b/src/main/deploy/pathplanner/paths/BC Left Corner Bite.path index f79f7401..e7041c51 100644 --- a/src/main/deploy/pathplanner/paths/BC Left Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/BC Left Corner Bite.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 7.584251069900143, - "y": 7.6935575540939345 + "y": 7.693557554093935 }, "isLocked": false, "linkedName": null @@ -17,11 +17,11 @@ { "anchor": { "x": 7.909455555555555, - "y": 5.0 + "y": 5.61 }, "prevControl": { "x": 7.9484375712176165, - "y": 6.305755429044663 + "y": 6.915755429044664 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path b/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path index ddcae2ff..0b448fbd 100644 --- a/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path @@ -4,12 +4,12 @@ { "anchor": { "x": 7.909455555555555, - "y": 5.0 + "y": 5.61 }, "prevControl": null, "nextControl": { - "x": 6.204177777777778, - "y": 4.902555555555557 + "x": 6.857055555555556, + "y": 5.574622222222222 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 6.507 }, "prevControl": { - "x": 6.447665111532734, - "y": 5.075127676809553 + "x": 6.428299999999999, + "y": 6.091077777777778 }, "nextControl": { - "x": 6.399064196369385, - "y": 7.569997891332222 + "x": 6.3979760418229406, + "y": 7.569976136537918 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 7.441 }, "prevControl": { - "x": 6.311366666666668, - "y": 7.41038597242035 + "x": 5.9215888888888895, + "y": 7.416322222222221 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path index 9240bf7f..8506962c 100644 --- a/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.48, + "x": 3.53, "y": 0.57 }, "prevControl": null, "nextControl": { - "x": 5.433737517831668, + "x": 5.483737517831668, "y": 0.5829386590584889 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path b/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path index 01d71b55..980515ba 100644 --- a/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path @@ -17,11 +17,11 @@ { "anchor": { "x": 7.909455555555555, - "y": 3.0 + "y": 2.39 }, "prevControl": { "x": 7.948433333333332, - "y": 1.6942444444444456 + "y": 1.0842444444444457 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path b/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path index a542a80b..5fd59ceb 100644 --- a/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path @@ -4,12 +4,12 @@ { "anchor": { "x": 7.909455555555555, - "y": 3.0 + "y": 2.39 }, "prevControl": null, "nextControl": { - "x": 6.204177777777778, - "y": 2.902555555555556 + "x": 6.701144444444445, + "y": 2.3687 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 1.4925534950071324 }, "prevControl": { - "x": 6.44766178400938, - "y": 2.924425883014991 + "x": 6.433111579891319, + "y": 1.793603073252706 }, "nextControl": { - "x": 6.399066666666666, - "y": 0.42955555555555525 + "x": 6.379577777777778, + "y": 0.5854666666666649 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 0.559 }, "prevControl": { - "x": 6.311366666666668, - "y": 0.5283859724203506 + "x": 6.028777777777779, + "y": 0.5367444444444436 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path b/src/main/deploy/pathplanner/paths/Left Corner To Center Dot XXX.path similarity index 100% rename from src/main/deploy/pathplanner/paths/Left Corner to Center Dot.path rename to src/main/deploy/pathplanner/paths/Left Corner To Center Dot XXX.path diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Dot v4.path b/src/main/deploy/pathplanner/paths/Left Corner To Dot XXX.path similarity index 100% rename from src/main/deploy/pathplanner/paths/Left Corner To Dot v4.path rename to src/main/deploy/pathplanner/paths/Left Corner To Dot XXX.path diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Dot v3.path b/src/main/deploy/pathplanner/paths/Left Corner To Dot v3.path deleted file mode 100644 index 0e69c7db..00000000 --- a/src/main/deploy/pathplanner/paths/Left Corner To Dot v3.path +++ /dev/null @@ -1,67 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.612, - "y": 7.441 - }, - "prevControl": null, - "nextControl": { - "x": 5.137616333629864, - "y": 7.448897901816431 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.254, - "y": 7.441 - }, - "prevControl": { - "x": 7.931262293036603, - "y": 7.447711041684223 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.35, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.36281588447653357, - "rotationDegrees": -2.0 - }, - { - "waypointRelativePos": 0.7914438502673795, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.19, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "BC To Dot", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner to Dot.path b/src/main/deploy/pathplanner/paths/Left Corner to Dot.path deleted file mode 100644 index 37ebd0ed..00000000 --- a/src/main/deploy/pathplanner/paths/Left Corner to Dot.path +++ /dev/null @@ -1,67 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.308, - "y": 7.441 - }, - "prevControl": null, - "nextControl": { - "x": 4.833616333629863, - "y": 7.448897901816431 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.254, - "y": 7.441 - }, - "prevControl": { - "x": 7.931262293036603, - "y": 7.447711041684223 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.35, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.36281588447653357, - "rotationDegrees": -2.0 - }, - { - "waypointRelativePos": 0.7914438502673795, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.19, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "BC To Dot", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot v5.path b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt1.path similarity index 60% rename from src/main/deploy/pathplanner/paths/Right Corner To Dot v5.path rename to src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt1.path index f82812f4..a483881a 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Dot v5.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt1.path @@ -3,41 +3,32 @@ "waypoints": [ { "anchor": { - "x": 3.3, - "y": 0.55 + "x": 3.28, + "y": 0.56 }, "prevControl": null, "nextControl": { - "x": 6.101243937232525, - "y": 0.6684179743223976 + "x": 3.548577777777778, + "y": 0.5559777777777778 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.254, - "y": 0.559 + "x": 6.27, + "y": 0.57 }, "prevControl": { - "x": 8.007798061746946, - "y": 0.5155879555832673 + "x": 6.02, + "y": 0.57 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.2547717842323645, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.75, - "rotationDegrees": 180.0 - } - ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -46,17 +37,17 @@ "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": 180.0 + "velocity": 3.8, + "rotation": 0.0 }, "reversed": false, "folder": "BC To Dot", "idealStartingState": { - "velocity": 0, + "velocity": 0.0, "rotation": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot v4.path b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt2.path similarity index 63% rename from src/main/deploy/pathplanner/paths/Right Corner To Dot v4.path rename to src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt2.path index 2d05be59..e2c90078 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Dot v4.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt2.path @@ -3,41 +3,32 @@ "waypoints": [ { "anchor": { - "x": 3.48, + "x": 6.27, "y": 0.57 }, "prevControl": null, "nextControl": { - "x": 6.281243937232524, - "y": 0.6884179743223975 + "x": 6.436401056802655, + "y": 0.7829836402426446 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.254, - "y": 0.559 + "x": 8.289, + "y": 4.064233333333333 }, "prevControl": { - "x": 8.007798061746946, - "y": 0.5155879555832673 + "x": 8.135084631168585, + "y": 3.8672306449316527 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.2547717842323645, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.75, - "rotationDegrees": 180.0 - } - ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -46,12 +37,12 @@ "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false + "nominalVoltage": 12.7, + "unlimited": true }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": -119.99999999999999 }, "reversed": false, "folder": "BC To Dot", diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot v2.path b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot v2.path deleted file mode 100644 index 6d34c835..00000000 --- a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot v2.path +++ /dev/null @@ -1,83 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.48, - "y": 0.57 - }, - "prevControl": null, - "nextControl": { - "x": 5.818616522811345, - "y": 0.5423255240443889 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.20456226879871, - "y": 1.7965413070243335 - }, - "prevControl": { - "x": 6.193748458692971, - "y": 0.6502774352651044 - }, - "nextControl": { - "x": 6.2118058772904625, - "y": 2.564363807519222 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.27, - "y": 4.067 - }, - "prevControl": { - "x": 5.685499383471953, - "y": 4.089069050550844 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.23, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1.109909909909912, - "rotationDegrees": 90.0 - }, - { - "waypointRelativePos": 1.6432432432432433, - "rotationDegrees": 0.4456470247826539 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.19, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "BC To Dot", - "idealStartingState": { - "velocity": 0.0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path index bbea90e7..21c9f268 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.282, - "y": 0.559 + "x": 3.48, + "y": 0.57 }, "prevControl": null, "nextControl": { - "x": 5.6206165228113445, - "y": 0.531325524044389 + "x": 5.818616522811345, + "y": 0.5423255240443889 }, "isLocked": false, "linkedName": null @@ -56,6 +56,10 @@ { "waypointRelativePos": 1.6432432432432433, "rotationDegrees": 0.4456470247826539 + }, + { + "waypointRelativePos": 1.7, + "rotationDegrees": -10.05657822827732 } ], "constraintZones": [], @@ -71,7 +75,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": -90.0 + "rotation": 180.0 }, "reversed": false, "folder": "BC To Dot", diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot v3.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot v3.path deleted file mode 100644 index 578d5ceb..00000000 --- a/src/main/deploy/pathplanner/paths/Right Corner To Dot v3.path +++ /dev/null @@ -1,77 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.612, - "y": 0.559 - }, - "prevControl": null, - "nextControl": { - "x": 6.413243937232525, - "y": 0.6774179743223976 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.254, - "y": 0.559 - }, - "prevControl": { - "x": 8.500236542443773, - "y": 0.6022153348400272 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3795309168443479, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.75, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.75, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.19, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "BC To Dot", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path index 560a1799..4bb7223d 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.308, - "y": 0.559 + "x": 3.28, + "y": 0.56 }, "prevControl": null, "nextControl": { - "x": 6.109243937232525, - "y": 0.6774179743223976 + "x": 6.081243937232525, + "y": 0.6784179743223976 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 0.559 }, "prevControl": { - "x": 8.500236542443773, - "y": 0.6022153348400271 + "x": 8.007798061746946, + "y": 0.5155879555832673 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.3795309168443479, + "waypointRelativePos": 0.2547717842323645, "rotationDegrees": 0.0 }, { @@ -38,21 +38,7 @@ "rotationDegrees": 180.0 } ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.75, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index be63fdcd..85a3dcd6 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -16,6 +16,7 @@ import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; +import com.stuypulse.robot.commands.superstructure.SuperstructureStow; import com.stuypulse.robot.commands.swerve.SwerveAutonInit; import com.stuypulse.robot.commands.swerve.SwerveTeleopInit; import com.stuypulse.robot.commands.vision.BlackListAllTagsForAllCameras; @@ -256,6 +257,7 @@ public void teleopInit() { CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); CommandScheduler.getInstance().schedule(new IntakeDeploy()); + CommandScheduler.getInstance().schedule(new HandoffStop().alongWith(new SpindexerStop())); if (auto != null) { auto.cancel(); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 6389ba10..2b452b23 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,26 +7,18 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.auton.regular.CenterTwoCornerBC; import com.stuypulse.robot.commands.auton.regular.Depot; import com.stuypulse.robot.commands.auton.regular.LeftBump; import com.stuypulse.robot.commands.auton.regular.LeftFollow; import com.stuypulse.robot.commands.auton.regular.LeftTwoCorner; -import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerBC; -import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerBCCenter; import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerShallow; import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; import com.stuypulse.robot.commands.auton.regular.RightBump; import com.stuypulse.robot.commands.auton.regular.RightFollow; import com.stuypulse.robot.commands.auton.regular.RightTwoCorner; -import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBC; -import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCCenter; -import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCNew; -import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCNewFive; -import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCNewFour; -import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCNewSix; -import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCNewThree; -import com.stuypulse.robot.commands.auton.regular.RightTwoCornerBCNewTwo; +import com.stuypulse.robot.commands.auton.regular.TwoCornerBC; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerShallow; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; @@ -437,61 +429,30 @@ public void configureAutons() { AutonConfig LEFT_TWO_CORNER = new AutonConfig("Left Two Corner", LeftTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); LEFT_TWO_CORNER.register(autonChooser); - - AutonConfig LEFT_TWO_CORNER_BC = new AutonConfig("BC Left Two Corner", LeftTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "Left Score To Score", "Left Corner To Dot"); - LEFT_TWO_CORNER_BC.register(autonChooser); - - - AutonConfig LEFT_TWO_CORNER_V3 = new AutonConfig("V3 Left BC Two Corner", LeftTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + + //CHANGE THESE PATHS !! + AutonConfig BC_LEFT_TWO_CORNER = new AutonConfig("BC Left Two Corner", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "Left Score To Score", "Left Corner To Dot v3"); - LEFT_TWO_CORNER_V3.register(autonChooser); + BC_LEFT_TWO_CORNER.register(autonChooser); - AutonConfig LEFT_TWO_CORNER_BC_CENTER = new AutonConfig("BC Left Two Corner BC Center", LeftTwoCornerBCCenter::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig CENTER_BC_LEFT_TWO_CORNER = new AutonConfig("BC Center Left Two Corner", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "Left Score To Score", "Left Corner To Center Dot"); - LEFT_TWO_CORNER_BC_CENTER.register(autonChooser); + CENTER_BC_LEFT_TWO_CORNER.register(autonChooser); + // ***** AutonConfig RIGHT_TWO_CORNER = new AutonConfig("Right Two Corner", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CORNER.register(autonChooser); - AutonConfig RIGHT_TWO_CORNER_BC = new AutonConfig("BC Right Two Corner", RightTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "Right Score To Score", "Right Corner To Dot"); - RIGHT_TWO_CORNER_BC.register(autonChooser); - - AutonConfig RIGHT_TWO_CORNER_V3 = new AutonConfig("V3 Right BC Two Corner", RightTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "Right Score To Score", "Right Corner To Dot v3"); - RIGHT_TWO_CORNER_V3.register(autonChooser); - - AutonConfig RIGHT_TWO_CORNER_NEW = new AutonConfig("NEW Right BC Two Corner", RightTwoCornerBCNew::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot v5"); - RIGHT_TWO_CORNER_NEW.register(autonChooser); + AutonConfig BC_RIGHT_TWO_CORNER = new AutonConfig("BC Right Two Corner", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); + BC_RIGHT_TWO_CORNER.register(autonChooser); //NEW TWO IS TO CENTER DOT AS OF RIGHT NOW - AutonConfig RIGHT_TWO_CORNER_NEW_TWO = new AutonConfig("NEW TWO Right BC Two Corner", RightTwoCornerBCNew::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Center Dot v2"); - RIGHT_TWO_CORNER_NEW_TWO.register(autonChooser); - - AutonConfig RIGHT_TWO_CORNER_NEW_THREE = new AutonConfig("NEW THREE Right BC Two Corner", RightTwoCornerBCNewThree::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); - RIGHT_TWO_CORNER_NEW_THREE.register(autonChooser); - - AutonConfig RIGHT_TWO_CORNER_NEW_FOUR = new AutonConfig("NEW FOUR Right BC Two Corner", RightTwoCornerBCNewFour::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); - RIGHT_TWO_CORNER_NEW_FOUR.register(autonChooser); - - AutonConfig RIGHT_TWO_CORNER_NEW_FIVE = new AutonConfig("NEW FIVE Right BC Two Corner", RightTwoCornerBCNewFive::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); - RIGHT_TWO_CORNER_NEW_FIVE.register(autonChooser); - - AutonConfig RIGHT_TWO_CORNER_NEW_SIX = new AutonConfig("NEW SIX Right BC Two Corner", RightTwoCornerBCNewSix::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot v4"); - RIGHT_TWO_CORNER_NEW_SIX.register(autonChooser); - - - AutonConfig RIGHT_TWO_CORNER_BC_CENTER = new AutonConfig("BC Right Two Corner BC Center", RightTwoCornerBCCenter::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "Right Score To Score", "Right Corner To Center Dot"); - RIGHT_TWO_CORNER_BC_CENTER.register(autonChooser); + //Update other paths on pathplanner + AutonConfig BC_CENTER_RIGHT_TWO_CORNER = new AutonConfig("Center BC Right Two Corner", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); + BC_CENTER_RIGHT_TWO_CORNER.register(autonChooser); AutonConfig LEFT_TWO_CORNER_SHALLOW = new AutonConfig("Left Two Corner Shallow", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, "Left To Shallow", "Left Shallow To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); @@ -517,7 +478,6 @@ public void configureAutons() { AutonConfig RIGHT_FOLLOW = new AutonConfig("Right Follow", RightFollow::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Follow To Bump", "Right Follow To Score"); RIGHT_FOLLOW.register(autonChooser); - // AutonConfig EMPTY_TEST = new AutonConfig("Empty Test", EmptyTest::new, prevWaitTimeOne, prevWaitTimeTwo, // "Right Trench Score To Corner"); // EMPTY_TEST.register(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewThree.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java similarity index 86% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewThree.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java index 499c6f13..824ac760 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewThree.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java @@ -7,7 +7,6 @@ import java.util.Set; -import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.commands.handoff.HandoffRun; @@ -29,9 +28,11 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class RightTwoCornerBCNewThree extends SequentialCommandGroup { - //this one removes pathfinder + optimized timeouts, and removes is hopper empty, with timeout on group - public RightTwoCornerBCNewThree(PathPlannerPath... paths) { +public class CenterTwoCornerBC extends SequentialCommandGroup { + //this one puts a timeout only on parallel command + //optimizations, remove wait commands + + public CenterTwoCornerBC(PathPlannerPath... paths) { addCommands( @@ -48,13 +49,14 @@ public RightTwoCornerBCNewThree(PathPlannerPath... paths) { new SuperstructureAutoInterpolation() ), new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), - new IntakeAutoDigest().withTimeout(2) - ).withTimeout(2.0), //update to 3.0 for actual, this just ensures pathfinding occurs + new IntakeAutoDigest() + ).withTimeout(1.5), //update to 3.0 for actual, this just ensures pathfinding occurs new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating // NZ Trip 2 @@ -70,12 +72,14 @@ public RightTwoCornerBCNewThree(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), - new IntakeAutoDigest().withTimeout(2) - // new WaitCommand(1.0) - ).withTimeout(2.0), + new IntakeAutoDigest(), + new WaitCommand(5) + ), //removed time out - run to the max new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), + new SwerveXMode() ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java deleted file mode 100644 index 7cb1aba5..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBC.java +++ /dev/null @@ -1,88 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; - -import java.util.Set; - -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; -import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class LeftTwoCornerBC extends SequentialCommandGroup { - - public LeftTwoCornerBC(PathPlannerPath... paths) { - - addCommands( - - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - - Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.2).andThen(new IntakeDeploy()) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //changed to 4 bcs of delay (from 5) - // new WaitCommand(1.0).andThen( - // new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) - ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - // NZ Trip 2 - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[3], PathConstraints.unlimitedConstraints(12)), - new HandoffStop(), - new SpindexerStop() - ), - - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down - // new WaitCommand(1.0) - ).withTimeout(1.0), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure not in SOTM and digestion works - - CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), - new SwerveXMode() - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java deleted file mode 100644 index a6d1c533..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerBCCenter.java +++ /dev/null @@ -1,89 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; - -import java.util.Set; - -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; -import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class LeftTwoCornerBCCenter extends SequentialCommandGroup { - - public LeftTwoCornerBCCenter(PathPlannerPath... paths) { - - addCommands( - - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - - Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.2).andThen(new IntakeDeploy()) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //changed to 4 bcs of delay (from 5) - // new WaitCommand(1.0).andThen( - // new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) - ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - // NZ Trip 2 - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[3], PathConstraints.unlimitedConstraints(12)), - new HandoffStop(), - new SpindexerStop() - ), - - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down - // new WaitCommand(1.0) - ).withTimeout(1.0), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure not in SOTM and digestion works - - CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), - new SwerveXMode() - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java deleted file mode 100644 index 77771e0b..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBC.java +++ /dev/null @@ -1,88 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; - -import java.util.Set; - -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; -import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class RightTwoCornerBC extends SequentialCommandGroup { - - public RightTwoCornerBC(PathPlannerPath... paths) { - - addCommands( - - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - - Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.2).andThen(new IntakeDeploy()) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).withTimeout(1.0), //uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(2.0)), //changed to 4 bcs of delay (from 5) - new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(2.5)) - ).withTimeout(2.0), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - // NZ Trip 2 - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[3], PathConstraints.unlimitedConstraints(12)), - new HandoffStop(), - new SpindexerStop() - ), - - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).withTimeout(1.0), //uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.0)) //cut this down - // new WaitCommand(1.0) - ).withTimeout(1.0), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure not in SOTM and digestion works - - CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), - new SwerveXMode() - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java deleted file mode 100644 index 632e4a4f..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCCenter.java +++ /dev/null @@ -1,86 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; - -import java.util.Set; - -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; -import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class RightTwoCornerBCCenter extends SequentialCommandGroup { - - public RightTwoCornerBCCenter(PathPlannerPath... paths) { - - addCommands( - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - - Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.2).andThen(new IntakeDeploy()) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //changed to 4 bcs of delay (from 5) - // new WaitCommand(1.0).andThen( - // new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) - ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), - // NZ Trip 2 - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[3], PathConstraints.unlimitedConstraints(12)), - new HandoffStop(), - new SpindexerStop() - ), - - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) //cut this down - // new WaitCommand(1.0) - ).withTimeout(1.0), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), - - CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[4], PathConstraints.unlimitedConstraints(12)), - new SwerveXMode() - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewCenter.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewCenter.java deleted file mode 100644 index 8dffaa98..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewCenter.java +++ /dev/null @@ -1,91 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; - -import java.util.Set; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; -import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class RightTwoCornerBCNewCenter extends SequentialCommandGroup { - //this one puts a timeout only on parallel command - //optimizations, remove wait commands - - public RightTwoCornerBCNewCenter(PathPlannerPath... paths) { - - addCommands( - - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - - Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.2).andThen(new IntakeDeploy()) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty())), //changed to 4 bcs of delay (from 5) - new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty())) - ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - // NZ Trip 2 - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffStop(), - new SpindexerStop() - ), - - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty())), //changed to 4 bcs of delay (from 5) - new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty())) - ).withTimeout(1.0), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), - - new SwerveXMode() - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFive.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFive.java deleted file mode 100644 index 81a41d38..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFive.java +++ /dev/null @@ -1,82 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; - -import java.util.Set; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; -import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class RightTwoCornerBCNewFive extends SequentialCommandGroup { - //this one removes pathfinder + optimized timeouts, and removes is hopper empty, and puts timeout only on digestion - public RightTwoCornerBCNewFive(PathPlannerPath... paths) { - - addCommands( - - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - - Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.2).andThen(new IntakeDeploy()) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new IntakeAutoDigest().withTimeout(3) - ), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - // NZ Trip 2 - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffStop(), - new SpindexerStop() - ), - - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new IntakeAutoDigest().withTimeout(3) - ), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), - new SwerveXMode() - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFour.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFour.java deleted file mode 100644 index 2dd43f23..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewFour.java +++ /dev/null @@ -1,83 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; - -import java.util.Set; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; -import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class RightTwoCornerBCNewFour extends SequentialCommandGroup { - //this one removes pathfinder, and removes is hopper empty, and puts timeout only on group, not even on digestion - public RightTwoCornerBCNewFour(PathPlannerPath... paths) { - - addCommands( - - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - - Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.2).andThen(new IntakeDeploy()) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new IntakeAutoDigest() - ).withTimeout(2.0), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - // NZ Trip 2 - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffStop(), - new SpindexerStop() - ), - - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new IntakeAutoDigest() - ).withTimeout(2.0), - - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), - new SwerveXMode() - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewSix.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewSix.java deleted file mode 100644 index 9ce88dc4..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewSix.java +++ /dev/null @@ -1,84 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; - -import java.util.Set; - -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; -import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class RightTwoCornerBCNewSix extends SequentialCommandGroup { - //this one removes pathfinder, removes is hopper empty, and puts timeout only on the path - public RightTwoCornerBCNewSix(PathPlannerPath... paths) { - - addCommands( - - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - - Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.2).andThen(new IntakeDeploy()) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).withTimeout(2.0),// uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new IntakeAutoDigest() - ), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - // NZ Trip 2 - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffStop(), - new SpindexerStop() - ), - - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).withTimeout(1.0), //uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new IntakeAutoDigest().withTimeout(2) - // new WaitCommand(1.0) - ).withTimeout(2.0), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), - new SwerveXMode() - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewTwo.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewTwo.java deleted file mode 100644 index f623760b..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNewTwo.java +++ /dev/null @@ -1,93 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; - -import java.util.Set; - -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; -import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class RightTwoCornerBCNewTwo extends SequentialCommandGroup { - - public RightTwoCornerBCNewTwo(PathPlannerPath... paths) { - //SKETCHY - //this one removes pathfinder + optimized timeouts - - addCommands( - - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - - Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.2).andThen(new IntakeDeploy()) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.5)), //changed to 4 bcs of delay (from 5) - new WaitCommand(0.5).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.5)) - ), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - // NZ Trip 2 - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffStop(), - new SpindexerStop() - ), - - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.5)), //changed to 4 bcs of delay (from 5) - new WaitCommand(0.5).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(1.0)) - ), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), - - new SwerveXMode() - //TODO: not accounting for the first loop!! - in terms of paths!!! - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNew.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java similarity index 92% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNew.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java index 72fa5595..4f44dd4d 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerBCNew.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java @@ -28,11 +28,11 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class RightTwoCornerBCNew extends SequentialCommandGroup { +public class TwoCornerBC extends SequentialCommandGroup { //this one puts a timeout only on parallel command //optimizations, remove wait commands - public RightTwoCornerBCNew(PathPlannerPath... paths) { + public TwoCornerBC(PathPlannerPath... paths) { addCommands( @@ -55,7 +55,7 @@ public RightTwoCornerBCNew(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new IntakeAutoDigest() - ).withTimeout(1.25), //update to 3.0 for actual, this just ensures pathfinding occurs + ).withTimeout(1.5), //update to 3.0 for actual, this just ensures pathfinding occurs new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating // NZ Trip 2 @@ -71,8 +71,9 @@ public RightTwoCornerBCNew(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues new HandoffRun(), new SpindexerRun(), - new IntakeAutoDigest() - ).withTimeout(4.2), + new IntakeAutoDigest(), + new WaitCommand(5) + ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), From ba76e776c7b4c60c140d4f9780992d074ed5a478 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Thu, 28 May 2026 21:12:46 -0400 Subject: [PATCH 57/97] feat: left autons. Double checked all decimals/points. --- .../paths/BC Left Bite Score To Score.path | 145 ++++++++++++++++++ .../paths/BC Left NZ To Score.path | 4 +- .../paths/BC Right NZ To Score.path | 4 +- .../paths/Left Corner To Center Dot pt1.path | 54 +++++++ .../paths/Left Corner To Center Dot pt2.path | 54 +++++++ ...o Dot XXX.path => Left Corner To Dot.path} | 16 +- .../paths/Right Corner To Center Dot pt2.path | 2 +- .../paths/Right Corner To Dot.path | 4 +- .../com/stuypulse/robot/RobotContainer.java | 39 +++-- .../auton/regular/CenterTwoCornerBC.java | 14 +- .../commands/auton/regular/TwoCornerBC.java | 14 +- 11 files changed, 297 insertions(+), 53 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/BC Left Bite Score To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt1.path create mode 100644 src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt2.path rename src/main/deploy/pathplanner/paths/{Left Corner To Dot XXX.path => Left Corner To Dot.path} (81%) diff --git a/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score.path b/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score.path new file mode 100644 index 00000000..915897b5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.57, + "y": 7.42 + }, + "prevControl": null, + "nextControl": { + "x": 7.8268188302425035, + "y": 7.488123468654376 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.7265763195435095, + "y": 5.904636233951498 + }, + "prevControl": { + "x": 7.687760342368046, + "y": 7.884251069900142 + }, + "nextControl": { + "x": 7.76360928403665, + "y": 4.015955044801327 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.794992867332382, + "y": 3.782696148359487 + }, + "prevControl": { + "x": 7.6166741233975355, + "y": 3.782696148359487 + }, + "nextControl": { + "x": 5.966918687589157, + "y": 3.782696148359487 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.109243937232525, + "y": 6.059900142653353 + }, + "prevControl": { + "x": 6.143981742457801, + "y": 4.624070860008561 + }, + "nextControl": { + "x": 6.070427960057062, + "y": 7.664293865905849 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.651, + "y": 7.441 + }, + "prevControl": { + "x": 6.341589561259271, + "y": 7.591629770992365 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.307036247334758, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.8272921108741973, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.2366737739872133, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.7421203438395327, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.6183368869935886, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.0533049040511733, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.3176972281449895, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8835202761000936, + "maxWaypointRelativePos": 3.057808455565133, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path b/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path index 0b448fbd..663980ce 100644 --- a/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 3.6120827389443653, + "x": 3.65, "y": 7.441 }, "prevControl": { - "x": 5.9215888888888895, + "x": 5.959506149944524, "y": 7.416322222222221 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path b/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path index 5fd59ceb..71b53033 100644 --- a/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 3.6120827389443653, + "x": 3.612, "y": 0.559 }, "prevControl": { - "x": 6.028777777777779, + "x": 6.028695038833414, "y": 0.5367444444444436 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt1.path b/src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt1.path new file mode 100644 index 00000000..98a93f05 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.31, + "y": 7.44 + }, + "prevControl": null, + "nextControl": { + "x": 3.5785777777777783, + "y": 7.4359777777777785 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.27, + "y": 7.44 + }, + "prevControl": { + "x": 6.02, + "y": 7.44 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.8, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt2.path b/src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt2.path new file mode 100644 index 00000000..caaf09af --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.27, + "y": 7.44 + }, + "prevControl": null, + "nextControl": { + "x": 6.436401056802655, + "y": 7.227016359757356 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.289, + "y": 4.064233333333333 + }, + "prevControl": { + "x": 8.135084631168585, + "y": 4.261236021735014 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Dot XXX.path b/src/main/deploy/pathplanner/paths/Left Corner To Dot.path similarity index 81% rename from src/main/deploy/pathplanner/paths/Left Corner To Dot XXX.path rename to src/main/deploy/pathplanner/paths/Left Corner To Dot.path index 479fa787..a508d0c2 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner To Dot XXX.path +++ b/src/main/deploy/pathplanner/paths/Left Corner To Dot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.52, - "y": 7.42 + "x": 3.31, + "y": 7.44 }, "prevControl": null, "nextControl": { - "x": 5.045616333629863, - "y": 7.427897901816431 + "x": 4.835616333629863, + "y": 7.4478979018164315 }, "isLocked": false, "linkedName": null @@ -36,10 +36,6 @@ { "waypointRelativePos": 0.36281588447653357, "rotationDegrees": -2.0 - }, - { - "waypointRelativePos": 0.7914438502673795, - "rotationDegrees": 180.0 } ], "constraintZones": [], @@ -51,7 +47,7 @@ "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -63,5 +59,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt2.path b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt2.path index e2c90078..07002c83 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt2.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt2.path @@ -37,7 +37,7 @@ "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, + "nominalVoltage": 12.0, "unlimited": true }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path index 4bb7223d..d37e996b 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path @@ -47,7 +47,7 @@ "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -59,5 +59,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 2b452b23..98dcbeab 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -429,31 +429,11 @@ public void configureAutons() { AutonConfig LEFT_TWO_CORNER = new AutonConfig("Left Two Corner", LeftTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); LEFT_TWO_CORNER.register(autonChooser); - - //CHANGE THESE PATHS !! - AutonConfig BC_LEFT_TWO_CORNER = new AutonConfig("BC Left Two Corner", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "Left Score To Score", "Left Corner To Dot v3"); - BC_LEFT_TWO_CORNER.register(autonChooser); - - AutonConfig CENTER_BC_LEFT_TWO_CORNER = new AutonConfig("BC Center Left Two Corner", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "Left Score To Score", "Left Corner To Center Dot"); - CENTER_BC_LEFT_TWO_CORNER.register(autonChooser); - // ***** AutonConfig RIGHT_TWO_CORNER = new AutonConfig("Right Two Corner", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CORNER.register(autonChooser); - AutonConfig BC_RIGHT_TWO_CORNER = new AutonConfig("BC Right Two Corner", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); - BC_RIGHT_TWO_CORNER.register(autonChooser); - - //NEW TWO IS TO CENTER DOT AS OF RIGHT NOW - //Update other paths on pathplanner - AutonConfig BC_CENTER_RIGHT_TWO_CORNER = new AutonConfig("Center BC Right Two Corner", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); - BC_CENTER_RIGHT_TWO_CORNER.register(autonChooser); - AutonConfig LEFT_TWO_CORNER_SHALLOW = new AutonConfig("Left Two Corner Shallow", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, "Left To Shallow", "Left Shallow To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); LEFT_TWO_CORNER_SHALLOW.register(autonChooser); @@ -469,6 +449,25 @@ public void configureAutons() { AutonConfig RIGHT_TWO_CORNER_VARIANT = new AutonConfig("Right Two Corner Variant", RightTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner Bite", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CORNER_VARIANT.register(autonChooser); + + //BC RIGHT + AutonConfig BC_RIGHT_TWO_CORNER = new AutonConfig("BC Right Two Corner", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); + BC_RIGHT_TWO_CORNER.register(autonChooser); + + AutonConfig BC_CENTER_RIGHT_TWO_CORNER = new AutonConfig("Center BC Right Two Corner", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); + BC_CENTER_RIGHT_TWO_CORNER.register(autonChooser); + + //BC LEFT + //TODO: check for no nulls/typos in strings + AutonConfig BC_LEFT_TWO_CORNER = new AutonConfig("BC Left Two Corner", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Dot"); + BC_LEFT_TWO_CORNER.register(autonChooser); + + AutonConfig CENTER_BC_LEFT_TWO_CORNER = new AutonConfig("BC Center Left Two Corner", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); + CENTER_BC_LEFT_TWO_CORNER.register(autonChooser); // FOLLOWS AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java index 824ac760..88e18fcb 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java @@ -29,8 +29,6 @@ import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class CenterTwoCornerBC extends SequentialCommandGroup { - //this one puts a timeout only on parallel command - //optimizations, remove wait commands public CenterTwoCornerBC(PathPlannerPath... paths) { @@ -52,12 +50,12 @@ public CenterTwoCornerBC(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), new HandoffRun(), new SpindexerRun(), new IntakeAutoDigest() - ).withTimeout(1.5), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + ).withTimeout(1.5), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 new ParallelCommandGroup( @@ -69,13 +67,13 @@ public CenterTwoCornerBC(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), new HandoffRun(), new SpindexerRun(), new IntakeAutoDigest(), new WaitCommand(5) - ), //removed time out - run to the max - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure SOTM is over CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java index 4f44dd4d..2c60bd45 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java @@ -29,9 +29,7 @@ import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class TwoCornerBC extends SequentialCommandGroup { - //this one puts a timeout only on parallel command - //optimizations, remove wait commands - + public TwoCornerBC(PathPlannerPath... paths) { addCommands( @@ -51,12 +49,12 @@ public TwoCornerBC(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), new HandoffRun(), new SpindexerRun(), new IntakeAutoDigest() - ).withTimeout(1.5), //update to 3.0 for actual, this just ensures pathfinding occurs - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + ).withTimeout(1.5), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 new ParallelCommandGroup( @@ -68,13 +66,13 @@ public TwoCornerBC(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),//.withTimeout(1.0), uncomment if we have issues + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), new HandoffRun(), new SpindexerRun(), new IntakeAutoDigest(), new WaitCommand(5) ), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //still have to shoot so we go back to interpolating + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure SOTM is over CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), From 32a5436927f2baf2a3c58b254b9dfa03a752478c Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Thu, 28 May 2026 21:59:19 -0400 Subject: [PATCH 58/97] feat: cleaned up LED code. Cleaned up LimelightVision and added methods to Cameras as a result. Removed existing and incomplete check for distance from last pose when seeing April tags. --- .../stuypulse/robot/constants/Cameras.java | 35 +++- .../robot/subsystems/leds/LEDController.java | 152 ++++++++---------- .../subsystems/vision/LimelightVision.java | 66 +------- 3 files changed, 103 insertions(+), 150 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index b9668aed..4f99f7fa 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -7,6 +7,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.robot.util.vision.LimelightHelpers.LimelightResults; import com.stuypulse.robot.util.vision.LimelightHelpers.RawFiducial; @@ -45,14 +46,14 @@ public static class Camera { private SmartBoolean isEnabled; private String keyName; + private double LLHeartbeat = -1; + private int loopCounter = 0; + private int rejectedCounterNotNull; private int rejectedCounterAngularVelocity; private int rejectedCounterInvalidPosition; private int rejectedCounterTargetArea; - // private boolean isDead; - // private double heartBeat; - private LimelightResults result; private Pipeline currentPipeline; @@ -124,6 +125,34 @@ public int getNumberOfTagsSeen() { return LimelightHelpers.getRawFiducials(this.getName()).length; } + public void updateHeartBeat() { + LLHeartbeat = LimelightHelpers.getHeartbeat(this.getName()); + } + + public void incrementLoopCounter() { + loopCounter += 1; + } + + public boolean isAlive() { + //latest - old heartbeat + if (LimelightHelpers.getHeartbeat(this.getName()) - LLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && LLHeartbeat != -1) { + return false; + } + else { + return true; + } + } + + public void updateLEDs() { + switch (this.getName()) { + case "limelight-right" -> { if (!isAlive()) LEDController.isRightLLDead = true; } + + case "limelight-left" -> { if (!isAlive()) LEDController.isLeftLLDead = true; } + + case "limelight-back" -> { if (!isAlive()) LEDController.isBackLLDead = true; } + } + } + // public boolean seesTag() { // return getNumberOfTagsSeen() == 0; // } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index a328ab36..b5214d19 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -6,16 +6,10 @@ package com.stuypulse.robot.subsystems.leds; -import java.util.Optional; - import com.ctre.phoenix6.configs.CANdleConfiguration; import com.ctre.phoenix6.configs.CANdleFeaturesConfigs; -import com.ctre.phoenix6.configs.CustomParamsConfigs; import com.ctre.phoenix6.configs.LEDConfigs; import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.EmptyAnimation; -import com.ctre.phoenix6.controls.RainbowAnimation; -import com.ctre.phoenix6.controls.SingleFadeAnimation; import com.ctre.phoenix6.controls.SolidColor; import com.ctre.phoenix6.hardware.CANdle; import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue; @@ -23,28 +17,25 @@ import com.ctre.phoenix6.signals.StatusLedWhenActiveValue; import com.ctre.phoenix6.signals.StripTypeValue; import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Cameras.Camera; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import dev.doglog.DogLog; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDController extends SubsystemBase { private final static LEDController instance; - public static boolean isLeftLLDead = false; - public static boolean isBackLLDead = false; - public static boolean isRightLLDead = false; - // public static boolean isLeftLLDeadControlApplied; - // public static boolean isBackLLDeadControlApplied; - // public static boolean isRightLLDeadControlApplied; + public static boolean isLeftLLDead; + public static boolean isBackLLDead; + public static boolean isRightLLDead; +; + public boolean leftDeadAnimationCleared; + public boolean backDeadAnimationCleared; + public boolean rightDeadAnimationCleared; - private Pose2d lastPoseOnAprilTag; - private boolean initialPoseUpdated = false; + // private Pose2d lastPoseOnAprilTag; + // private boolean initialPoseUpdated = false; static { instance = new LEDController(); @@ -58,11 +49,34 @@ public static LEDController getInstance() { private CANdleConfiguration candleConfigs; private ControlRequest ledPattern = Settings.LED.solidColorRequest.withColor(Settings.LED.DISABLED); - // different portions of the LED should be a different color to indicate whether - // certain limelights are dead - // add the flashing aspect based on if we don't see a tag (with debounce) - // one way to go further with the flashing aspect is make it flash faster over - // DISTANCE (since last tag was seen) rather than time + private LEDController() { + leftDeadAnimationCleared = false; + backDeadAnimationCleared = false; + rightDeadAnimationCleared = false; + + isLeftLLDead = false; + isBackLLDead = false; + isRightLLDead = false; + + leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); + // lastPoseOnAprilTag = new Pose2d(); + + candleConfigs = new CANdleConfiguration() + .withLED( + new LEDConfigs() + .withBrightnessScalar(1.0) + .withStripType(StripTypeValue.GRB) + .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)) + + .withCANdleFeatures( + new CANdleFeaturesConfigs().withStatusLedWhenActive(StatusLedWhenActiveValue.Enabled)); + + leds.getConfigurator().apply(candleConfigs); + + leds.setControl(ledPattern); + } + + // add the flashing aspect based on if we don't see a tag (with debounce or by distance) public enum LedState { PASSING_TRENCH(Settings.LED.PASSING_TRENCH), @@ -100,23 +114,18 @@ public ControlRequest getAnimation() { private LedState state = LedState.DISABLED; private LedState cachedState = LedState.DISABLED; - // CHANGE apply pattern command to change state - + //TODO: make branch for the distance flashing thing public void applyPattern() { + // if (initialPoseUpdated && + // lastPoseOnAprilTag.getTranslation() + // .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()) > Settings.LED.APRIL_TAG_DISTANCE_THRESHOLD) { + // + // } + if (cachedState != state) { - // if (cachedState.getAnimation().getName() != "SolidColor") { - // leds.clearAllAnimations(); - // } - // clearAllAnimations every loop - // if (!(cachedState.getAnimation().getName().equals(state.getAnimation().getName()))) { - // this.ledPattern = state.getAnimation(); - // } - - // else if (ledPattern instanceof SolidColor) { - SolidColor solidColor = (SolidColor) ledPattern; - solidColor.withColor(state.getColor()); - // SolidColor.class.cast(ledPattern).withColor(null); //change if neccesary - // } + SolidColor solidColor = (SolidColor) ledPattern; + solidColor.withColor(state.getColor()); + cachedState = state; } } @@ -125,31 +134,15 @@ public void changeState(LedState state) { this.state = state; } - private LEDController() { - - // isLeftLLDeadControlApplied= false; - // isBackLLDeadControlApplied= false; - // isRightLLDeadControlApplied = false; - - leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); - lastPoseOnAprilTag = new Pose2d(); - - candleConfigs = new CANdleConfiguration() - .withLED( - new LEDConfigs() - .withBrightnessScalar(1.0) - .withStripType(StripTypeValue.GRB) - .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)) - - .withCANdleFeatures( - new CANdleFeaturesConfigs().withStatusLedWhenActive(StatusLedWhenActiveValue.Enabled)); - - leds.getConfigurator().apply(candleConfigs); - - leds.setControl(ledPattern); - } public void periodicAfterScheduler() { + // if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || + // Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || + // Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { + // lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); + // initialPoseUpdated = true; + // } + if (RobotContainer.EnabledSubsystems.LEDS.get()) { applyPattern(); leds.setControl(ledPattern); @@ -157,47 +150,30 @@ public void periodicAfterScheduler() { leds.clearAllAnimations(); } - // reflective of the 3 LED gap between them + //deadAnimationClear booleans ensure we aren't clearing animations 3 times per loop. if (isRightLLDead) { - //TODO: when it goes back on CLEAR ANIMATIONS !! leds.setControl(Settings.LED.RIGHT_DEAD_STRIP .withColor(Settings.LED.RIGHTDEAD)); - // isRightLLDeadControlApplied = true; - } else if (!isRightLLDead /*&& isRightLLDeadControlApplied*/) { + rightDeadAnimationCleared = false; + } else if (!isRightLLDead && !rightDeadAnimationCleared) { leds.clearAllAnimations(); - //isRightLLDeadControlApplied = false; + rightDeadAnimationCleared = true; } if (isLeftLLDead) { leds.setControl(Settings.LED.LEFT_DEAD_STRIP .withColor(Settings.LED.LEFTDEAD)); - //isLeftLLDeadControlApplied = true; - } else if (!isLeftLLDead /*&& isLeftLLDeadControlApplied */) { + leftDeadAnimationCleared = false; + } else if (!isLeftLLDead && !leftDeadAnimationCleared) { leds.clearAllAnimations(); - //isLeftLLDeadControlApplied = false; + leftDeadAnimationCleared = true; } if (isBackLLDead) { leds.setControl(Settings.LED.BACK_DEAD_STRIP .withColor(Settings.LED.BACKDEAD)); - //isBackLLDeadControlApplied = true; - } else if (!isBackLLDead /*&& isBackLLDeadControlApplied*/) { + backDeadAnimationCleared = false; + } else if (!isBackLLDead && !backDeadAnimationCleared) { leds.clearAllAnimations(); - //isBackLLDeadControlApplied = false; - } - - if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || - Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || - Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { - lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); - initialPoseUpdated = true; - } - else { - - } - - if (initialPoseUpdated && - lastPoseOnAprilTag.getTranslation() - .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()) > Settings.LED.APRIL_TAG_DISTANCE_THRESHOLD) { - //TODO: add flashing + backDeadAnimationCleared = true; } DogLog.log("LED/Applied Pattern Name", ledPattern.getName()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 63d31f75..68e3dfb4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -50,14 +50,6 @@ public static LimelightVision getInstance() { private int maxTagCount; private MegaTagMode megaTagMode; - private double leftLLHeartbeat = -1; //change to -1 is we need to switch the way we do this - private double rightLLHeartbeat = -1; - private double backLLHeartbeat = -1; - - private int leftLoopCounter = 0; - private int rightLoopCounter = 0; - private int backLoopCounter = 0; - private Pose2d[] limelightPoseArray; // private StructPublisher leftLimelightPosePublisher; @@ -232,59 +224,15 @@ public void periodicAfterScheduler() { DogLog.log("LED/heartbeat" + limelightName, LimelightHelpers.getHeartbeat(limelightName)); - if (limelightName.equals(Cameras.LimelightCameras[0].getName())) { - DogLog.log("LED/Right Loop Counter", rightLoopCounter); - DogLog.log("LED/variable heartbeat " + limelightName, rightLLHeartbeat); - rightLoopCounter += 1; - if (rightLoopCounter == 50) { - DogLog.log("LED/Right Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat); - if (LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1) { - LEDController.isRightLLDead = true; - } - else { - LEDController.isRightLLDead = false; - } - rightLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); - rightLoopCounter = 0; - } - } - if (limelightName.equals(Cameras.LimelightCameras[1].getName())) { - DogLog.log("LED/Left Loop Counter", leftLoopCounter); - DogLog.log("LED/variable heartbeat " + limelightName, leftLLHeartbeat); - leftLoopCounter += 1; - if (leftLoopCounter == 50) { - DogLog.log("LED/Left Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat); - if (LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1) { - LEDController.isLeftLLDead = true; - } - else { - LEDController.isLeftLLDead = false; - } - leftLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); - leftLoopCounter = 0; - } - } - if (limelightName.equals(Cameras.LimelightCameras[2].getName())) { - DogLog.log("LED/Back Loop Counter", backLoopCounter); - DogLog.log("LED/variable heartbeat " + limelightName, backLLHeartbeat); - backLoopCounter += 1; - if (backLoopCounter == 50) { - DogLog.log("LED/Back Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat); - if (LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && backLLHeartbeat != -1) { - LEDController.isBackLLDead = true; - } - else { - LEDController.isBackLLDead = false; - } - backLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); - backLoopCounter = 0; - } - } - + Cameras.LimelightCameras[i].updateLEDs(); + + //Ensure this is below updateLEDs(), otherwise the cameras will never appear as dead + Cameras.LimelightCameras[i].updateHeartBeat(); + Cameras.LimelightCameras[i].incrementLoopCounter(); - // Seed robot heading (used by MT2) - LimelightHelpers.SetRobotOrientation( + // Seed robot heading (used by MT2) + LimelightHelpers.SetRobotOrientation( limelightName, (CommandSwerveDrivetrain.getInstance().getPose().getRotation().getDegrees() + (Robot.isBlue() ? 0 : 180)) % 360, 0, From f62437a31ce17fd9f2e9c9b21e203be71e2d3d6c Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 29 May 2026 18:52:19 -0400 Subject: [PATCH 59/97] Revert "feat: cleaned up LED code. Cleaned up LimelightVision and added methods to Cameras as a result. Removed existing and incomplete check for distance from last pose when seeing April tags." This reverts commit 32a5436927f2baf2a3c58b254b9dfa03a752478c. --- .../stuypulse/robot/constants/Cameras.java | 35 +--- .../robot/subsystems/leds/LEDController.java | 152 ++++++++++-------- .../subsystems/vision/LimelightVision.java | 66 +++++++- 3 files changed, 150 insertions(+), 103 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 4f99f7fa..b9668aed 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -7,7 +7,6 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.robot.util.vision.LimelightHelpers.LimelightResults; import com.stuypulse.robot.util.vision.LimelightHelpers.RawFiducial; @@ -46,14 +45,14 @@ public static class Camera { private SmartBoolean isEnabled; private String keyName; - private double LLHeartbeat = -1; - private int loopCounter = 0; - private int rejectedCounterNotNull; private int rejectedCounterAngularVelocity; private int rejectedCounterInvalidPosition; private int rejectedCounterTargetArea; + // private boolean isDead; + // private double heartBeat; + private LimelightResults result; private Pipeline currentPipeline; @@ -125,34 +124,6 @@ public int getNumberOfTagsSeen() { return LimelightHelpers.getRawFiducials(this.getName()).length; } - public void updateHeartBeat() { - LLHeartbeat = LimelightHelpers.getHeartbeat(this.getName()); - } - - public void incrementLoopCounter() { - loopCounter += 1; - } - - public boolean isAlive() { - //latest - old heartbeat - if (LimelightHelpers.getHeartbeat(this.getName()) - LLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && LLHeartbeat != -1) { - return false; - } - else { - return true; - } - } - - public void updateLEDs() { - switch (this.getName()) { - case "limelight-right" -> { if (!isAlive()) LEDController.isRightLLDead = true; } - - case "limelight-left" -> { if (!isAlive()) LEDController.isLeftLLDead = true; } - - case "limelight-back" -> { if (!isAlive()) LEDController.isBackLLDead = true; } - } - } - // public boolean seesTag() { // return getNumberOfTagsSeen() == 0; // } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index b5214d19..a328ab36 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -6,10 +6,16 @@ package com.stuypulse.robot.subsystems.leds; +import java.util.Optional; + import com.ctre.phoenix6.configs.CANdleConfiguration; import com.ctre.phoenix6.configs.CANdleFeaturesConfigs; +import com.ctre.phoenix6.configs.CustomParamsConfigs; import com.ctre.phoenix6.configs.LEDConfigs; import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.EmptyAnimation; +import com.ctre.phoenix6.controls.RainbowAnimation; +import com.ctre.phoenix6.controls.SingleFadeAnimation; import com.ctre.phoenix6.controls.SolidColor; import com.ctre.phoenix6.hardware.CANdle; import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue; @@ -17,25 +23,28 @@ import com.ctre.phoenix6.signals.StatusLedWhenActiveValue; import com.ctre.phoenix6.signals.StripTypeValue; import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Cameras.Camera; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import dev.doglog.DogLog; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDController extends SubsystemBase { private final static LEDController instance; - public static boolean isLeftLLDead; - public static boolean isBackLLDead; - public static boolean isRightLLDead; -; - public boolean leftDeadAnimationCleared; - public boolean backDeadAnimationCleared; - public boolean rightDeadAnimationCleared; + public static boolean isLeftLLDead = false; + public static boolean isBackLLDead = false; + public static boolean isRightLLDead = false; + // public static boolean isLeftLLDeadControlApplied; + // public static boolean isBackLLDeadControlApplied; + // public static boolean isRightLLDeadControlApplied; - // private Pose2d lastPoseOnAprilTag; - // private boolean initialPoseUpdated = false; + private Pose2d lastPoseOnAprilTag; + private boolean initialPoseUpdated = false; static { instance = new LEDController(); @@ -49,34 +58,11 @@ public static LEDController getInstance() { private CANdleConfiguration candleConfigs; private ControlRequest ledPattern = Settings.LED.solidColorRequest.withColor(Settings.LED.DISABLED); - private LEDController() { - leftDeadAnimationCleared = false; - backDeadAnimationCleared = false; - rightDeadAnimationCleared = false; - - isLeftLLDead = false; - isBackLLDead = false; - isRightLLDead = false; - - leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); - // lastPoseOnAprilTag = new Pose2d(); - - candleConfigs = new CANdleConfiguration() - .withLED( - new LEDConfigs() - .withBrightnessScalar(1.0) - .withStripType(StripTypeValue.GRB) - .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)) - - .withCANdleFeatures( - new CANdleFeaturesConfigs().withStatusLedWhenActive(StatusLedWhenActiveValue.Enabled)); - - leds.getConfigurator().apply(candleConfigs); - - leds.setControl(ledPattern); - } - - // add the flashing aspect based on if we don't see a tag (with debounce or by distance) + // different portions of the LED should be a different color to indicate whether + // certain limelights are dead + // add the flashing aspect based on if we don't see a tag (with debounce) + // one way to go further with the flashing aspect is make it flash faster over + // DISTANCE (since last tag was seen) rather than time public enum LedState { PASSING_TRENCH(Settings.LED.PASSING_TRENCH), @@ -114,18 +100,23 @@ public ControlRequest getAnimation() { private LedState state = LedState.DISABLED; private LedState cachedState = LedState.DISABLED; - //TODO: make branch for the distance flashing thing - public void applyPattern() { - // if (initialPoseUpdated && - // lastPoseOnAprilTag.getTranslation() - // .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()) > Settings.LED.APRIL_TAG_DISTANCE_THRESHOLD) { - // - // } + // CHANGE apply pattern command to change state + public void applyPattern() { if (cachedState != state) { - SolidColor solidColor = (SolidColor) ledPattern; - solidColor.withColor(state.getColor()); - + // if (cachedState.getAnimation().getName() != "SolidColor") { + // leds.clearAllAnimations(); + // } + // clearAllAnimations every loop + // if (!(cachedState.getAnimation().getName().equals(state.getAnimation().getName()))) { + // this.ledPattern = state.getAnimation(); + // } + + // else if (ledPattern instanceof SolidColor) { + SolidColor solidColor = (SolidColor) ledPattern; + solidColor.withColor(state.getColor()); + // SolidColor.class.cast(ledPattern).withColor(null); //change if neccesary + // } cachedState = state; } } @@ -134,15 +125,31 @@ public void changeState(LedState state) { this.state = state; } + private LEDController() { - public void periodicAfterScheduler() { - // if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || - // Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || - // Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { - // lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); - // initialPoseUpdated = true; - // } + // isLeftLLDeadControlApplied= false; + // isBackLLDeadControlApplied= false; + // isRightLLDeadControlApplied = false; + + leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); + lastPoseOnAprilTag = new Pose2d(); + + candleConfigs = new CANdleConfiguration() + .withLED( + new LEDConfigs() + .withBrightnessScalar(1.0) + .withStripType(StripTypeValue.GRB) + .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)) + + .withCANdleFeatures( + new CANdleFeaturesConfigs().withStatusLedWhenActive(StatusLedWhenActiveValue.Enabled)); + + leds.getConfigurator().apply(candleConfigs); + + leds.setControl(ledPattern); + } + public void periodicAfterScheduler() { if (RobotContainer.EnabledSubsystems.LEDS.get()) { applyPattern(); leds.setControl(ledPattern); @@ -150,30 +157,47 @@ public void periodicAfterScheduler() { leds.clearAllAnimations(); } - //deadAnimationClear booleans ensure we aren't clearing animations 3 times per loop. + // reflective of the 3 LED gap between them if (isRightLLDead) { + //TODO: when it goes back on CLEAR ANIMATIONS !! leds.setControl(Settings.LED.RIGHT_DEAD_STRIP .withColor(Settings.LED.RIGHTDEAD)); - rightDeadAnimationCleared = false; - } else if (!isRightLLDead && !rightDeadAnimationCleared) { + // isRightLLDeadControlApplied = true; + } else if (!isRightLLDead /*&& isRightLLDeadControlApplied*/) { leds.clearAllAnimations(); - rightDeadAnimationCleared = true; + //isRightLLDeadControlApplied = false; } if (isLeftLLDead) { leds.setControl(Settings.LED.LEFT_DEAD_STRIP .withColor(Settings.LED.LEFTDEAD)); - leftDeadAnimationCleared = false; - } else if (!isLeftLLDead && !leftDeadAnimationCleared) { + //isLeftLLDeadControlApplied = true; + } else if (!isLeftLLDead /*&& isLeftLLDeadControlApplied */) { leds.clearAllAnimations(); - leftDeadAnimationCleared = true; + //isLeftLLDeadControlApplied = false; } if (isBackLLDead) { leds.setControl(Settings.LED.BACK_DEAD_STRIP .withColor(Settings.LED.BACKDEAD)); - backDeadAnimationCleared = false; - } else if (!isBackLLDead && !backDeadAnimationCleared) { + //isBackLLDeadControlApplied = true; + } else if (!isBackLLDead /*&& isBackLLDeadControlApplied*/) { leds.clearAllAnimations(); - backDeadAnimationCleared = true; + //isBackLLDeadControlApplied = false; + } + + if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || + Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || + Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { + lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); + initialPoseUpdated = true; + } + else { + + } + + if (initialPoseUpdated && + lastPoseOnAprilTag.getTranslation() + .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()) > Settings.LED.APRIL_TAG_DISTANCE_THRESHOLD) { + //TODO: add flashing } DogLog.log("LED/Applied Pattern Name", ledPattern.getName()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 68e3dfb4..63d31f75 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -50,6 +50,14 @@ public static LimelightVision getInstance() { private int maxTagCount; private MegaTagMode megaTagMode; + private double leftLLHeartbeat = -1; //change to -1 is we need to switch the way we do this + private double rightLLHeartbeat = -1; + private double backLLHeartbeat = -1; + + private int leftLoopCounter = 0; + private int rightLoopCounter = 0; + private int backLoopCounter = 0; + private Pose2d[] limelightPoseArray; // private StructPublisher leftLimelightPosePublisher; @@ -224,15 +232,59 @@ public void periodicAfterScheduler() { DogLog.log("LED/heartbeat" + limelightName, LimelightHelpers.getHeartbeat(limelightName)); - Cameras.LimelightCameras[i].updateLEDs(); - - //Ensure this is below updateLEDs(), otherwise the cameras will never appear as dead - Cameras.LimelightCameras[i].updateHeartBeat(); - Cameras.LimelightCameras[i].incrementLoopCounter(); + if (limelightName.equals(Cameras.LimelightCameras[0].getName())) { + DogLog.log("LED/Right Loop Counter", rightLoopCounter); + DogLog.log("LED/variable heartbeat " + limelightName, rightLLHeartbeat); + rightLoopCounter += 1; + if (rightLoopCounter == 50) { + DogLog.log("LED/Right Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat); + if (LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1) { + LEDController.isRightLLDead = true; + } + else { + LEDController.isRightLLDead = false; + } + rightLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); + rightLoopCounter = 0; + } + } + if (limelightName.equals(Cameras.LimelightCameras[1].getName())) { + DogLog.log("LED/Left Loop Counter", leftLoopCounter); + DogLog.log("LED/variable heartbeat " + limelightName, leftLLHeartbeat); + leftLoopCounter += 1; + if (leftLoopCounter == 50) { + DogLog.log("LED/Left Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat); + if (LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1) { + LEDController.isLeftLLDead = true; + } + else { + LEDController.isLeftLLDead = false; + } + leftLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); + leftLoopCounter = 0; + } + } + if (limelightName.equals(Cameras.LimelightCameras[2].getName())) { + DogLog.log("LED/Back Loop Counter", backLoopCounter); + DogLog.log("LED/variable heartbeat " + limelightName, backLLHeartbeat); + backLoopCounter += 1; + if (backLoopCounter == 50) { + DogLog.log("LED/Back Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat); + if (LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && backLLHeartbeat != -1) { + LEDController.isBackLLDead = true; + } + else { + LEDController.isBackLLDead = false; + } + backLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); + backLoopCounter = 0; + } + } + - // Seed robot heading (used by MT2) - LimelightHelpers.SetRobotOrientation( + // Seed robot heading (used by MT2) + LimelightHelpers.SetRobotOrientation( limelightName, (CommandSwerveDrivetrain.getInstance().getPose().getRotation().getDegrees() + (Robot.isBlue() ? 0 : 180)) % 360, 0, From 40e8527db347956ac8b5b0cffafddc830edd2d52 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 29 May 2026 21:13:11 -0400 Subject: [PATCH 60/97] feat: add different swipe auton, remove automatic stop of shooting --- .../deploy/pathplanner/paths/BC Right Score To Score.path | 6 +++--- src/main/java/com/stuypulse/robot/Robot.java | 1 - src/main/java/com/stuypulse/robot/RobotContainer.java | 4 ++++ 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/BC Right Score To Score.path b/src/main/deploy/pathplanner/paths/BC Right Score To Score.path index 34fed6be..ca06a66d 100644 --- a/src/main/deploy/pathplanner/paths/BC Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/BC Right Score To Score.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.48, + "x": 3.53, "y": 0.57 }, "prevControl": null, "nextControl": { - "x": 7.083082715798332, + "x": 7.133082715798332, "y": 0.5311980499756427 }, "isLocked": false, @@ -52,7 +52,7 @@ "y": 0.559 }, "prevControl": { - "x": 7.6730891295992585, + "x": 7.673089129599258, "y": 0.5209487097696721 }, "nextControl": null, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 85a3dcd6..8ba7106f 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -257,7 +257,6 @@ public void teleopInit() { CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); CommandScheduler.getInstance().schedule(new IntakeDeploy()); - CommandScheduler.getInstance().schedule(new HandoffStop().alongWith(new SpindexerStop())); if (auto != null) { auto.cancel(); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 98dcbeab..7ef05ca8 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -455,6 +455,10 @@ public void configureAutons() { "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); BC_RIGHT_TWO_CORNER.register(autonChooser); + AutonConfig NEW_BC_RIGHT_TWO_CORNER = new AutonConfig("BC Right NEW Two Corner", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot"); + NEW_BC_RIGHT_TWO_CORNER.register(autonChooser); + AutonConfig BC_CENTER_RIGHT_TWO_CORNER = new AutonConfig("Center BC Right Two Corner", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); BC_CENTER_RIGHT_TWO_CORNER.register(autonChooser); From dc4fe7bac604afebd6dd4787d11f1a4c1050d64a Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Sat, 30 May 2026 07:05:40 -0400 Subject: [PATCH 61/97] feat: push everything said to be done in comp-se-log. --- .../paths/BC Left Score To Score.path | 127 ++++++++++++++++++ .../pathplanner/paths/Left Corner To Dot.path | 4 +- .../paths/Right Corner To Dot.path | 14 +- .../com/stuypulse/robot/RobotContainer.java | 57 ++++---- .../stuypulse/robot/constants/Settings.java | 16 +-- .../robot/subsystems/leds/LEDController.java | 24 ++-- 6 files changed, 187 insertions(+), 55 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/BC Left Score To Score.path diff --git a/src/main/deploy/pathplanner/paths/BC Left Score To Score.path b/src/main/deploy/pathplanner/paths/BC Left Score To Score.path new file mode 100644 index 00000000..020e4bc6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left Score To Score.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.57, + "y": 7.42 + }, + "prevControl": null, + "nextControl": { + "x": 6.983047455470738, + "y": 7.5513268447837145 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.931333333333333, + "y": 5.399222222222222 + }, + "prevControl": { + "x": 5.950822222222223, + "y": 7.640444444444444 + }, + "nextControl": { + "x": 5.923225885345372, + "y": 4.466865703606724 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.207855555555556, + "y": 4.9314888888888895 + }, + "prevControl": { + "x": 6.837550478693018, + "y": 4.655234747475271 + }, + "nextControl": { + "x": 7.849570069471864, + "y": 5.410219274052943 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0667047075606275, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 7.3644732636065955, + "y": 7.433121689698743 + }, + "nextControl": { + "x": 5.087089871611983, + "y": 7.49266112478357 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6506870229007635, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 5.306835382387213, + "y": 7.399604078672524 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1891117478510029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.5, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.882729211087425, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Dot.path b/src/main/deploy/pathplanner/paths/Left Corner To Dot.path index a508d0c2..8e3881c6 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner To Dot.path +++ b/src/main/deploy/pathplanner/paths/Left Corner To Dot.path @@ -30,11 +30,11 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.35, + "waypointRelativePos": 0.58, "rotationDegrees": 0.0 }, { - "waypointRelativePos": 0.36281588447653357, + "waypointRelativePos": 0.59, "rotationDegrees": -2.0 } ], diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path index d37e996b..cbfe0b65 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.081243937232525, - "y": 0.6784179743223976 + "x": 4.440424253599775, + "y": 0.56 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 0.559 }, "prevControl": { - "x": 8.007798061746946, - "y": 0.5155879555832673 + "x": 8.003999999999998, + "y": 0.559 }, "nextControl": null, "isLocked": false, @@ -30,12 +30,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.2547717842323645, + "waypointRelativePos": 0.58, "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.75, - "rotationDegrees": 180.0 } ], "constraintZones": [], diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 7ef05ca8..f20085b3 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -426,52 +426,63 @@ public void configureAutons() { BC_TEST.register(autonChooser); // TWO CYCLES (CORNER) - AutonConfig LEFT_TWO_CORNER = new AutonConfig("Left Two Corner", LeftTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig LEFT_OUT_OUT = new AutonConfig("Left Out Out", LeftTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); - LEFT_TWO_CORNER.register(autonChooser); + LEFT_OUT_OUT.register(autonChooser); - AutonConfig RIGHT_TWO_CORNER = new AutonConfig("Right Two Corner", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig RIGHT_OUT_OUT = new AutonConfig("Right Out Out", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); - RIGHT_TWO_CORNER.register(autonChooser); + RIGHT_OUT_OUT.register(autonChooser); - AutonConfig LEFT_TWO_CORNER_SHALLOW = new AutonConfig("Left Two Corner Shallow", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig LEFT_OUT_OUT_SHALLOW = new AutonConfig("Left Out Out Shallow", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, "Left To Shallow", "Left Shallow To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); - LEFT_TWO_CORNER_SHALLOW.register(autonChooser); + LEFT_OUT_OUT_SHALLOW.register(autonChooser); - AutonConfig RIGHT_TWO_CORNER_SHALLOW = new AutonConfig("Right Two Corner Shallow", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig RIGHT_OUT_OUT_SHALLOW = new AutonConfig("Right Out Out Shallow", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, "Right To Shallow", "Right Shallow To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); - RIGHT_TWO_CORNER_SHALLOW.register(autonChooser); + RIGHT_OUT_OUT_SHALLOW.register(autonChooser); - AutonConfig LEFT_TWO_CORNER_VARIANT = new AutonConfig("Left Two Corner Variant", LeftTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig LEFT_OUT_IN = new AutonConfig("Left Out In", LeftTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, "Left Corner Bite", "Left NZ To Score", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); - LEFT_TWO_CORNER_VARIANT.register(autonChooser); + LEFT_OUT_IN.register(autonChooser); - AutonConfig RIGHT_TWO_CORNER_VARIANT = new AutonConfig("Right Two Corner Variant", RightTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig RIGHT_OUT_IN = new AutonConfig("Right Out In", RightTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner Bite", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); - RIGHT_TWO_CORNER_VARIANT.register(autonChooser); + RIGHT_OUT_IN.register(autonChooser); //BC RIGHT - AutonConfig BC_RIGHT_TWO_CORNER = new AutonConfig("BC Right Two Corner", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig RIGHT_OUT_OUT_DOT = new AutonConfig("Right Out Out Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); - BC_RIGHT_TWO_CORNER.register(autonChooser); + RIGHT_OUT_OUT_DOT.register(autonChooser); - AutonConfig NEW_BC_RIGHT_TWO_CORNER = new AutonConfig("BC Right NEW Two Corner", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig RIGHT_OUT_IN_DOT = new AutonConfig("Right Out In Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot"); - NEW_BC_RIGHT_TWO_CORNER.register(autonChooser); + RIGHT_OUT_IN_DOT.register(autonChooser); - AutonConfig BC_CENTER_RIGHT_TWO_CORNER = new AutonConfig("Center BC Right Two Corner", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig RIGHT_OUT_OUT_CENTER_DOT = new AutonConfig("Right Out Out Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); - BC_CENTER_RIGHT_TWO_CORNER.register(autonChooser); + RIGHT_OUT_OUT_CENTER_DOT.register(autonChooser); + + AutonConfig RIGHT_OUT_IN_CENTER_DOT = new AutonConfig("Right Out In Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); + RIGHT_OUT_IN_CENTER_DOT.register(autonChooser); //BC LEFT - //TODO: check for no nulls/typos in strings - AutonConfig BC_LEFT_TWO_CORNER = new AutonConfig("BC Left Two Corner", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig LEFT_OUT_OUT_DOT = new AutonConfig("Left Out Out Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Dot"); - BC_LEFT_TWO_CORNER.register(autonChooser); + LEFT_OUT_OUT_DOT.register(autonChooser); - AutonConfig CENTER_BC_LEFT_TWO_CORNER = new AutonConfig("BC Center Left Two Corner", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig LEFT_OUT_IN_DOT = new AutonConfig("Left Out In Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Dot"); + LEFT_OUT_IN_DOT.register(autonChooser); + + AutonConfig LEFT_OUT_OUT_CENTER_DOT = new AutonConfig("Left Out Out Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); - CENTER_BC_LEFT_TWO_CORNER.register(autonChooser); + LEFT_OUT_OUT_CENTER_DOT.register(autonChooser); + + AutonConfig LEFT_OUT_IN_CENTER_DOT = new AutonConfig("Left Out In Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); + LEFT_OUT_IN_CENTER_DOT.register(autonChooser); // FOLLOWS AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 449d9cab..cf03f60f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -6,7 +6,6 @@ package com.stuypulse.robot.constants; import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.RainbowAnimation; import com.ctre.phoenix6.controls.SolidColor; import com.ctre.phoenix6.signals.RGBWColor; @@ -14,7 +13,6 @@ import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; -import edu.wpi.first.hal.LEDJNI; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; @@ -23,9 +21,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.util.Color; /*- @@ -426,13 +421,12 @@ public static RGBWColor rgbwConverter(Color color) { RGBWColor DISABLED_ALIGNED = rgbwConverter(Color.kGreen); RGBWColor DISABLED = rgbwConverter(Color.kRed); - RGBWColor LEFTDEAD = rgbwConverter(Color.kWhite); - RGBWColor RIGHTDEAD = rgbwConverter(Color.kWhite); - RGBWColor BACKDEAD = rgbwConverter(Color.kWhite); + RGBWColor LLDEAD = rgbwConverter(Color.kWhite); - SolidColor RIGHT_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 4, Settings.LED.LED_LENGTH - 1); - SolidColor BACK_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 11, Settings.LED.LED_LENGTH - 8); - SolidColor LEFT_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 18, Settings.LED.LED_LENGTH - 15); + SolidColor RIGHT_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 6, Settings.LED.LED_LENGTH - 2); + SolidColor BACK_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 13, Settings.LED.LED_LENGTH - 9); + SolidColor LEFT_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 20, Settings.LED.LED_LENGTH - 16); + SolidColor CANDLE_DEAD_STRIP = new SolidColor(0, 7); // RGBWColor.gradient(GradientType.kDiscontinuous, Color.kRed, Color.kWhite).scrollAtRelativeSpeed(Percent.per(Second).of(25)); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index a328ab36..1d7c4e6a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -6,16 +6,10 @@ package com.stuypulse.robot.subsystems.leds; -import java.util.Optional; - import com.ctre.phoenix6.configs.CANdleConfiguration; import com.ctre.phoenix6.configs.CANdleFeaturesConfigs; -import com.ctre.phoenix6.configs.CustomParamsConfigs; import com.ctre.phoenix6.configs.LEDConfigs; import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.EmptyAnimation; -import com.ctre.phoenix6.controls.RainbowAnimation; -import com.ctre.phoenix6.controls.SingleFadeAnimation; import com.ctre.phoenix6.controls.SolidColor; import com.ctre.phoenix6.hardware.CANdle; import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue; @@ -26,7 +20,6 @@ import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Cameras.Camera; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import dev.doglog.DogLog; @@ -161,7 +154,7 @@ public void periodicAfterScheduler() { if (isRightLLDead) { //TODO: when it goes back on CLEAR ANIMATIONS !! leds.setControl(Settings.LED.RIGHT_DEAD_STRIP - .withColor(Settings.LED.RIGHTDEAD)); + .withColor(Settings.LED.LLDEAD)); // isRightLLDeadControlApplied = true; } else if (!isRightLLDead /*&& isRightLLDeadControlApplied*/) { leds.clearAllAnimations(); @@ -169,7 +162,7 @@ public void periodicAfterScheduler() { } if (isLeftLLDead) { leds.setControl(Settings.LED.LEFT_DEAD_STRIP - .withColor(Settings.LED.LEFTDEAD)); + .withColor(Settings.LED.LLDEAD)); //isLeftLLDeadControlApplied = true; } else if (!isLeftLLDead /*&& isLeftLLDeadControlApplied */) { leds.clearAllAnimations(); @@ -177,13 +170,24 @@ public void periodicAfterScheduler() { } if (isBackLLDead) { leds.setControl(Settings.LED.BACK_DEAD_STRIP - .withColor(Settings.LED.BACKDEAD)); + .withColor(Settings.LED.LLDEAD)); //isBackLLDeadControlApplied = true; } else if (!isBackLLDead /*&& isBackLLDeadControlApplied*/) { leds.clearAllAnimations(); //isBackLLDeadControlApplied = false; } + if (isBackLLDead || isLeftLLDead || isRightLLDead) { + leds.setControl(Settings.LED.CANDLE_DEAD_STRIP + .withColor(Settings.LED.LLDEAD)); + //isBackLLDeadControlApplied = true; + } else if (!(isBackLLDead || isLeftLLDead || isRightLLDead) /*&& isBackLLDeadControlApplied*/) { + leds.clearAllAnimations(); + //isBackLLDeadControlApplied = false; + } + + + if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { From 88395d5483936a39bb15eb549e3078d5dbc58290 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 30 May 2026 08:49:12 -0400 Subject: [PATCH 62/97] refactor: (autons) standardize naming convention --- .../com/stuypulse/robot/RobotContainer.java | 58 +++++++++---------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index f20085b3..7d5cbdeb 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -426,63 +426,63 @@ public void configureAutons() { BC_TEST.register(autonChooser); // TWO CYCLES (CORNER) - AutonConfig LEFT_OUT_OUT = new AutonConfig("Left Out Out", LeftTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig L_CN_FN = new AutonConfig("Left Corner-Near Far-Near", LeftTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); - LEFT_OUT_OUT.register(autonChooser); + L_CN_FN.register(autonChooser); - AutonConfig RIGHT_OUT_OUT = new AutonConfig("Right Out Out", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig R_CN_FN = new AutonConfig("Right Corner-Near Far-Near", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); - RIGHT_OUT_OUT.register(autonChooser); + R_CN_FN.register(autonChooser); - AutonConfig LEFT_OUT_OUT_SHALLOW = new AutonConfig("Left Out Out Shallow", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig L_FNS_FN = new AutonConfig("Left Far-Near Shallow Far-Near", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, "Left To Shallow", "Left Shallow To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); - LEFT_OUT_OUT_SHALLOW.register(autonChooser); + L_FNS_FN.register(autonChooser); - AutonConfig RIGHT_OUT_OUT_SHALLOW = new AutonConfig("Right Out Out Shallow", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig R_FNS_FN = new AutonConfig("Right Far-Near Shallow Far-Near", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, "Right To Shallow", "Right Shallow To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); - RIGHT_OUT_OUT_SHALLOW.register(autonChooser); + R_FNS_FN.register(autonChooser); - AutonConfig LEFT_OUT_IN = new AutonConfig("Left Out In", LeftTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig L_CN_NF = new AutonConfig("Left Corner-Near Near-Far", LeftTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, "Left Corner Bite", "Left NZ To Score", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); - LEFT_OUT_IN.register(autonChooser); + L_CN_NF.register(autonChooser); - AutonConfig RIGHT_OUT_IN = new AutonConfig("Right Out In", RightTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig R_CN_NF = new AutonConfig("Right Corner-Near Near-Far", RightTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner Bite", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); - RIGHT_OUT_IN.register(autonChooser); + R_CN_NF.register(autonChooser); //BC RIGHT - AutonConfig RIGHT_OUT_OUT_DOT = new AutonConfig("Right Out Out Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig R_CN_FN_D = new AutonConfig("Right Corner-Near Far-Near Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); - RIGHT_OUT_OUT_DOT.register(autonChooser); + R_CN_FN_D.register(autonChooser); - AutonConfig RIGHT_OUT_IN_DOT = new AutonConfig("Right Out In Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig R_CN_NF_D = new AutonConfig("Right Corner-Near Near-Far Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot"); - RIGHT_OUT_IN_DOT.register(autonChooser); + R_CN_NF_D.register(autonChooser); - AutonConfig RIGHT_OUT_OUT_CENTER_DOT = new AutonConfig("Right Out Out Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig R_CN_FN_CD = new AutonConfig("Right Corner-Near Far-Near Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); - RIGHT_OUT_OUT_CENTER_DOT.register(autonChooser); + R_CN_FN_CD.register(autonChooser); - AutonConfig RIGHT_OUT_IN_CENTER_DOT = new AutonConfig("Right Out In Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); - RIGHT_OUT_IN_CENTER_DOT.register(autonChooser); + AutonConfig R_CN_NF_CD = new AutonConfig("Right Corner-Near Near-Far Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); + R_CN_NF_CD.register(autonChooser); //BC LEFT - AutonConfig LEFT_OUT_OUT_DOT = new AutonConfig("Left Out Out Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig L_CN_FN_D = new AutonConfig("Left Corner-Near Far-Near Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Dot"); - LEFT_OUT_OUT_DOT.register(autonChooser); + L_CN_FN_D.register(autonChooser); - AutonConfig LEFT_OUT_IN_DOT = new AutonConfig("Left Out In Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig L_CN_NF_D = new AutonConfig("Left Corner-Near Near-Far Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Dot"); - LEFT_OUT_IN_DOT.register(autonChooser); + L_CN_NF_D.register(autonChooser); - AutonConfig LEFT_OUT_OUT_CENTER_DOT = new AutonConfig("Left Out Out Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig L_CN_FN_CD = new AutonConfig("Left Corner-Near Far-Near Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); - LEFT_OUT_OUT_CENTER_DOT.register(autonChooser); + L_CN_FN_CD.register(autonChooser); - AutonConfig LEFT_OUT_IN_CENTER_DOT = new AutonConfig("Left Out In Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig L_CN_NF_CD = new AutonConfig("Left Corner-Near Near-Far Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); - LEFT_OUT_IN_CENTER_DOT.register(autonChooser); + L_CN_NF_CD.register(autonChooser); // FOLLOWS AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, From 1ec81a4e783ee4beacc206cd070b1a72e45a497c Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Sat, 30 May 2026 10:13:56 -0400 Subject: [PATCH 63/97] feat: auton changes. Added 2 new autons (4 total). FIXED shoot to corner for the right side (through commits I don't think I changed it, but the Y values were very slightly different, making the shoot to corner last 5 seconds) - and compensated where neccessary. --- .../autos/BC Left Shallow One Cycle Dot.auto | 37 +++++ .../autos/BC Right Shallow One Cycle Dot.auto | 37 +++++ .../autos/Left NY Second Pass.auto | 43 ++++++ .../autos/Right NY Second Pass.auto | 43 ++++++ .../paths/BC Left Score To Score NY.path | 127 ++++++++++++++++++ .../paths/BC Left Shallow To Score.path | 59 ++++++++ .../pathplanner/paths/BC Left To Shallow.path | 81 +++++++++++ .../paths/BC Right Score To Score NY.path | 127 ++++++++++++++++++ .../paths/BC Right Shallow To Score.path | 59 ++++++++ .../paths/BC Right To Shallow.path | 81 +++++++++++ .../paths/Left Bite Score To Score.path | 6 +- .../paths/Left Corner Bite To Score.path | 4 +- .../paths/Left Corner To Dot Straight.path | 54 ++++++++ .../pathplanner/paths/Left NZ To Score.path | 4 +- .../paths/Left Score To Corner.path | 4 +- .../paths/Left Score To Score NY.path | 127 ++++++++++++++++++ .../paths/Left Score To Score.path | 4 +- .../paths/Left Shallow To Score.path | 4 +- .../paths/Right Bite Score To Score.path | 4 +- .../paths/Right Corner To Center Dot.path | 4 +- .../paths/Right Corner To Dot Straight.path | 54 ++++++++ .../pathplanner/paths/Right NZ To Score.path | 4 +- .../paths/Right Score To Corner.path | 4 +- .../paths/Right Score To Score NY.path | 111 +++++++++++++++ .../paths/Right Score To Score.path | 4 +- .../paths/Right Shallow To Score.path | 4 +- .../com/stuypulse/robot/RobotContainer.java | 71 ++++++---- .../auton/regular/ShallowSwipeDot.java | 66 +++++++++ .../handoff/Right Score To Score NY.path | 111 +++++++++++++++ 29 files changed, 1287 insertions(+), 51 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/BC Left Shallow One Cycle Dot.auto create mode 100644 src/main/deploy/pathplanner/autos/BC Right Shallow One Cycle Dot.auto create mode 100644 src/main/deploy/pathplanner/autos/Left NY Second Pass.auto create mode 100644 src/main/deploy/pathplanner/autos/Right NY Second Pass.auto create mode 100644 src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path create mode 100644 src/main/deploy/pathplanner/paths/BC Left Shallow To Score.path create mode 100644 src/main/deploy/pathplanner/paths/BC Left To Shallow.path create mode 100644 src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path create mode 100644 src/main/deploy/pathplanner/paths/BC Right Shallow To Score.path create mode 100644 src/main/deploy/pathplanner/paths/BC Right To Shallow.path create mode 100644 src/main/deploy/pathplanner/paths/Left Corner To Dot Straight.path create mode 100644 src/main/deploy/pathplanner/paths/Left Score To Score NY.path create mode 100644 src/main/deploy/pathplanner/paths/Right Corner To Dot Straight.path create mode 100644 src/main/deploy/pathplanner/paths/Right Score To Score NY.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/ShallowSwipeDot.java create mode 100644 src/main/java/com/stuypulse/robot/commands/handoff/Right Score To Score NY.path diff --git a/src/main/deploy/pathplanner/autos/BC Left Shallow One Cycle Dot.auto b/src/main/deploy/pathplanner/autos/BC Left Shallow One Cycle Dot.auto new file mode 100644 index 00000000..8d807cdf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Left Shallow One Cycle Dot.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BC Left To Shallow" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Left Shallow To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Corner To Dot Straight" + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Main", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Right Shallow One Cycle Dot.auto b/src/main/deploy/pathplanner/autos/BC Right Shallow One Cycle Dot.auto new file mode 100644 index 00000000..d0153b3a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Right Shallow One Cycle Dot.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BC Right To Shallow" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Right Shallow To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Corner To Dot Straight" + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Main", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left NY Second Pass.auto b/src/main/deploy/pathplanner/autos/Left NY Second Pass.auto new file mode 100644 index 00000000..603fdf5c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left NY Second Pass.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Left Score To Score NY" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right NY Second Pass.auto b/src/main/deploy/pathplanner/autos/Right NY Second Pass.auto new file mode 100644 index 00000000..bb246b2c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right NY Second Pass.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": null + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path b/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path new file mode 100644 index 00000000..08cad4d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.308, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 6.486898777611385, + "y": 7.524587798893349 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.963, + "y": 5.645 + }, + "prevControl": { + "x": 6.1264170176518595, + "y": 7.5128650589220225 + }, + "nextControl": { + "x": 5.890486422033948, + "y": 4.816166011187668 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.237, + "y": 5.672 + }, + "prevControl": { + "x": 7.237, + "y": 4.771999999999999 + }, + "nextControl": { + "x": 7.237, + "y": 5.922 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0667047075606275, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 7.316619314107814, + "y": 7.43437277334577 + }, + "nextControl": { + "x": 5.087089871611983, + "y": 7.49266112478357 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 5.294108416547789, + "y": 7.399604078672524 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1891117478510029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.5, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.2, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left Shallow To Score.path b/src/main/deploy/pathplanner/paths/BC Left Shallow To Score.path new file mode 100644 index 00000000..0593e3eb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left Shallow To Score.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.791269614835947, + "y": 5.174 + }, + "prevControl": null, + "nextControl": { + "x": 6.45637470114939, + "y": 7.949690106876804 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6506870229007635, + "y": 7.441 + }, + "prevControl": { + "x": 6.09859528645011, + "y": 7.650208015267176 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3816631130063977, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left To Shallow.path b/src/main/deploy/pathplanner/paths/BC Left To Shallow.path new file mode 100644 index 00000000..d1fdb1ee --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 6.63948673912623, + "y": 7.707882561953116 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.791269614835947, + "y": 5.174 + }, + "prevControl": { + "x": 7.584251069900143, + "y": 6.933657631954351 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.9253731343283487, + "rotationDegrees": -55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6867989646246767, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path b/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path new file mode 100644 index 00000000..52484b2d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.46089863956213, + "y": 0.4753134455838692 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.963, + "y": 2.355 + }, + "prevControl": { + "x": 5.799582982348141, + "y": 0.4871349410779766 + }, + "nextControl": { + "x": 6.035513577966052, + "y": 3.1838339888123324 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.237, + "y": 2.328 + }, + "prevControl": { + "x": 7.158559831527108, + "y": 3.2245752282825713 + }, + "nextControl": { + "x": 7.276220084236447, + "y": 1.8797123858587144 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0667047075606275, + "y": 0.559 + }, + "prevControl": { + "x": 7.316619267089228, + "y": 0.5655355134171268 + }, + "nextControl": { + "x": 5.087090244053958, + "y": 0.5072311198219507 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 5.268148066661913, + "y": 0.6003141499164485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1891117478510029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.5, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.2, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Right Shallow To Score.path b/src/main/deploy/pathplanner/paths/BC Right Shallow To Score.path new file mode 100644 index 00000000..6150939e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right Shallow To Score.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.868901569186875, + "y": 2.935 + }, + "prevControl": null, + "nextControl": { + "x": 6.404423364113454, + "y": 0.20272922519291026 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.559 + }, + "prevControl": { + "x": 6.018673323823109, + "y": 0.36492011412268166 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Right To Shallow.path b/src/main/deploy/pathplanner/paths/BC Right To Shallow.path new file mode 100644 index 00000000..859548fe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 6.938723126508627, + "y": 0.3260177844037628 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.868901569186875, + "y": 2.935 + }, + "prevControl": { + "x": 7.902374851629624, + "y": 1.7048568702290083 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.20916905444126394, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.5200573065902583, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.7736389684813755, + "rotationDegrees": 55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6143226919758464, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path index 358aa9d5..06ea202b 100644 --- a/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 7.564404708105101, + "x": 7.5644047081051005, "y": 7.509029957203994 }, "isLocked": false, @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 3.6506870229007635, + "x": 3.63796005706134, "y": 7.440906488549619 }, "prevControl": { - "x": 6.341276584160035, + "x": 6.328549618320611, "y": 7.591536259541984 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path b/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path index 25d25e6b..55b7a9a4 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 3.6506870229007635, + "x": 3.63796005706134, "y": 7.440906488549619 }, "prevControl": { - "x": 6.755965196937856, + "x": 6.743238231098433, "y": 7.6125392296718974 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Dot Straight.path b/src/main/deploy/pathplanner/paths/Left Corner To Dot Straight.path new file mode 100644 index 00000000..037feffc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner To Dot Straight.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.31, + "y": 7.44 + }, + "prevControl": null, + "nextControl": { + "x": 4.835636776657973, + "y": 7.44 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 7.441 + }, + "prevControl": { + "x": 7.931192525525695, + "y": 7.441 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 1f2d3ab9..523fd574 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 3.6506870229007635, + "x": 3.63796005706134, "y": 7.440906488549619 }, "prevControl": { - "x": 6.484465049928673, + "x": 6.4717380840892496, "y": 7.483152639087019 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Corner.path b/src/main/deploy/pathplanner/paths/Left Score To Corner.path index 98999296..082d1fcb 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Corner.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.6506870229007635, + "x": 3.63796005706134, "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 3.2366011042814318, + "x": 3.2238741384420084, "y": 7.349786421407005 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score NY.path b/src/main/deploy/pathplanner/paths/Left Score To Score NY.path new file mode 100644 index 00000000..8a3f8436 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Score To Score NY.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 6.717360912981454, + "y": 7.521968616262482 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 5.863409415121255, + "y": 5.244764621968616 + }, + "prevControl": { + "x": 5.962233970579355, + "y": 7.616553952963002 + }, + "nextControl": { + "x": 5.824593437945791, + "y": 4.31318116975749 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.937318116975749, + "y": 4.571954350927247 + }, + "prevControl": { + "x": 6.567013040113212, + "y": 4.295700209513629 + }, + "nextControl": { + "x": 7.5790326308920575, + "y": 5.0506847360913 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0667047075606275, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 7.3644732636065955, + "y": 7.433121689698743 + }, + "nextControl": { + "x": 5.087089871611983, + "y": 7.49266112478357 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 5.294108416547789, + "y": 7.399604078672524 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1891117478510029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.5, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.2, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index da774b21..347260f5 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 3.6506870229007635, + "x": 3.63796005706134, "y": 7.440906488549619 }, "prevControl": { - "x": 5.306835382387213, + "x": 5.29410841654779, "y": 7.399604078672524 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Left Shallow To Score.path b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path index ca317290..886e9aa9 100644 --- a/src/main/deploy/pathplanner/paths/Left Shallow To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.6506870229007635, + "x": 3.63796005706134, "y": 7.440906488549619 }, "prevControl": { - "x": 6.09859528645011, + "x": 6.085868320610687, "y": 7.650114503816795 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path index 909087b9..d7526e8b 100644 --- a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 3.6120827389443653, + "x": 3.612, "y": 0.559 }, "prevControl": { - "x": 6.1480599144079875, + "x": 6.147977175463622, "y": 0.5460613409415132 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path index 21c9f268..6e2fd9e1 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path @@ -4,12 +4,12 @@ { "anchor": { "x": 3.48, - "y": 0.57 + "y": 0.559 }, "prevControl": null, "nextControl": { "x": 5.818616522811345, - "y": 0.5423255240443889 + "y": 0.531325524044389 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot Straight.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot Straight.path new file mode 100644 index 00000000..2a3aea96 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot Straight.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.28, + "y": 0.56 + }, + "prevControl": null, + "nextControl": { + "x": 4.440424253599775, + "y": 0.56 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 0.559 + }, + "prevControl": { + "x": 8.003999999999998, + "y": 0.559 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 2da238c0..263ff2ce 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 3.6120827389443653, + "x": 3.612, "y": 0.559 }, "prevControl": { - "x": 6.311366666666668, + "x": 6.311283927722303, "y": 0.5283859724203506 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Corner.path b/src/main/deploy/pathplanner/paths/Right Score To Corner.path index 46dd4a62..206480da 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Corner.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 3.199971130397284, - "y": 0.6068359978960558 + "x": 3.199970867125222, + "y": 0.6068337301834343 }, "isLocked": false, "linkedName": "Right Trench Score" diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score NY.path b/src/main/deploy/pathplanner/paths/Right Score To Score NY.path new file mode 100644 index 00000000..5dc3f595 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Score To Score NY.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.574952924393722, + "y": 0.6236932952924387 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 5.850470756062768, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 5.850470756062768, + "y": 0.3280741797432247 + }, + "nextControl": { + "x": 5.850470756062768, + "y": 4.150659142168011 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.302881844380403, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 7.215816731986725, + "y": 3.83143356936277 + }, + "nextControl": { + "x": 7.525057636887608, + "y": 0.34949567723342856 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 7.673006390654899, + "y": 0.520948709769672 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17051509769094172, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.644760213143872, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.289978678038381, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.6703967446591785, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index db85e6e5..9263a4ea 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -48,11 +48,11 @@ }, { "anchor": { - "x": 3.6120827389443653, + "x": 3.612, "y": 0.559 }, "prevControl": { - "x": 7.673089129599259, + "x": 7.673006390654893, "y": 0.5209487097696721 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path index be0232ff..ef087155 100644 --- a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.6120827389443653, + "x": 3.612, "y": 0.559 }, "prevControl": { - "x": 6.018673323823109, + "x": 6.018590584878744, "y": 0.36492011412268166 }, "nextControl": null, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index f20085b3..9d9b1c35 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,7 +7,6 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.auton.regular.CenterTwoCornerBC; import com.stuypulse.robot.commands.auton.regular.Depot; import com.stuypulse.robot.commands.auton.regular.LeftBump; import com.stuypulse.robot.commands.auton.regular.LeftFollow; @@ -18,10 +17,10 @@ import com.stuypulse.robot.commands.auton.regular.RightBump; import com.stuypulse.robot.commands.auton.regular.RightFollow; import com.stuypulse.robot.commands.auton.regular.RightTwoCorner; -import com.stuypulse.robot.commands.auton.regular.TwoCornerBC; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerShallow; import com.stuypulse.robot.commands.auton.regular.RightTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; +import com.stuypulse.robot.commands.auton.regular.ShallowSwipeDot; import com.stuypulse.robot.commands.auton.test.PathfindTest; import com.stuypulse.robot.commands.auton.test.TestBC; import com.stuypulse.robot.commands.handoff.HandoffRun; @@ -450,39 +449,59 @@ public void configureAutons() { "Right Corner Bite", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_OUT_IN.register(autonChooser); + AutonConfig LEFT_SHALLOW_SWIPE_DOT = new AutonConfig("Left Shallow Swipe Dot", ShallowSwipeDot::new, + "BC Left To Shallow", "BC Left Shallow To Score", "Left Score To Corner", "Left Corner To Dot Straight" + ); + LEFT_SHALLOW_SWIPE_DOT.register(autonChooser); + + AutonConfig RIGHT_SHALLOW_SWIPE_DOT = new AutonConfig("Right Shallow Swipe Dot", ShallowSwipeDot::new, + "BC Right To Shallow", "BC Right Shallow To Score", "Right Score To Corner", "Right Corner To Dot Straight" + ); + RIGHT_SHALLOW_SWIPE_DOT.register(autonChooser); + + AutonConfig R_CN_NFS = new AutonConfig("Right Corner-Near Near-Far-Short", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right Corner Bite", "Right NZ To Score", "BC Right Score To Score NY", "Right Score To Corner", "Right Score To NZ (F)"); + R_CN_NFS.register(autonChooser); + + AutonConfig L_CN_NFS = new AutonConfig("Left Corner-Near Near-Far-Short", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + "Left Corner Bite", "Left NZ To Score", "BC Left Score To Score NY", "Left Score To Corner", "Left Score To NZ (F)"); + L_CN_NFS.register(autonChooser); + + + //BC RIGHT - AutonConfig RIGHT_OUT_OUT_DOT = new AutonConfig("Right Out Out Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); - RIGHT_OUT_OUT_DOT.register(autonChooser); + // AutonConfig RIGHT_OUT_OUT_DOT = new AutonConfig("Right Out Out Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); + // RIGHT_OUT_OUT_DOT.register(autonChooser); - AutonConfig RIGHT_OUT_IN_DOT = new AutonConfig("Right Out In Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot"); - RIGHT_OUT_IN_DOT.register(autonChooser); + // AutonConfig RIGHT_OUT_IN_DOT = new AutonConfig("Right Out In Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot"); + // RIGHT_OUT_IN_DOT.register(autonChooser); - AutonConfig RIGHT_OUT_OUT_CENTER_DOT = new AutonConfig("Right Out Out Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); - RIGHT_OUT_OUT_CENTER_DOT.register(autonChooser); + // AutonConfig RIGHT_OUT_OUT_CENTER_DOT = new AutonConfig("Right Out Out Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); + // RIGHT_OUT_OUT_CENTER_DOT.register(autonChooser); - AutonConfig RIGHT_OUT_IN_CENTER_DOT = new AutonConfig("Right Out In Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); - RIGHT_OUT_IN_CENTER_DOT.register(autonChooser); + // AutonConfig RIGHT_OUT_IN_CENTER_DOT = new AutonConfig("Right Out In Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); + // RIGHT_OUT_IN_CENTER_DOT.register(autonChooser); //BC LEFT - AutonConfig LEFT_OUT_OUT_DOT = new AutonConfig("Left Out Out Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Dot"); - LEFT_OUT_OUT_DOT.register(autonChooser); + // AutonConfig LEFT_OUT_OUT_DOT = new AutonConfig("Left Out Out Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Dot"); + // LEFT_OUT_OUT_DOT.register(autonChooser); - AutonConfig LEFT_OUT_IN_DOT = new AutonConfig("Left Out In Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Dot"); - LEFT_OUT_IN_DOT.register(autonChooser); + // AutonConfig LEFT_OUT_IN_DOT = new AutonConfig("Left Out In Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Dot"); + // LEFT_OUT_IN_DOT.register(autonChooser); - AutonConfig LEFT_OUT_OUT_CENTER_DOT = new AutonConfig("Left Out Out Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); - LEFT_OUT_OUT_CENTER_DOT.register(autonChooser); + // AutonConfig LEFT_OUT_OUT_CENTER_DOT = new AutonConfig("Left Out Out Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); + // LEFT_OUT_OUT_CENTER_DOT.register(autonChooser); - AutonConfig LEFT_OUT_IN_CENTER_DOT = new AutonConfig("Left Out In Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); - LEFT_OUT_IN_CENTER_DOT.register(autonChooser); + // AutonConfig LEFT_OUT_IN_CENTER_DOT = new AutonConfig("Left Out In Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); + // LEFT_OUT_IN_CENTER_DOT.register(autonChooser); // FOLLOWS AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/ShallowSwipeDot.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/ShallowSwipeDot.java new file mode 100644 index 00000000..f945cb7a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/ShallowSwipeDot.java @@ -0,0 +1,66 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class ShallowSwipeDot extends SequentialCommandGroup { + + public ShallowSwipeDot(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest(), + new WaitCommand(6.5) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure SOTM is over + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + + new SwerveXMode() + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/Right Score To Score NY.path b/src/main/java/com/stuypulse/robot/commands/handoff/Right Score To Score NY.path new file mode 100644 index 00000000..d0f23ac2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/handoff/Right Score To Score NY.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": null, + "nextControl": { + "x": 6.575035663338087, + "y": 0.6515406562054205 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 5.850470756062768, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 5.850470756062768, + "y": 0.3280741797432247 + }, + "nextControl": { + "x": 5.850470756062768, + "y": 4.150659142168011 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.302881844380403, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 7.215816731986725, + "y": 3.83143356936277 + }, + "nextControl": { + "x": 7.525057636887608, + "y": 0.34949567723342856 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 7.673089129599265, + "y": 0.5487960706826538 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17051509769094172, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.644760213143872, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.289978678038381, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.6703967446591785, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file From 892287e55aa4e54ff806cde60e72fb748c2e5a40 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 30 May 2026 11:59:13 -0400 Subject: [PATCH 64/97] refactor: rename certain autons --- .../deploy/pathplanner/autos/Right NY Second Pass.auto | 2 +- src/main/java/com/stuypulse/robot/RobotContainer.java | 10 +++++----- .../java/com/stuypulse/robot/constants/Settings.java | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Right NY Second Pass.auto b/src/main/deploy/pathplanner/autos/Right NY Second Pass.auto index bb246b2c..d249d7ee 100644 --- a/src/main/deploy/pathplanner/autos/Right NY Second Pass.auto +++ b/src/main/deploy/pathplanner/autos/Right NY Second Pass.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": null + "pathName": "BC Right Score To Score NY" } }, { diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index bf2f3b51..5586524a 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -449,21 +449,21 @@ public void configureAutons() { "Right Corner Bite", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); R_CN_NF.register(autonChooser); - AutonConfig LEFT_SHALLOW_SWIPE_DOT = new AutonConfig("Left Shallow Swipe Dot", ShallowSwipeDot::new, + AutonConfig L_CNL_D = new AutonConfig("Left Corner-Near Long Dot", ShallowSwipeDot::new, "BC Left To Shallow", "BC Left Shallow To Score", "Left Score To Corner", "Left Corner To Dot Straight" ); - LEFT_SHALLOW_SWIPE_DOT.register(autonChooser); + L_CNL_D.register(autonChooser); - AutonConfig RIGHT_SHALLOW_SWIPE_DOT = new AutonConfig("Right Shallow Swipe Dot", ShallowSwipeDot::new, + AutonConfig R_CNL_D = new AutonConfig("Right Corner-Near Long Dot", ShallowSwipeDot::new, "BC Right To Shallow", "BC Right Shallow To Score", "Right Score To Corner", "Right Corner To Dot Straight" ); - RIGHT_SHALLOW_SWIPE_DOT.register(autonChooser); + R_CNL_D.register(autonChooser); AutonConfig R_CN_NFS = new AutonConfig("Right Corner-Near Near-Far-Short", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner Bite", "Right NZ To Score", "BC Right Score To Score NY", "Right Score To Corner", "Right Score To NZ (F)"); R_CN_NFS.register(autonChooser); - AutonConfig L_CN_NFS = new AutonConfig("Left Corner-Near Near-Far-Short", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + AutonConfig L_CN_NFS = new AutonConfig("Left Corner-Near Near-Far-Short", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, "Left Corner Bite", "Left NZ To Score", "BC Left Score To Score NY", "Left Score To Corner", "Left Score To NZ (F)"); L_CN_NFS.register(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index cf03f60f..ac909ab8 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -168,9 +168,9 @@ public interface FerryRPMInterpolation { {5.16, 3300.0}, {6.94, 3600.0}, {7.87, 3800.0}, - // {9.77, 4300.0}, //TODO: ADD DATA BACK IN COMP - // {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS EXTRAPOLATED!!! - // {11.516, 4900.0} + {9.77, 4300.0}, //TODO: ADD DATA BACK IN COMP + {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS EXTRAPOLATED!!! + {11.516, 4900.0} }; } From 166d3381bc252edc77b4d569441cbab5c2966719 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Sat, 30 May 2026 12:29:45 -0400 Subject: [PATCH 65/97] feat: updated the NY paths to rotate less aggressively on return. Had to compensate by 0.3 seconds (END OF THE WORLD!!!) --- .../paths/BC Left Score To Score NY.path | 34 ++++++++--------- .../paths/BC Right Score To Score NY.path | 38 +++++++++---------- 2 files changed, 36 insertions(+), 36 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path b/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path index 08cad4d7..2e1450ae 100644 --- a/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path +++ b/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path @@ -16,31 +16,31 @@ }, { "anchor": { - "x": 5.963, + "x": 6.163, "y": 5.645 }, "prevControl": { - "x": 6.1264170176518595, - "y": 7.5128650589220225 + "x": 6.488590333125494, + "y": 7.49151453689789 }, "nextControl": { - "x": 5.890486422033948, - "y": 4.816166011187668 + "x": 6.001159898414421, + "y": 4.7271591741926215 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.237, + "x": 7.637, "y": 5.672 }, "prevControl": { - "x": 7.237, + "x": 7.637, "y": 4.771999999999999 }, "nextControl": { - "x": 7.237, + "x": 7.637, "y": 5.922 }, "isLocked": false, @@ -48,15 +48,15 @@ }, { "anchor": { - "x": 7.0667047075606275, + "x": 7.067, "y": 7.440906488549619 }, "prevControl": { - "x": 7.316619314107814, - "y": 7.43437277334577 + "x": 8.01667550487931, + "y": 7.416078370774993 }, "nextControl": { - "x": 5.087089871611983, + "x": 5.087385164051356, "y": 7.49266112478357 }, "isLocked": false, @@ -78,11 +78,11 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.1891117478510029, + "waypointRelativePos": 0.34587995930824006, "rotationDegrees": 0.0 }, { - "waypointRelativePos": 0.6340248962655519, + "waypointRelativePos": 0.8, "rotationDegrees": -90.0 }, { @@ -94,11 +94,11 @@ "rotationDegrees": 90.0 }, { - "waypointRelativePos": 2.5, - "rotationDegrees": 90.0 + "waypointRelativePos": 3.0, + "rotationDegrees": 0.0 }, { - "waypointRelativePos": 3.2, + "waypointRelativePos": 3.05, "rotationDegrees": 0.0 } ], diff --git a/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path b/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path index 52484b2d..0f6099e8 100644 --- a/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path +++ b/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path @@ -16,47 +16,47 @@ }, { "anchor": { - "x": 5.963, + "x": 6.163, "y": 2.355 }, "prevControl": { - "x": 5.799582982348141, - "y": 0.4871349410779766 + "x": 6.488590333125495, + "y": 0.5084854631021092 }, "nextControl": { - "x": 6.035513577966052, - "y": 3.1838339888123324 + "x": 6.001159898414421, + "y": 3.272840825807378 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.237, + "x": 7.637, "y": 2.328 }, "prevControl": { - "x": 7.158559831527108, - "y": 3.2245752282825713 + "x": 7.637, + "y": 3.228 }, "nextControl": { - "x": 7.276220084236447, - "y": 1.8797123858587144 + "x": 7.637, + "y": 2.078 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.0667047075606275, + "x": 7.067, "y": 0.559 }, "prevControl": { - "x": 7.316619267089228, - "y": 0.5655355134171268 + "x": 8.016675326208684, + "y": 0.583834950985082 }, "nextControl": { - "x": 5.087090244053958, + "x": 5.087385536493331, "y": 0.5072311198219507 }, "isLocked": false, @@ -78,11 +78,11 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.1891117478510029, + "waypointRelativePos": 0.34587995930824006, "rotationDegrees": 0.0 }, { - "waypointRelativePos": 0.6340248962655519, + "waypointRelativePos": 0.8, "rotationDegrees": 90.0 }, { @@ -94,11 +94,11 @@ "rotationDegrees": -90.0 }, { - "waypointRelativePos": 2.5, - "rotationDegrees": -90.0 + "waypointRelativePos": 3.0, + "rotationDegrees": 0.0 }, { - "waypointRelativePos": 3.2, + "waypointRelativePos": 3.05, "rotationDegrees": 0.0 } ], From b41dd3bcf3378948ba68a6b924d5efe17c04e494 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Sat, 30 May 2026 14:42:14 -0400 Subject: [PATCH 66/97] feat: auton changes. Score path shifted more towards NZ to make a bigger tolerance of the trench --- .../BC Left Bite Score To Score LONG.path | 145 ++++++++++++++++++ .../paths/BC Left Bite Score To Score.path | 66 ++++---- .../paths/BC Left Score To Score.path | 20 +-- .../paths/BC Right Bite Score To Score.path | 32 ++-- .../paths/BC Right Score To Score.path | 16 +- 5 files changed, 212 insertions(+), 67 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/BC Left Bite Score To Score LONG.path diff --git a/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score LONG.path b/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score LONG.path new file mode 100644 index 00000000..915897b5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score LONG.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.57, + "y": 7.42 + }, + "prevControl": null, + "nextControl": { + "x": 7.8268188302425035, + "y": 7.488123468654376 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.7265763195435095, + "y": 5.904636233951498 + }, + "prevControl": { + "x": 7.687760342368046, + "y": 7.884251069900142 + }, + "nextControl": { + "x": 7.76360928403665, + "y": 4.015955044801327 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.794992867332382, + "y": 3.782696148359487 + }, + "prevControl": { + "x": 7.6166741233975355, + "y": 3.782696148359487 + }, + "nextControl": { + "x": 5.966918687589157, + "y": 3.782696148359487 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.109243937232525, + "y": 6.059900142653353 + }, + "prevControl": { + "x": 6.143981742457801, + "y": 4.624070860008561 + }, + "nextControl": { + "x": 6.070427960057062, + "y": 7.664293865905849 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.651, + "y": 7.441 + }, + "prevControl": { + "x": 6.341589561259271, + "y": 7.591629770992365 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.307036247334758, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.8272921108741973, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.2366737739872133, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.7421203438395327, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.6183368869935886, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.0533049040511733, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.3176972281449895, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8835202761000936, + "maxWaypointRelativePos": 3.057808455565133, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score.path b/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score.path index 915897b5..d5713997 100644 --- a/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score.path @@ -8,68 +8,68 @@ }, "prevControl": null, "nextControl": { - "x": 7.8268188302425035, - "y": 7.488123468654376 + "x": 5.483737517831668, + "y": 7.453938659058489 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.7265763195435095, - "y": 5.904636233951498 + "x": 8.04, + "y": 6.999 }, "prevControl": { - "x": 7.687760342368046, - "y": 7.884251069900142 + "x": 8.017259660107046, + "y": 7.684860122523228 }, "nextControl": { - "x": 7.76360928403665, - "y": 4.015955044801327 + "x": 8.117647798240464, + "y": 4.6571032356791555 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.794992867332382, - "y": 3.782696148359487 + "x": 7.302, + "y": 4.075 }, "prevControl": { - "x": 7.6166741233975355, - "y": 3.782696148359487 + "x": 8.414514968383727, + "y": 4.053397767604201 }, "nextControl": { - "x": 5.966918687589157, - "y": 3.782696148359487 + "x": 5.969318116975748, + "y": 4.100877318116975 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.109243937232525, - "y": 6.059900142653353 + "x": 6.332, + "y": 5.912 }, "prevControl": { - "x": 6.143981742457801, - "y": 4.624070860008561 + "x": 6.310128594617119, + "y": 4.825295699627398 }, "nextControl": { - "x": 6.070427960057062, - "y": 7.664293865905849 + "x": 6.370800820617092, + "y": 7.839860504820692 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.651, + "x": 3.6120827389443653, "y": 7.441 }, "prevControl": { - "x": 6.341589561259271, - "y": 7.591629770992365 + "x": 6.148059987278764, + "y": 7.453924368494453 }, "nextControl": null, "isLocked": false, @@ -78,39 +78,39 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.307036247334758, + "waypointRelativePos": 0.58, "rotationDegrees": 0.0 }, { - "waypointRelativePos": 0.8272921108741973, + "waypointRelativePos": 1.06, "rotationDegrees": -90.0 }, { - "waypointRelativePos": 1.2366737739872133, + "waypointRelativePos": 1.38, "rotationDegrees": -90.0 }, { - "waypointRelativePos": 1.7421203438395327, + "waypointRelativePos": 1.98, "rotationDegrees": 180.0 }, { - "waypointRelativePos": 2.6183368869935886, + "waypointRelativePos": 2.5, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 3.0533049040511733, + "waypointRelativePos": 3.08, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 3.3176972281449895, + "waypointRelativePos": 3.44, "rotationDegrees": 0.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.8835202761000936, - "maxWaypointRelativePos": 3.057808455565133, + "minWaypointRelativePos": 1.0353753235547816, + "maxWaypointRelativePos": 3.1406384814495194, "constraints": { "maxVelocity": 2.5, "maxAcceleration": 10.0, @@ -128,7 +128,7 @@ "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/BC Left Score To Score.path b/src/main/deploy/pathplanner/paths/BC Left Score To Score.path index 020e4bc6..63f3db33 100644 --- a/src/main/deploy/pathplanner/paths/BC Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/BC Left Score To Score.path @@ -9,22 +9,22 @@ "prevControl": null, "nextControl": { "x": 6.983047455470738, - "y": 7.5513268447837145 + "y": 7.572326844783714 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.931333333333333, + "x": 6.131, "y": 5.399222222222222 }, "prevControl": { - "x": 5.950822222222223, + "x": 6.150488888888891, "y": 7.640444444444444 }, "nextControl": { - "x": 5.923225885345372, + "x": 6.122892552012039, "y": 4.466865703606724 }, "isLocked": false, @@ -32,15 +32,15 @@ }, { "anchor": { - "x": 7.207855555555556, + "x": 7.408, "y": 4.9314888888888895 }, "prevControl": { - "x": 6.837550478693018, + "x": 7.037694923137463, "y": 4.655234747475271 }, "nextControl": { - "x": 7.849570069471864, + "x": 8.049714513916308, "y": 5.410219274052943 }, "isLocked": false, @@ -48,15 +48,15 @@ }, { "anchor": { - "x": 7.0667047075606275, + "x": 7.267, "y": 7.440906488549619 }, "prevControl": { - "x": 7.3644732636065955, + "x": 7.564768556045968, "y": 7.433121689698743 }, "nextControl": { - "x": 5.087089871611983, + "x": 5.287385164051356, "y": 7.49266112478357 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path index 8506962c..8e70455a 100644 --- a/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 7.739514978601996, + "x": 8.04, "y": 1.0008844507845935 }, "prevControl": { - "x": 7.7167792788333145, + "x": 8.017264300231318, "y": 0.31502417442937847 }, "nextControl": { - "x": 7.817146932952923, + "x": 8.117631954350927, "y": 3.3427817403708993 }, "isLocked": false, @@ -32,15 +32,15 @@ }, { "anchor": { - "x": 7.002011412268189, + "x": 7.302, "y": 3.925021398002854 }, "prevControl": { - "x": 8.114526380651917, + "x": 8.414514968383727, "y": 3.9034191656070543 }, "nextControl": { - "x": 5.669329529243937, + "x": 5.969318116975748, "y": 3.9508987161198283 }, "isLocked": false, @@ -48,15 +48,15 @@ }, { "anchor": { - "x": 6.031611982881596, + "x": 6.332, "y": 2.0877318116975756 }, "prevControl": { - "x": 6.009732033987853, + "x": 6.310120051106661, "y": 3.174435940086786 }, "nextControl": { - "x": 6.070427960057061, + "x": 6.370815977175208, "y": 0.15987161198288025 }, "isLocked": false, @@ -78,31 +78,31 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.5714285714285716, + "waypointRelativePos": 0.58, "rotationDegrees": 0.0 }, { - "waypointRelativePos": 0.9722814498933815, + "waypointRelativePos": 1.06, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 1.3475479744136523, + "waypointRelativePos": 1.38, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 1.936034115138597, + "waypointRelativePos": 1.98, "rotationDegrees": 180.0 }, { - "waypointRelativePos": 2.4818763326225852, + "waypointRelativePos": 2.5, "rotationDegrees": -90.0 }, { - "waypointRelativePos": 3.0618336886993425, + "waypointRelativePos": 3.08, "rotationDegrees": -90.0 }, { - "waypointRelativePos": 3.4968017057569094, + "waypointRelativePos": 3.44, "rotationDegrees": 0.0 } ], diff --git a/src/main/deploy/pathplanner/paths/BC Right Score To Score.path b/src/main/deploy/pathplanner/paths/BC Right Score To Score.path index ca06a66d..7e0dc8a9 100644 --- a/src/main/deploy/pathplanner/paths/BC Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/BC Right Score To Score.path @@ -9,22 +9,22 @@ "prevControl": null, "nextControl": { "x": 7.133082715798332, - "y": 0.5311980499756427 + "y": 0.5201980499756428 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.850470756062768, + "x": 6.05, "y": 2.851112696148359 }, "prevControl": { - "x": 5.850470756062768, + "x": 6.05, "y": 0.3280741797432247 }, "nextControl": { - "x": 5.850470756062768, + "x": 6.05, "y": 4.150659142168011 }, "isLocked": false, @@ -32,15 +32,15 @@ }, { "anchor": { - "x": 7.302881844380403, + "x": 7.503, "y": 2.851112696148359 }, "prevControl": { - "x": 7.215816731986725, + "x": 7.415934887606322, "y": 3.83143356936277 }, "nextControl": { - "x": 7.525057636887608, + "x": 7.725175792507205, "y": 0.34949567723342856 }, "isLocked": false, @@ -52,7 +52,7 @@ "y": 0.559 }, "prevControl": { - "x": 7.673089129599258, + "x": 7.673089129599257, "y": 0.5209487097696721 }, "nextControl": null, From da26d3a3a57ecf490294535cd6d3670a420976c8 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 30 May 2026 15:27:58 -0400 Subject: [PATCH 67/97] feat: log LL latency --- .../com/stuypulse/robot/subsystems/vision/LimelightVision.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 63d31f75..a171bdd4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -375,8 +375,7 @@ public void periodicAfterScheduler() { DogLog.log("Vision/Limelight Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); // this is just the yaw of the internal imu DogLog.log("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); - - + DogLog.log("Vision/latency_pipeline " + limelightName , LimelightHelpers.getLatency_Pipeline(limelightName)); } } From aa629ea4d88c7d20c7109a8a48be0463c2202dd2 Mon Sep 17 00:00:00 2001 From: Apetrock Date: Sat, 30 May 2026 16:37:58 -0400 Subject: [PATCH 68/97] Feat: latency dead check for limelights --- .../subsystems/vision/LimelightVision.java | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 63d31f75..f38a83cf 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -54,6 +54,10 @@ public static LimelightVision getInstance() { private double rightLLHeartbeat = -1; private double backLLHeartbeat = -1; + private double prevLeftLLLatency = -1; //change to -1 is we need to switch the way we do this + private double prevRightLLLatency = -1; + private double prevBackLLLatency = -1; + private int leftLoopCounter = 0; private int rightLoopCounter = 0; private int backLoopCounter = 0; @@ -238,14 +242,17 @@ public void periodicAfterScheduler() { rightLoopCounter += 1; if (rightLoopCounter == 50) { DogLog.log("LED/Right Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat); - if (LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1) { + boolean isDeadLatency = (prevRightLLLatency == LimelightHelpers.getLatency_Pipeline(limelightName)); + if (LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1 || isDeadLatency) { LEDController.isRightLLDead = true; } else { LEDController.isRightLLDead = false; } rightLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); + prevRightLLLatency = LimelightHelpers.getLatency_Pipeline(limelightName); rightLoopCounter = 0; + DogLog.log("Vision/" + limelightName +"/is dead by latency", isDeadLatency); } } if (limelightName.equals(Cameras.LimelightCameras[1].getName())) { @@ -254,7 +261,8 @@ public void periodicAfterScheduler() { leftLoopCounter += 1; if (leftLoopCounter == 50) { DogLog.log("LED/Left Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat); - if (LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1) { + boolean isDeadLatency = (prevLeftLLLatency == LimelightHelpers.getLatency_Pipeline(limelightName)); + if (LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1 || isDeadLatency) { LEDController.isLeftLLDead = true; } else { @@ -262,6 +270,8 @@ public void periodicAfterScheduler() { } leftLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); leftLoopCounter = 0; + prevLeftLLLatency = LimelightHelpers.getLatency_Pipeline(limelightName); + DogLog.log("Vision/" + limelightName +"/is dead by latency", isDeadLatency); } } if (limelightName.equals(Cameras.LimelightCameras[2].getName())) { @@ -269,8 +279,9 @@ public void periodicAfterScheduler() { DogLog.log("LED/variable heartbeat " + limelightName, backLLHeartbeat); backLoopCounter += 1; if (backLoopCounter == 50) { + boolean isDeadLatency = (prevBackLLLatency == LimelightHelpers.getLatency_Pipeline(limelightName)); DogLog.log("LED/Back Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat); - if (LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && backLLHeartbeat != -1) { + if (LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && backLLHeartbeat != -1 || isDeadLatency) { LEDController.isBackLLDead = true; } else { @@ -278,11 +289,10 @@ public void periodicAfterScheduler() { } backLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); backLoopCounter = 0; + prevBackLLLatency = LimelightHelpers.getLatency_Pipeline(limelightName); + DogLog.log("Vision/" + limelightName +"/is dead by latency", isDeadLatency); } } - - - // Seed robot heading (used by MT2) LimelightHelpers.SetRobotOrientation( limelightName, From 870b3218f9054787b6b6715ad18fc4e65fdeca01 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 30 May 2026 17:09:39 -0400 Subject: [PATCH 69/97] feat: duplicate anti collision for corner bites. SHALLOW ACTUALLY MODIFIED. --- .../Left Corner Bite Anti Collision.path | 81 +++++++++++++++++++ .../Left NZ To Score Anti Collision.path | 79 ++++++++++++++++++ .../paths/Left Shallow To Score.path | 4 +- .../pathplanner/paths/Left To Shallow.path | 4 +- .../Right Corner Bite Anti Collision.path | 81 +++++++++++++++++++ .../Right NZ To Score Anti Collision.path | 79 ++++++++++++++++++ .../paths/Right Shallow To Score.path | 4 +- .../pathplanner/paths/Right To Shallow.path | 4 +- src/main/deploy/pathplanner/settings.json | 3 +- .../com/stuypulse/robot/RobotContainer.java | 12 +-- 10 files changed, 336 insertions(+), 15 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path create mode 100644 src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path create mode 100644 src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path create mode 100644 src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path b/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path new file mode 100644 index 00000000..61414bc9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 6.639728958630526, + "y": 7.677232524964337 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.764, + "y": 4.902 + }, + "prevControl": { + "x": 7.267033333333333, + "y": 7.864311111111112 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "L-Anti-Collision" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.9402985074626858, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6212018906144481, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Anti-Anti Collision", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path b/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path new file mode 100644 index 00000000..f11d066a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.764, + "y": 4.902 + }, + "prevControl": null, + "nextControl": { + "x": 6.117188888888889, + "y": 4.853277777777779 + }, + "isLocked": false, + "linkedName": "L-Anti-Collision" + }, + { + "anchor": { + "x": 6.510342368045648, + "y": 6.383 + }, + "prevControl": { + "x": 6.4994078379642675, + "y": 5.628079185575136 + }, + "nextControl": { + "x": 6.530430041188528, + "y": 7.769854529281173 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.4716451806743285, + "y": 7.4831512540767555 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.3432835820895521, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Anti-Anti Collision", + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Shallow To Score.path b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path index 886e9aa9..86f7bf60 100644 --- a/src/main/deploy/pathplanner/paths/Left Shallow To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 7.791269614835947, + "x": 7.569, "y": 5.374151212553495 }, "prevControl": null, "nextControl": { - "x": 6.457815375526781, + "x": 6.235545760690834, "y": 7.811409368840588 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left To Shallow.path b/src/main/deploy/pathplanner/paths/Left To Shallow.path index 1b597877..de8407c0 100644 --- a/src/main/deploy/pathplanner/paths/Left To Shallow.path +++ b/src/main/deploy/pathplanner/paths/Left To Shallow.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 7.791269614835947, + "x": 7.569, "y": 5.374151212553495 }, "prevControl": { - "x": 7.584251069900143, + "x": 7.361981455064195, "y": 7.133808844507846 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path b/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path new file mode 100644 index 00000000..124bc038 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 7.584251069900143, + "y": 0.48333808844507875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.637, + "y": 3.235955555555555 + }, + "prevControl": { + "x": 7.675977777777777, + "y": 1.9302000000000006 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "R-Anti-Collision" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17621776504297812, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.41791044776119385, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.7782515991471214, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5077650236326793, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Anti-Anti Collision", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path b/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path new file mode 100644 index 00000000..9eda0bbc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.637, + "y": 3.235955555555555 + }, + "prevControl": null, + "nextControl": { + "x": 5.9317817608038474, + "y": 3.1385145133157755 + }, + "isLocked": false, + "linkedName": "R-Anti-Collision" + }, + { + "anchor": { + "x": 6.419771754636234, + "y": 1.4925534950071324 + }, + "prevControl": { + "x": 6.437298642104417, + "y": 2.392382816720799 + }, + "nextControl": { + "x": 6.399066666666666, + "y": 0.42955555555555525 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 6.511813503834114, + "y": 0.5261116588846113 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.23445825932504563, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.488272921108742, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Anti-Anti Collision", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path index ef087155..151d3989 100644 --- a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 7.868901569186875, + "x": 7.569, "y": 2.7346647646219684 }, "prevControl": null, "nextControl": { - "x": 6.419771754636235, + "x": 6.1198701854493605, "y": 0.34101283880171307 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right To Shallow.path b/src/main/deploy/pathplanner/paths/Right To Shallow.path index a90eea65..4aedf86c 100644 --- a/src/main/deploy/pathplanner/paths/Right To Shallow.path +++ b/src/main/deploy/pathplanner/paths/Right To Shallow.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 7.868901569186875, + "x": 7.569, "y": 2.7346647646219684 }, "prevControl": { - "x": 7.902374851629624, + "x": 7.602473282442749, "y": 1.5045216348509767 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 48443623..4c890d06 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,13 +3,14 @@ "robotLength": 0.762, "holonomicMode": true, "pathFolders": [ + "Anti-Anti Collision", + "BC To Dot", "BC modified", "Bump Stuff", "Follow", "Non-Collision", "PathFinder Test", "To Depot", - "BC To Dot", "To NZ", "To Score" ], diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5586524a..203b9617 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -426,11 +426,11 @@ public void configureAutons() { // TWO CYCLES (CORNER) AutonConfig L_CN_FN = new AutonConfig("Left Corner-Near Far-Near", LeftTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + "Left Corner Bite Anti Collision", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); L_CN_FN.register(autonChooser); AutonConfig R_CN_FN = new AutonConfig("Right Corner-Near Far-Near", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + "Right Corner Bite Anti Collision", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); R_CN_FN.register(autonChooser); AutonConfig L_FNS_FN = new AutonConfig("Left Far-Near Shallow Far-Near", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, @@ -442,11 +442,11 @@ public void configureAutons() { R_FNS_FN.register(autonChooser); AutonConfig L_CN_NF = new AutonConfig("Left Corner-Near Near-Far", LeftTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite", "Left NZ To Score", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + "Left Corner Bite Anti Collision", "Left NZ To Score", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); L_CN_NF.register(autonChooser); AutonConfig R_CN_NF = new AutonConfig("Right Corner-Near Near-Far", RightTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + "Right Corner Bite Anti Collision", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); R_CN_NF.register(autonChooser); AutonConfig L_CNL_D = new AutonConfig("Left Corner-Near Long Dot", ShallowSwipeDot::new, @@ -460,11 +460,11 @@ public void configureAutons() { R_CNL_D.register(autonChooser); AutonConfig R_CN_NFS = new AutonConfig("Right Corner-Near Near-Far-Short", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite", "Right NZ To Score", "BC Right Score To Score NY", "Right Score To Corner", "Right Score To NZ (F)"); + "Right Corner Bite Anti Collision", "Right NZ To Score", "BC Right Score To Score NY", "Right Score To Corner", "Right Score To NZ (F)"); R_CN_NFS.register(autonChooser); AutonConfig L_CN_NFS = new AutonConfig("Left Corner-Near Near-Far-Short", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite", "Left NZ To Score", "BC Left Score To Score NY", "Left Score To Corner", "Left Score To NZ (F)"); + "Left Corner Bite Anti Collision", "Left NZ To Score", "BC Left Score To Score NY", "Left Score To Corner", "Left Score To NZ (F)"); L_CN_NFS.register(autonChooser); From 0903d2600ab2e8ff2a37027c4d1ea98a978a96bb Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 30 May 2026 17:11:39 -0400 Subject: [PATCH 70/97] FIX: increased intake pushdown voltage in auto --- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index ac909ab8..439fa2cc 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -78,7 +78,7 @@ public interface Intake { double PUSHDOWN_VOLTAGE = -3.0; double PUSHDOWN_CURRENT_TELEOP = -75.0;//new SmartNumber("Intake/Pushdown Current", -65.0); //TODO: GET ACTUAL TYTY - double PUSHDOWN_CURRENT_AUTON = -80.0; + double PUSHDOWN_CURRENT_AUTON = -95.0; double GEAR_RATIO = 32.0/20.0 * 64.0/18.0 * 60.0/8.0; From 7ef994b62626e978f0c01a0b47bbcaf68dc55bb1 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Sat, 30 May 2026 18:13:57 -0400 Subject: [PATCH 71/97] feat: refresh and apply new supply current limit configs at start of auton/teleop --- src/main/java/com/stuypulse/robot/Robot.java | 5 ++++- .../robot/subsystems/intake/Intake.java | 2 ++ .../robot/subsystems/intake/IntakeImpl.java | 21 ++++++++++++++++++- .../robot/subsystems/intake/IntakeSim.java | 6 ++++++ 4 files changed, 32 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 8ba7106f..9912027a 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -16,13 +16,13 @@ import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; -import com.stuypulse.robot.commands.superstructure.SuperstructureStow; import com.stuypulse.robot.commands.swerve.SwerveAutonInit; import com.stuypulse.robot.commands.swerve.SwerveTeleopInit; import com.stuypulse.robot.commands.vision.BlackListAllTagsForAllCameras; import com.stuypulse.robot.commands.vision.SetMegaTagMode; import com.stuypulse.robot.commands.vision.WhitelistAllTagsForAllCameras; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -45,6 +45,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; public class Robot extends TimedRobot { @@ -223,6 +224,7 @@ public void autonomousInit() { mode = RobotMode.AUTON; CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); + CommandScheduler.getInstance().schedule(new InstantCommand(() -> Intake.getInstance().autonInit(), Intake.getInstance())); auto = robot.getAutonomousCommand(); @@ -257,6 +259,7 @@ public void teleopInit() { CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); CommandScheduler.getInstance().schedule(new IntakeDeploy()); + CommandScheduler.getInstance().schedule(new InstantCommand(() -> Intake.getInstance().teleopInit(), Intake.getInstance())); if (auto != null) { auto.cancel(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index e0710c95..68a66f1a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -93,6 +93,8 @@ public void setRollerState(RollerState state) { public abstract void setPivotVoltageOverride(Optional voltage); public abstract SysIdRoutine getPivotSysIdRoutine(); public abstract boolean pivotStalling(); + public abstract void teleopInit(); + public abstract void autonInit(); public abstract void seedPivotDeployed(); public abstract void seedPivotStowed(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 330d5fa1..f0461446 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -8,6 +8,8 @@ import java.util.Optional; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionVoltage; @@ -77,7 +79,7 @@ public IntakeImpl() { .withInvertedValue(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) - .withSupplyCurrentLimitAmps(10.0) // was 60 on practice day + .withSupplyCurrentLimitAmps(20.0) // was 60 on practice day .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) @@ -145,6 +147,22 @@ public IntakeImpl() { .filtered(new BDebounce.Falling(0.1)); } + @Override + public void teleopInit() { + TalonFXConfiguration newConfiguration = new TalonFXConfiguration(); + pivot.getConfigurator().refresh(newConfiguration); + newConfiguration.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(10)); + pivot.getConfigurator().apply(newConfiguration); + } + + @Override + public void autonInit() { + TalonFXConfiguration newConfiguration = new TalonFXConfiguration(); + pivot.getConfigurator().refresh(newConfiguration); + newConfiguration.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(20)); + pivot.getConfigurator().apply(newConfiguration); + } + @Override public boolean pivotStalling() { return pivotStalling.get(); @@ -280,6 +298,7 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge + String.valueOf(Ports.Intake.ROLLER_LEADER) + ")", rollerLeader.isConnected()); DogLog.log("Robot/CAN/Main/Intake Roller Follower Motor Connected? (ID " + String.valueOf(Ports.Intake.ROLLER_FOLLOWER) + ")", rollerFollower.isConnected()); + } Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java index ca3dbffb..7c4485b0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java @@ -216,4 +216,10 @@ public SysIdRoutine getPivotSysIdRoutine() { public double getCurrentDraw() { return 0; } + + @Override + public void teleopInit() {} + + @Override + public void autonInit() {} } \ No newline at end of file From ebbeef81b14b9cc45b14c0fd85c333df47e9e796 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 30 May 2026 18:37:57 -0400 Subject: [PATCH 72/97] FEAT: ll pose logging --- .../subsystems/vision/LimelightVision.java | 30 ++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 1132fc67..e503b00a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -29,6 +29,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -64,9 +66,9 @@ public static LimelightVision getInstance() { private Pose2d[] limelightPoseArray; - // private StructPublisher leftLimelightPosePublisher; - // private StructPublisher rightLimelightPosePublisher; - // private StructPublisher backLimelightPosePublisher; + private StructPublisher leftLimelightPosePublisher; + private StructPublisher rightLimelightPosePublisher; + private StructPublisher backLimelightPosePublisher; private boolean hasData; private BStream debouncedHasData; @@ -87,9 +89,9 @@ public void setPipeline(Pipeline pipeline) { public LimelightVision() { limelightPoseArray = new Pose2d[Cameras.LimelightCameras.length]; - // leftLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Left", Pose2d.struct).publish(); - // rightLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Right", Pose2d.struct).publish(); - // backLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Back", Pose2d.struct).publish(); + leftLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Left", Pose2d.struct).publish(); + rightLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Right", Pose2d.struct).publish(); + backLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Back", Pose2d.struct).publish(); names = new String[Cameras.LimelightCameras.length]; @@ -364,14 +366,14 @@ public void periodicAfterScheduler() { DogLog.log("Vision/Pose Estimate Y " + limelightName, poseEstimate.pose.getY()); DogLog.log("Vision/Pose Estimate Theta " + limelightName, poseEstimate.pose.getRotation().getDegrees()); - // switch (limelightName) { - // case "limelight-right" -> - // rightLimelightPosePublisher.set(robotPose); - // case "limelight-left" -> - // leftLimelightPosePublisher.set(robotPose); - // case "limelight-back" -> - // backLimelightPosePublisher.set(robotPose); - // } + switch (limelightName) { + case "limelight-right" -> + rightLimelightPosePublisher.set(robotPose); + case "limelight-left" -> + leftLimelightPosePublisher.set(robotPose); + case "limelight-back" -> + backLimelightPosePublisher.set(robotPose); + } DogLog.log("Vision/" + names[i] + " Has Data", true); From 0ecc97d3ed8b29fc318a83ff513f8f95d3f3b9ec Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 30 May 2026 18:38:22 -0400 Subject: [PATCH 73/97] FEAT: upped intake torque currents for auto and tele --- src/main/java/com/stuypulse/robot/constants/Settings.java | 4 ++-- .../com/stuypulse/robot/subsystems/intake/IntakeImpl.java | 8 ++++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 439fa2cc..6d7aa3ef 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -77,8 +77,8 @@ public interface Intake { double HOMING_VOLTAGE = 3.0; double PUSHDOWN_VOLTAGE = -3.0; - double PUSHDOWN_CURRENT_TELEOP = -75.0;//new SmartNumber("Intake/Pushdown Current", -65.0); //TODO: GET ACTUAL TYTY - double PUSHDOWN_CURRENT_AUTON = -95.0; + double PUSHDOWN_CURRENT_TELEOP = -55.0;//new SmartNumber("Intake/Pushdown Current", -65.0); //TODO: GET ACTUAL TYTY + double PUSHDOWN_CURRENT_AUTON = -80.0; double GEAR_RATIO = 32.0/20.0 * 64.0/18.0 * 60.0/8.0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 330d5fa1..054052bf 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -59,6 +59,7 @@ public class IntakeImpl extends Intake { private final BStream isPivotBelowPushDownThreshold; StatusSignal pivotSupplyCurrent; + StatusSignal pivotTorqueCurrent; StatusSignal pivotStatorCurrent; StatusSignal rollerLeaderSupplyCurrent; StatusSignal rollerLeaderStatorCurrent; @@ -77,7 +78,7 @@ public IntakeImpl() { .withInvertedValue(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) - .withSupplyCurrentLimitAmps(10.0) // was 60 on practice day + .withSupplyCurrentLimitAmps(20.0) // was 60 on practice day .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) @@ -121,6 +122,7 @@ public IntakeImpl() { pivotSupplyCurrent = pivot.getSupplyCurrent(); pivotStatorCurrent = pivot.getStatorCurrent(); + pivotTorqueCurrent = pivot.getTorqueCurrent(); pivotMotorPosition = pivot.getPosition(); rollerLeaderSupplyCurrent = rollerLeader.getSupplyCurrent(); rollerLeaderStatorCurrent = rollerLeader.getStatorCurrent(); @@ -135,7 +137,7 @@ public IntakeImpl() { PhoenixUtil.registerToRio(pivotSupplyCurrent, pivotStatorCurrent, pivotMotorPosition, rollerLeaderSupplyCurrent, rollerLeaderStatorCurrent, rollerFollowerSupplyCurrent, rollerFollowerStatorCurrent, rollerLeaderTemperature, rollerFollowerTemperature, pivotTemperature, pivotMotorVoltage, - rollerLeaderVoltage, rollerFollowerVoltage); + rollerLeaderVoltage, rollerFollowerVoltage, pivotTorqueCurrent); pivotStalling = BStream.create( () -> Math.abs(pivotSupplyCurrent.getValueAsDouble()) > Settings.Intake.PIVOT_STALL_CURRENT) @@ -264,6 +266,7 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge rollerFollowerSupplyCurrent.getValueAsDouble()); DogLog.log("Intake/Roller Follower Stator Current (amps)", rollerFollowerStatorCurrent.getValueAsDouble()); + // Pivot DogLog.log("Intake/Pivot Voltage (volts)", pivotMotorVoltage.getValueAsDouble()); @@ -272,6 +275,7 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge DogLog.log("Intake/Pivot Stator Current (amps)", pivotStatorCurrent.getValueAsDouble()); DogLog.log("Intake/Pivot is below pushdown Threshold", isPivotBelowPushDownThreshold.get()); + DogLog.log("Intake/Pivot Torque Current", pivotTorqueCurrent.getValueAsDouble()); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log("Robot/CAN/Main/Intake Pivot Motor Connected? (ID " From 465cde109ccd769181b135d41926ddb3775a9120 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Sat, 30 May 2026 18:40:38 -0400 Subject: [PATCH 74/97] feat: slow down on turns where we get beached due to intake design --- .../paths/BC Left Score To Score NY.path | 42 ++++++++++++++++++- .../paths/BC Right Score To Score NY.path | 42 ++++++++++++++++++- 2 files changed, 82 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path b/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path index 2e1450ae..f4d10dba 100644 --- a/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path +++ b/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path @@ -102,7 +102,47 @@ "rotationDegrees": 0.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.4, + "maxWaypointRelativePos": 0.7, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.2, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.67, + "maxWaypointRelativePos": 1.92, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path b/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path index 0f6099e8..c9fbf182 100644 --- a/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path +++ b/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path @@ -102,7 +102,47 @@ "rotationDegrees": 0.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.4, + "maxWaypointRelativePos": 0.7, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.25, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.67, + "maxWaypointRelativePos": 1.92, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { From 007b0d00045f866ef9374060da21b28703003107 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 30 May 2026 20:04:25 -0400 Subject: [PATCH 75/97] feat: log camera pose estimates --- src/main/java/com/stuypulse/robot/Robot.java | 4 +- .../com/stuypulse/robot/RobotContainer.java | 12 +-- .../subsystems/vision/LimelightVision.java | 79 ++++++++++--------- 3 files changed, 50 insertions(+), 45 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 9912027a..488503f6 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -224,7 +224,7 @@ public void autonomousInit() { mode = RobotMode.AUTON; CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); - CommandScheduler.getInstance().schedule(new InstantCommand(() -> Intake.getInstance().autonInit(), Intake.getInstance())); + // CommandScheduler.getInstance().schedule(new InstantCommand(() -> Intake.getInstance().autonInit(), Intake.getInstance())); auto = robot.getAutonomousCommand(); @@ -259,7 +259,7 @@ public void teleopInit() { CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); CommandScheduler.getInstance().schedule(new IntakeDeploy()); - CommandScheduler.getInstance().schedule(new InstantCommand(() -> Intake.getInstance().teleopInit(), Intake.getInstance())); + // CommandScheduler.getInstance().schedule(new InstantCommand(() -> Intake.getInstance().teleopInit(), Intake.getInstance())); if (auto != null) { auto.cancel(); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 203b9617..fa7025db 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -426,11 +426,11 @@ public void configureAutons() { // TWO CYCLES (CORNER) AutonConfig L_CN_FN = new AutonConfig("Left Corner-Near Far-Near", LeftTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite Anti Collision", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); L_CN_FN.register(autonChooser); AutonConfig R_CN_FN = new AutonConfig("Right Corner-Near Far-Near", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite Anti Collision", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); R_CN_FN.register(autonChooser); AutonConfig L_FNS_FN = new AutonConfig("Left Far-Near Shallow Far-Near", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, @@ -442,11 +442,11 @@ public void configureAutons() { R_FNS_FN.register(autonChooser); AutonConfig L_CN_NF = new AutonConfig("Left Corner-Near Near-Far", LeftTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite Anti Collision", "Left NZ To Score", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); L_CN_NF.register(autonChooser); AutonConfig R_CN_NF = new AutonConfig("Right Corner-Near Near-Far", RightTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite Anti Collision", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); R_CN_NF.register(autonChooser); AutonConfig L_CNL_D = new AutonConfig("Left Corner-Near Long Dot", ShallowSwipeDot::new, @@ -460,11 +460,11 @@ public void configureAutons() { R_CNL_D.register(autonChooser); AutonConfig R_CN_NFS = new AutonConfig("Right Corner-Near Near-Far-Short", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite Anti Collision", "Right NZ To Score", "BC Right Score To Score NY", "Right Score To Corner", "Right Score To NZ (F)"); + "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "BC Right Score To Score NY", "Right Score To Corner", "Right Score To NZ (F)"); R_CN_NFS.register(autonChooser); AutonConfig L_CN_NFS = new AutonConfig("Left Corner-Near Near-Far-Short", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite Anti Collision", "Left NZ To Score", "BC Left Score To Score NY", "Left Score To Corner", "Left Score To NZ (F)"); + "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "BC Left Score To Score NY", "Left Score To Corner", "Left Score To NZ (F)"); L_CN_NFS.register(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index e503b00a..13df4ebe 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -1,7 +1,7 @@ /** ********************** PROJECT TRIBECBOT ************************ */ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ - /* Use of this source code is governed by an MIT-style license */ - /* that can be found in the repository LICENSE file. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ /** ************************************************************ */ package com.stuypulse.robot.subsystems.vision; @@ -52,23 +52,23 @@ public static LimelightVision getInstance() { private int maxTagCount; private MegaTagMode megaTagMode; - private double leftLLHeartbeat = -1; //change to -1 is we need to switch the way we do this + private double leftLLHeartbeat = -1; // change to -1 is we need to switch the way we do this private double rightLLHeartbeat = -1; private double backLLHeartbeat = -1; - private double prevLeftLLLatency = -1; //change to -1 is we need to switch the way we do this + private double prevLeftLLLatency = -1; // change to -1 is we need to switch the way we do this private double prevRightLLLatency = -1; private double prevBackLLLatency = -1; private int leftLoopCounter = 0; private int rightLoopCounter = 0; - private int backLoopCounter = 0; + private int backLoopCounter = 0; private Pose2d[] limelightPoseArray; - private StructPublisher leftLimelightPosePublisher; - private StructPublisher rightLimelightPosePublisher; - private StructPublisher backLimelightPosePublisher; + // private StructPublisher leftLimelightPosePublisher; + // private StructPublisher rightLimelightPosePublisher; + // private StructPublisher backLimelightPosePublisher; private boolean hasData; private BStream debouncedHasData; @@ -82,16 +82,22 @@ public enum MegaTagMode { } public void setPipeline(Pipeline pipeline) { - for(Camera camera: Cameras.LimelightCameras) { + for (Camera camera : Cameras.LimelightCameras) { camera.setPipeline(pipeline); } } public LimelightVision() { limelightPoseArray = new Pose2d[Cameras.LimelightCameras.length]; - leftLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Left", Pose2d.struct).publish(); - rightLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Right", Pose2d.struct).publish(); - backLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Back", Pose2d.struct).publish(); + // leftLimelightPosePublisher = + // NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Left", + // Pose2d.struct).publish(); + // rightLimelightPosePublisher = + // NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Right", + // Pose2d.struct).publish(); + // backLimelightPosePublisher = + // NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Back", + // Pose2d.struct).publish(); names = new String[Cameras.LimelightCameras.length]; @@ -107,8 +113,7 @@ public LimelightVision() { robotRelativePose.getZ(), Units.radiansToDegrees(robotRelativePose.getRotation().getX()), Units.radiansToDegrees(robotRelativePose.getRotation().getY()), - Units.radiansToDegrees(robotRelativePose.getRotation().getZ()) - ); + Units.radiansToDegrees(robotRelativePose.getRotation().getZ())); limelightPoseArray[i] = new Pose2d(); } @@ -160,33 +165,36 @@ public void setIMUMode(int mode) { * gyro. * * @param assistValue, an double that sets the correction speed of the - * complementary filter for the IMU. IMU Mode 4 uses the fusing of the - * internal IMU (1khz) with the external gyro reading as well. Higher values - * ranging towards 1 indicate a faster convergence of internal IMU to the - * robot IMU mode. Defaults to 0.001. + * complementary filter for the IMU. IMU Mode 4 uses the + * fusing of the + * internal IMU (1khz) with the external gyro reading as + * well. Higher values + * ranging towards 1 indicate a faster convergence of + * internal IMU to the + * robot IMU mode. Defaults to 0.001. */ public void setIMUAssistValue(double assistValue) { for (String name : names) { LimelightHelpers.SetIMUAssistAlpha(name, assistValue); } - } + } /** * Allows all tags except the specified ones by setting blacklisted tag * indexes to -1 in the full tag list before applying the new list. * * @param tagsToBlacklist array of tag IDs to exclude from detection - * @param limelight the name of the Limelight camera to configure + * @param limelight the name of the Limelight camera to configure */ public void setTagBlacklist(int[] tagsToBlacklist, String limelight) { int[] allTags = Field.ALL_TAGS.clone(); - + for (int i = 0; i < tagsToBlacklist.length; i++) { allTags[tagsToBlacklist[i] - 1] = -1; } int[] validTags = new int[allTags.length - tagsToBlacklist.length]; - + int counter = 0; for (int i = 0; i < allTags.length; i++) { if (allTags[i] != -1) { @@ -206,7 +214,7 @@ public void setTagBlacklist(int[] tagsToBlacklist, String limelight) { * as the whitelist. * * @param tagsToWhitelist array of tag IDs to allow for detection - * @param limelight the name of the Limelight camera to configure + * @param limelight the name of the Limelight camera to configure */ public void setTagWhitelist(int[] tagsToWhitelist, String limelight) { LimelightHelpers.SetFiducialIDFiltersOverride(limelight, tagsToWhitelist); @@ -307,16 +315,19 @@ public void periodicAfterScheduler() { ); PoseEstimate poseEstimate; - - // MegaTag switching - if (megaTagMode == MegaTagMode.MEGATAG1) { - poseEstimate = Robot.isBlue() + PoseEstimate poseEstimateMT1 = Robot.isBlue() ? LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName) : LimelightHelpers.getBotPoseEstimate_wpiRed(limelightName); - } else { - poseEstimate = Robot.isBlue() + + PoseEstimate poseEstimateMT2 = Robot.isBlue() ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName) : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(limelightName); + + // MegaTag switching + if (megaTagMode == MegaTagMode.MEGATAG1) { + poseEstimate = poseEstimateMT1; + } else { + poseEstimate = poseEstimateMT2; } @@ -366,14 +377,8 @@ public void periodicAfterScheduler() { DogLog.log("Vision/Pose Estimate Y " + limelightName, poseEstimate.pose.getY()); DogLog.log("Vision/Pose Estimate Theta " + limelightName, poseEstimate.pose.getRotation().getDegrees()); - switch (limelightName) { - case "limelight-right" -> - rightLimelightPosePublisher.set(robotPose); - case "limelight-left" -> - leftLimelightPosePublisher.set(robotPose); - case "limelight-back" -> - backLimelightPosePublisher.set(robotPose); - } + DogLog.log("Vision/" + Cameras.LimelightCameras[i].getName() + "/Pose MT1", poseEstimateMT1.pose); + DogLog.log("Vision/" + Cameras.LimelightCameras[i].getName() + "/Pose MT2", poseEstimateMT2.pose); DogLog.log("Vision/" + names[i] + " Has Data", true); From 134473bafcdcbec17d5097a789b5cd74e8883bf1 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sun, 31 May 2026 08:53:36 -0400 Subject: [PATCH 76/97] feat: log auton played --- .../autos/Left Anti Colliding NY.auto | 43 +++++++++++++++++++ .../com/stuypulse/robot/RobotContainer.java | 14 +++--- 2 files changed, 51 insertions(+), 6 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Left Anti Colliding NY.auto diff --git a/src/main/deploy/pathplanner/autos/Left Anti Colliding NY.auto b/src/main/deploy/pathplanner/autos/Left Anti Colliding NY.auto new file mode 100644 index 00000000..e8d8f532 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Anti Colliding NY.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Corner Bite Anti Collision" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score Anti Collision" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Left Score To Score NY" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index fa7025db..90dcb7a4 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -520,7 +520,6 @@ public void configureAutons() { PATH_FIND_TEST.register(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); - } public boolean hasWaitTimeOneChanged() { @@ -591,12 +590,15 @@ public void configureSysids() { } public Command getAutonomousCommand() { - if (autonChooser.getSelected() == null) { - return new DoNothingAuton(); - } - else { - return autonChooser.getSelected(); + Command autonCommand = autonChooser.getSelected(); + + if (autonCommand == null) { + autonCommand = new DoNothingAuton(); } + + DogLog.log("Auton/Selected", autonCommand.getName()); + + return autonCommand; } public static double getWaitTimeOne() { From 8c6a766e9f8d2aa59ad9f65b088dce3ba9648e8c Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sun, 31 May 2026 11:30:24 -0400 Subject: [PATCH 77/97] feat: readd cameras.log, remove manual corner shot rehead --- src/main/java/com/stuypulse/robot/RobotContainer.java | 4 ++-- .../stuypulse/robot/subsystems/vision/LimelightVision.java | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 90dcb7a4..1f2c15f5 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -304,7 +304,7 @@ private void configureButtonBindings() { new SuperstructureLeftCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()) .andThen(new SpindexerRun()), - new SwerveResetPoseLeftCorner(), + // new SwerveResetPoseLeftCorner(), new SwerveXMode() ) ) @@ -315,7 +315,7 @@ private void configureButtonBindings() { // .whileTrue(new LEDApplyPattern(Settings.LED.RIGHT_CORNER)) .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) - .onTrue(new SwerveResetPoseRightCorner()) + // .onTrue(new SwerveResetPoseRightCorner()) .whileTrue(new SuperstructureRightCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD) .andThen(new SpindexerRun()))) diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 13df4ebe..346113df 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -393,6 +393,8 @@ public void periodicAfterScheduler() { // this is just the yaw of the internal imu DogLog.log("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); DogLog.log("Vision/latency_pipeline " + limelightName , LimelightHelpers.getLatency_Pipeline(limelightName)); + + Cameras.LimelightCameras[i].log(); } } From 9237e00167ef1e8aa3a4f19b6279eb5fd7fbc5bf Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sun, 31 May 2026 11:44:19 -0400 Subject: [PATCH 78/97] REFACTOR: Change intake deploy color do purple --- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 6d7aa3ef..fb132238 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -416,7 +416,7 @@ public static RGBWColor rgbwConverter(Color color) { RGBWColor X_WHEELS = rgbwConverter(Color.kRed); RGBWColor INTAKE_STOW = rgbwConverter(Color.kBrown); //broken - RGBWColor INTAKE_DEPLOYED = rgbwConverter(Color.kGray); //broken + RGBWColor INTAKE_DEPLOYED = rgbwConverter(Color.kPurple); //broken RGBWColor DISABLED_ALIGNED = rgbwConverter(Color.kGreen); RGBWColor DISABLED = rgbwConverter(Color.kRed); From 2d655d6827bb75f9912edd800d7791671348d9fe Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sun, 31 May 2026 14:28:08 -0400 Subject: [PATCH 79/97] FEAT: Match 13. Made it such so we outtake for 0.2 seconds before moving on to Score to Score (2nd bite) --- .../commands/auton/regular/LeftTwoCornerShallow.java | 8 +++++++- .../commands/auton/regular/RightTwoCornerShallow.java | 8 +++++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java index 6c2a3d86..ed344397 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java @@ -11,12 +11,15 @@ import com.stuypulse.robot.commands.intake.IntakeAutoDigest; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeDigest; +import com.stuypulse.robot.commands.intake.IntakeOuttake; +import com.stuypulse.robot.commands.intake.IntakeSetState; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -61,7 +64,10 @@ public LeftTwoCornerShallow(PathPlannerPath... paths) { new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + new SuperstructureAutoInterpolation().alongWith(new IntakeSetState(PivotState.DEPLOY)), + new IntakeOuttake(), + new WaitCommand(0.2), + new IntakeDeploy(), // NZ Trip 2 new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java index fe74ea44..24e84ad9 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java @@ -11,12 +11,15 @@ import com.stuypulse.robot.commands.intake.IntakeAutoDigest; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeDigest; +import com.stuypulse.robot.commands.intake.IntakeOuttake; +import com.stuypulse.robot.commands.intake.IntakeSetState; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -61,7 +64,10 @@ public RightTwoCornerShallow(PathPlannerPath... paths) { new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + new SuperstructureAutoInterpolation().alongWith(new IntakeSetState(PivotState.DEPLOY)), + new IntakeOuttake(), + new WaitCommand(0.2), + new IntakeDeploy(), // NZ Trip 2 new ParallelCommandGroup( From f6765d726c2c0ff7db34ed55e9d444a5101bc31d Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sun, 31 May 2026 15:40:20 -0400 Subject: [PATCH 80/97] FEAT: LL cpu logging --- src/main/java/com/stuypulse/robot/constants/Cameras.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index b9668aed..6cff31a4 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -144,6 +144,7 @@ public void log() { DogLog.log(keyName + "Heartbeat", LimelightHelpers.getHeartbeat(name)); DogLog.log(keyName + "Temp (C)", LimelightHelpers.getLimelightDoubleArrayEntry(name, "hw").get()); + DogLog.log(keyName + "cpu usage", LimelightHelpers.getLimelightDoubleArrayEntry(name, "cpu").get()); DogLog.log(keyName + "Pose MT1", (Robot.isBlue() ? LimelightHelpers.getBotPoseEstimate_wpiBlue(name).pose : LimelightHelpers.getBotPoseEstimate_wpiRed(name).pose)); From a679dba3e839dc35e222c1aedb6a9b9552c9908a Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sun, 31 May 2026 16:43:21 -0400 Subject: [PATCH 81/97] FEAT: shortend 1'st path --- .../paths/Left Corner Bite Anti Collision.path | 4 ++-- .../paths/Left NZ To Score Anti Collision.path | 6 +++--- .../paths/Right Corner Bite Anti Collision.path | 8 ++++---- .../paths/Right NZ To Score Anti Collision.path | 8 ++++---- 4 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path b/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path index 61414bc9..27f664c2 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path @@ -17,11 +17,11 @@ { "anchor": { "x": 7.764, - "y": 4.902 + "y": 4.978 }, "prevControl": { "x": 7.267033333333333, - "y": 7.864311111111112 + "y": 7.940311111111112 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path b/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path index f11d066a..f4c7c772 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path @@ -4,12 +4,12 @@ { "anchor": { "x": 7.764, - "y": 4.902 + "y": 4.978 }, "prevControl": null, "nextControl": { "x": 6.117188888888889, - "y": 4.853277777777779 + "y": 4.929277777777779 }, "isLocked": false, "linkedName": "L-Anti-Collision" @@ -36,7 +36,7 @@ "y": 7.440906488549619 }, "prevControl": { - "x": 6.4716451806743285, + "x": 6.471645180674328, "y": 7.4831512540767555 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path b/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path index 124bc038..97fcc686 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.637, - "y": 3.235955555555555 + "x": 7.631, + "y": 3.16 }, "prevControl": { - "x": 7.675977777777777, - "y": 1.9302000000000006 + "x": 7.669977777777778, + "y": 1.8542444444444457 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path b/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path index 9eda0bbc..7bf6063d 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.637, - "y": 3.235955555555555 + "x": 7.631, + "y": 3.16 }, "prevControl": null, "nextControl": { - "x": 5.9317817608038474, - "y": 3.1385145133157755 + "x": 5.925781760803848, + "y": 3.0625589577602206 }, "isLocked": false, "linkedName": "R-Anti-Collision" From ea20c76d9c12714c38a90858db6c10b62800064d Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Thu, 28 May 2026 21:59:19 -0400 Subject: [PATCH 82/97] feat: cleaned up LED code. Cleaned up LimelightVision and added methods to Cameras as a result. Removed existing and incomplete check for distance from last pose when seeing April tags. --- .../stuypulse/robot/constants/Cameras.java | 35 +++- .../robot/subsystems/leds/LEDController.java | 156 +++++++----------- .../subsystems/vision/LimelightVision.java | 87 ++-------- 3 files changed, 110 insertions(+), 168 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 6cff31a4..832ce31f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -7,6 +7,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.robot.util.vision.LimelightHelpers.LimelightResults; import com.stuypulse.robot.util.vision.LimelightHelpers.RawFiducial; @@ -45,14 +46,14 @@ public static class Camera { private SmartBoolean isEnabled; private String keyName; + private double LLHeartbeat = -1; + private int loopCounter = 0; + private int rejectedCounterNotNull; private int rejectedCounterAngularVelocity; private int rejectedCounterInvalidPosition; private int rejectedCounterTargetArea; - // private boolean isDead; - // private double heartBeat; - private LimelightResults result; private Pipeline currentPipeline; @@ -124,6 +125,34 @@ public int getNumberOfTagsSeen() { return LimelightHelpers.getRawFiducials(this.getName()).length; } + public void updateHeartBeat() { + LLHeartbeat = LimelightHelpers.getHeartbeat(this.getName()); + } + + public void incrementLoopCounter() { + loopCounter += 1; + } + + public boolean isAlive() { + //latest - old heartbeat + if (LimelightHelpers.getHeartbeat(this.getName()) - LLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && LLHeartbeat != -1) { + return false; + } + else { + return true; + } + } + + public void updateLEDs() { + switch (this.getName()) { + case "limelight-right" -> { if (!isAlive()) LEDController.isRightLLDead = true; } + + case "limelight-left" -> { if (!isAlive()) LEDController.isLeftLLDead = true; } + + case "limelight-back" -> { if (!isAlive()) LEDController.isBackLLDead = true; } + } + } + // public boolean seesTag() { // return getNumberOfTagsSeen() == 0; // } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 1d7c4e6a..603f5d48 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -17,27 +17,25 @@ import com.ctre.phoenix6.signals.StatusLedWhenActiveValue; import com.ctre.phoenix6.signals.StripTypeValue; import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import dev.doglog.DogLog; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDController extends SubsystemBase { private final static LEDController instance; - public static boolean isLeftLLDead = false; - public static boolean isBackLLDead = false; - public static boolean isRightLLDead = false; - // public static boolean isLeftLLDeadControlApplied; - // public static boolean isBackLLDeadControlApplied; - // public static boolean isRightLLDeadControlApplied; + public static boolean isLeftLLDead; + public static boolean isBackLLDead; + public static boolean isRightLLDead; +; + public boolean leftDeadAnimationCleared; + public boolean backDeadAnimationCleared; + public boolean rightDeadAnimationCleared; - private Pose2d lastPoseOnAprilTag; - private boolean initialPoseUpdated = false; + // private Pose2d lastPoseOnAprilTag; + // private boolean initialPoseUpdated = false; static { instance = new LEDController(); @@ -51,11 +49,34 @@ public static LEDController getInstance() { private CANdleConfiguration candleConfigs; private ControlRequest ledPattern = Settings.LED.solidColorRequest.withColor(Settings.LED.DISABLED); - // different portions of the LED should be a different color to indicate whether - // certain limelights are dead - // add the flashing aspect based on if we don't see a tag (with debounce) - // one way to go further with the flashing aspect is make it flash faster over - // DISTANCE (since last tag was seen) rather than time + private LEDController() { + leftDeadAnimationCleared = false; + backDeadAnimationCleared = false; + rightDeadAnimationCleared = false; + + isLeftLLDead = false; + isBackLLDead = false; + isRightLLDead = false; + + leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); + // lastPoseOnAprilTag = new Pose2d(); + + candleConfigs = new CANdleConfiguration() + .withLED( + new LEDConfigs() + .withBrightnessScalar(1.0) + .withStripType(StripTypeValue.GRB) + .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)) + + .withCANdleFeatures( + new CANdleFeaturesConfigs().withStatusLedWhenActive(StatusLedWhenActiveValue.Enabled)); + + leds.getConfigurator().apply(candleConfigs); + + leds.setControl(ledPattern); + } + + // add the flashing aspect based on if we don't see a tag (with debounce or by distance) public enum LedState { PASSING_TRENCH(Settings.LED.PASSING_TRENCH), @@ -93,23 +114,18 @@ public ControlRequest getAnimation() { private LedState state = LedState.DISABLED; private LedState cachedState = LedState.DISABLED; - // CHANGE apply pattern command to change state - + //TODO: make branch for the distance flashing thing public void applyPattern() { + // if (initialPoseUpdated && + // lastPoseOnAprilTag.getTranslation() + // .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()) > Settings.LED.APRIL_TAG_DISTANCE_THRESHOLD) { + // + // } + if (cachedState != state) { - // if (cachedState.getAnimation().getName() != "SolidColor") { - // leds.clearAllAnimations(); - // } - // clearAllAnimations every loop - // if (!(cachedState.getAnimation().getName().equals(state.getAnimation().getName()))) { - // this.ledPattern = state.getAnimation(); - // } - - // else if (ledPattern instanceof SolidColor) { - SolidColor solidColor = (SolidColor) ledPattern; - solidColor.withColor(state.getColor()); - // SolidColor.class.cast(ledPattern).withColor(null); //change if neccesary - // } + SolidColor solidColor = (SolidColor) ledPattern; + solidColor.withColor(state.getColor()); + cachedState = state; } } @@ -118,31 +134,15 @@ public void changeState(LedState state) { this.state = state; } - private LEDController() { - - // isLeftLLDeadControlApplied= false; - // isBackLLDeadControlApplied= false; - // isRightLLDeadControlApplied = false; - - leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); - lastPoseOnAprilTag = new Pose2d(); - - candleConfigs = new CANdleConfiguration() - .withLED( - new LEDConfigs() - .withBrightnessScalar(1.0) - .withStripType(StripTypeValue.GRB) - .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)) - - .withCANdleFeatures( - new CANdleFeaturesConfigs().withStatusLedWhenActive(StatusLedWhenActiveValue.Enabled)); - - leds.getConfigurator().apply(candleConfigs); - - leds.setControl(ledPattern); - } public void periodicAfterScheduler() { + // if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || + // Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || + // Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { + // lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); + // initialPoseUpdated = true; + // } + if (RobotContainer.EnabledSubsystems.LEDS.get()) { applyPattern(); leds.setControl(ledPattern); @@ -150,58 +150,30 @@ public void periodicAfterScheduler() { leds.clearAllAnimations(); } - // reflective of the 3 LED gap between them + //deadAnimationClear booleans ensure we aren't clearing animations 3 times per loop. if (isRightLLDead) { - //TODO: when it goes back on CLEAR ANIMATIONS !! leds.setControl(Settings.LED.RIGHT_DEAD_STRIP .withColor(Settings.LED.LLDEAD)); - // isRightLLDeadControlApplied = true; - } else if (!isRightLLDead /*&& isRightLLDeadControlApplied*/) { + rightDeadAnimationCleared = false; + } else if (!isRightLLDead && !rightDeadAnimationCleared) { leds.clearAllAnimations(); - //isRightLLDeadControlApplied = false; + rightDeadAnimationCleared = true; } if (isLeftLLDead) { leds.setControl(Settings.LED.LEFT_DEAD_STRIP .withColor(Settings.LED.LLDEAD)); - //isLeftLLDeadControlApplied = true; - } else if (!isLeftLLDead /*&& isLeftLLDeadControlApplied */) { + leftDeadAnimationCleared = false; + } else if (!isLeftLLDead && !leftDeadAnimationCleared) { leds.clearAllAnimations(); - //isLeftLLDeadControlApplied = false; + leftDeadAnimationCleared = true; } if (isBackLLDead) { leds.setControl(Settings.LED.BACK_DEAD_STRIP .withColor(Settings.LED.LLDEAD)); - //isBackLLDeadControlApplied = true; - } else if (!isBackLLDead /*&& isBackLLDeadControlApplied*/) { + backDeadAnimationCleared = false; + } else if (!isBackLLDead && !backDeadAnimationCleared) { leds.clearAllAnimations(); - //isBackLLDeadControlApplied = false; - } - - if (isBackLLDead || isLeftLLDead || isRightLLDead) { - leds.setControl(Settings.LED.CANDLE_DEAD_STRIP - .withColor(Settings.LED.LLDEAD)); - //isBackLLDeadControlApplied = true; - } else if (!(isBackLLDead || isLeftLLDead || isRightLLDead) /*&& isBackLLDeadControlApplied*/) { - leds.clearAllAnimations(); - //isBackLLDeadControlApplied = false; - } - - - - if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || - Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || - Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { - lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); - initialPoseUpdated = true; - } - else { - - } - - if (initialPoseUpdated && - lastPoseOnAprilTag.getTranslation() - .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()) > Settings.LED.APRIL_TAG_DISTANCE_THRESHOLD) { - //TODO: add flashing + backDeadAnimationCleared = true; } DogLog.log("LED/Applied Pattern Name", ledPattern.getName()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 346113df..b004bf4a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -5,7 +5,6 @@ /** ************************************************************ */ package com.stuypulse.robot.subsystems.vision; -import java.sql.ResultSet; import java.util.Arrays; import com.stuypulse.robot.Robot; @@ -15,11 +14,9 @@ import com.stuypulse.robot.constants.Cameras.Camera.RejectionValue; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.robot.util.vision.LimelightHelpers.IMUData; -import com.stuypulse.robot.util.vision.LimelightHelpers.LimelightResults; import com.stuypulse.robot.util.vision.LimelightHelpers.PoseEstimate; import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.streams.booleans.BStream; @@ -29,10 +26,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LimelightVision extends SubsystemBase { @@ -52,18 +46,10 @@ public static LimelightVision getInstance() { private int maxTagCount; private MegaTagMode megaTagMode; - private double leftLLHeartbeat = -1; // change to -1 is we need to switch the way we do this - private double rightLLHeartbeat = -1; - private double backLLHeartbeat = -1; - private double prevLeftLLLatency = -1; // change to -1 is we need to switch the way we do this private double prevRightLLLatency = -1; private double prevBackLLLatency = -1; - private int leftLoopCounter = 0; - private int rightLoopCounter = 0; - private int backLoopCounter = 0; - private Pose2d[] limelightPoseArray; // private StructPublisher leftLimelightPosePublisher; @@ -246,65 +232,20 @@ public void periodicAfterScheduler() { DogLog.log("LED/heartbeat" + limelightName, LimelightHelpers.getHeartbeat(limelightName)); - if (limelightName.equals(Cameras.LimelightCameras[0].getName())) { - DogLog.log("LED/Right Loop Counter", rightLoopCounter); - DogLog.log("LED/variable heartbeat " + limelightName, rightLLHeartbeat); - rightLoopCounter += 1; - if (rightLoopCounter == 50) { - DogLog.log("LED/Right Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat); - boolean isDeadLatency = (prevRightLLLatency == LimelightHelpers.getLatency_Pipeline(limelightName)); - if (LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1 || isDeadLatency) { - LEDController.isRightLLDead = true; - } - else { - LEDController.isRightLLDead = false; - } - rightLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); - prevRightLLLatency = LimelightHelpers.getLatency_Pipeline(limelightName); - rightLoopCounter = 0; - DogLog.log("Vision/" + limelightName +"/is dead by latency", isDeadLatency); - } - } - if (limelightName.equals(Cameras.LimelightCameras[1].getName())) { - DogLog.log("LED/Left Loop Counter", leftLoopCounter); - DogLog.log("LED/variable heartbeat " + limelightName, leftLLHeartbeat); - leftLoopCounter += 1; - if (leftLoopCounter == 50) { - DogLog.log("LED/Left Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat); - boolean isDeadLatency = (prevLeftLLLatency == LimelightHelpers.getLatency_Pipeline(limelightName)); - if (LimelightHelpers.getHeartbeat(limelightName) - leftLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1 || isDeadLatency) { - LEDController.isLeftLLDead = true; - } - else { - LEDController.isLeftLLDead = false; - } - leftLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); - leftLoopCounter = 0; - prevLeftLLLatency = LimelightHelpers.getLatency_Pipeline(limelightName); - DogLog.log("Vision/" + limelightName +"/is dead by latency", isDeadLatency); - } - } - if (limelightName.equals(Cameras.LimelightCameras[2].getName())) { - DogLog.log("LED/Back Loop Counter", backLoopCounter); - DogLog.log("LED/variable heartbeat " + limelightName, backLLHeartbeat); - backLoopCounter += 1; - if (backLoopCounter == 50) { - boolean isDeadLatency = (prevBackLLLatency == LimelightHelpers.getLatency_Pipeline(limelightName)); - DogLog.log("LED/Back Limelight HB Diff", LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat); - if (LimelightHelpers.getHeartbeat(limelightName) - backLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && backLLHeartbeat != -1 || isDeadLatency) { - LEDController.isBackLLDead = true; - } - else { - LEDController.isBackLLDead = false; - } - backLLHeartbeat = LimelightHelpers.getHeartbeat(limelightName); - backLoopCounter = 0; - prevBackLLLatency = LimelightHelpers.getLatency_Pipeline(limelightName); - DogLog.log("Vision/" + limelightName +"/is dead by latency", isDeadLatency); - } - } - // Seed robot heading (used by MT2) - LimelightHelpers.SetRobotOrientation( + Cameras.LimelightCameras[i].updateLEDs(); + //boolean isDeadLatency = (prevRightLLLatency == LimelightHelpers.getLatency_Pipeline(limelightName)); + //if (LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1 || isDeadLatency) { + // LEDController.isRightLLDead = true; + + // prevRightLLLatency = LimelightHelpers.getLatency_Pipeline(limelightName); + + //Ensure this is below updateLEDs(), otherwise the cameras will never appear as dead + Cameras.LimelightCameras[i].updateHeartBeat(); + Cameras.LimelightCameras[i].incrementLoopCounter(); + + + // Seed robot heading (used by MT2) + LimelightHelpers.SetRobotOrientation( limelightName, (CommandSwerveDrivetrain.getInstance().getPose().getRotation().getDegrees() + (Robot.isBlue() ? 0 : 180)) % 360, 0, From 6eb7c3eae5793f1cb83f5fb067441daad796aa2f Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Fri, 29 May 2026 18:19:40 -0400 Subject: [PATCH 83/97] feat: changes to apply state. Leds during auton to indicate path. Note for future: double check that the pathplannertrajectory list of poses is being printed on elastic --- src/main/java/com/stuypulse/robot/Robot.java | 8 +- .../auton/regular/CenterTwoCornerBC.java | 98 +++++++++---------- .../commands/auton/regular/TwoCornerBC.java | 31 +++--- .../robot/commands/leds/LEDApplyState.java | 20 ++-- .../stuypulse/robot/constants/Settings.java | 3 + .../robot/subsystems/leds/LEDController.java | 4 +- 6 files changed, 85 insertions(+), 79 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 488503f6..3e7338db 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -14,6 +14,7 @@ import com.pathplanner.lib.commands.PathfindingCommand; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.leds.LEDApplyState; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.swerve.SwerveAutonInit; @@ -22,7 +23,7 @@ import com.stuypulse.robot.commands.vision.SetMegaTagMode; import com.stuypulse.robot.commands.vision.WhitelistAllTagsForAllCameras; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.leds.LEDController.LedState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -45,7 +46,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.InstantCommand; public class Robot extends TimedRobot { @@ -259,7 +259,9 @@ public void teleopInit() { CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); CommandScheduler.getInstance().schedule(new IntakeDeploy()); - // CommandScheduler.getInstance().schedule(new InstantCommand(() -> Intake.getInstance().teleopInit(), Intake.getInstance())); + CommandScheduler.getInstance().schedule(new HandoffStop().alongWith(new SpindexerStop())); + //Reset LEDs from auton. InstantCommand to be removed + CommandScheduler.getInstance().schedule(new LEDApplyState(LedState.RESET).withTimeout(1.0)); if (auto != null) { auto.cancel(); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java index 88e18fcb..7a71dd94 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java @@ -1,8 +1,8 @@ -/************************ PROJECT TRIBECBOT *************************/ +/** ********************** PROJECT TRIBECBOT ************************ */ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ + /* Use of this source code is governed by an MIT-style license */ + /* that can be found in the repository LICENSE file. */ +/** ************************************************************ */ package com.stuypulse.robot.commands.auton.regular; import java.util.Set; @@ -13,72 +13,68 @@ import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeAutoDigest; import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.leds.LEDApplyState; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.subsystems.leds.LEDController.LedState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class CenterTwoCornerBC extends SequentialCommandGroup { - + public CenterTwoCornerBC(PathPlannerPath... paths) { addCommands( + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).deadlineFor( + new WaitCommand(0.2).andThen(new IntakeDeploy()), + new LEDApplyState(LedState.AUTON_COLOR_ONE) + ), + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).deadlineFor( + new SuperstructureAutoInterpolation(), + new LEDApplyState(LedState.AUTON_COLOR_TWO) + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).withTimeout(1.5).deadlineFor( + new LEDApplyState(LedState.AUTON_COLOR_ONE), + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest() + ),//.withTimeout(1.5), moved to the path up top, for the deadline for + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + // NZ Trip 2 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).deadlineFor( + new LEDApplyState(LedState.AUTON_COLOR_TWO), + new HandoffStop(), + new SpindexerStop() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new WaitCommand(5).deadlineFor( //deadline for accounts for LED Apply States + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new LEDApplyState(LedState.AUTON_COLOR_ONE), + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest() + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure SOTM is over - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - - Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.2).andThen(new IntakeDeploy()) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), - new SuperstructureSOTM(), - - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffRun(), - new SpindexerRun(), - new IntakeAutoDigest() - ).withTimeout(1.5), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), - - // NZ Trip 2 - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffStop(), - new SpindexerStop() - ), - - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffRun(), - new SpindexerRun(), - new IntakeAutoDigest(), - new WaitCommand(5) - ), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure SOTM is over - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), - - new SwerveXMode() + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).deadlineFor(new LEDApplyState(LedState.AUTON_COLOR_TWO)), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]).deadlineFor(new LEDApplyState(LedState.AUTON_COLOR_ONE)), + + new SwerveXMode() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java index 2c60bd45..386b0924 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java @@ -13,17 +13,18 @@ import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeAutoDigest; import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.leds.LEDApplyState; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.subsystems.leds.LEDController.LedState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; @@ -38,43 +39,45 @@ public TwoCornerBC(PathPlannerPath... paths) { Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.2).andThen(new IntakeDeploy()) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).deadlineFor( + new WaitCommand(0.2).andThen(new IntakeDeploy()), + new LEDApplyState(LedState.AUTON_COLOR_ONE) ), // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).deadlineFor( + new SuperstructureAutoInterpolation(), + new LEDApplyState(LedState.AUTON_COLOR_TWO) ), new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).withTimeout(1.5).deadlineFor( + new LEDApplyState(LedState.AUTON_COLOR_ONE), new HandoffRun(), new SpindexerRun(), new IntakeAutoDigest() - ).withTimeout(1.5), + ),//.withTimeout(1.5), moved to the path up top, for the deadline for new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).deadlineFor( + new LEDApplyState(LedState.AUTON_COLOR_TWO), new HandoffStop(), new SpindexerStop() ), new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( + new WaitCommand(5).deadlineFor( //deadline for accounts for LED Apply States CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new LEDApplyState(LedState.AUTON_COLOR_ONE), new HandoffRun(), new SpindexerRun(), - new IntakeAutoDigest(), - new WaitCommand(5) + new IntakeAutoDigest() ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure SOTM is over - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).deadlineFor(new LEDApplyState(LedState.AUTON_COLOR_TWO)), new SwerveXMode() ); diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java index 74f1ba0f..871dc98d 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java @@ -8,32 +8,32 @@ package com.stuypulse.robot.commands.leds; -import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -import java.util.ResourceBundle.Control; import java.util.function.Supplier; -import com.ctre.phoenix6.controls.ControlRequest; import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.leds.LEDController.LedState; -public class LEDApplyState extends InstantCommand { +import edu.wpi.first.wpilibj2.command.Command; + +public class LEDApplyState extends Command { //This will not work as default command will override it. Either make the cached class the same as this OR (better solution) have a boolean that tells you if it is manually applied or not and if it is then default command dont change protected final LEDController leds; - protected final LedState state; + protected final Supplier state; - public LEDApplyState(LedState state) { + public LEDApplyState(Supplier state) { leds = LEDController.getInstance(); this.state = state; addRequirements(leds); } + public LEDApplyState(LedState state) { + this(() -> state); + } + @Override public void execute() { - leds.changeState(state); + leds.changeState(state.get()); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index fb132238..d1f8d607 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -421,6 +421,9 @@ public static RGBWColor rgbwConverter(Color color) { RGBWColor DISABLED_ALIGNED = rgbwConverter(Color.kGreen); RGBWColor DISABLED = rgbwConverter(Color.kRed); + RGBWColor AUTON_ONE = rgbwConverter(Color.kBlue); + RGBWColor AUTON_TWO = rgbwConverter(Color.kOrange); + RGBWColor LLDEAD = rgbwConverter(Color.kWhite); SolidColor RIGHT_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 6, Settings.LED.LED_LENGTH - 2); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 603f5d48..e9621453 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -94,7 +94,9 @@ public enum LedState { INTAKE_STOW(Settings.LED.INTAKE_STOW), INTAKE_DEPLOYED(Settings.LED.INTAKE_DEPLOYED), DISABLED_ALIGNED(Settings.LED.DISABLED_ALIGNED), - DISABLED(Settings.LED.DISABLED); + DISABLED(Settings.LED.DISABLED), + AUTON_COLOR_ONE(Settings.LED.AUTON_ONE), + AUTON_COLOR_TWO(Settings.LED.AUTON_TWO); private RGBWColor color; From 4afcb50e781f8b58633beb1a393961372b802f9a Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Fri, 29 May 2026 19:03:55 -0400 Subject: [PATCH 84/97] feat: fix for LEDs - didn't check the loop counter or reset it (yikes) --- .../stuypulse/robot/constants/Cameras.java | 58 +++++++++++++++---- .../subsystems/vision/LimelightVision.java | 3 +- 2 files changed, 49 insertions(+), 12 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 832ce31f..5cb25070 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -135,21 +135,57 @@ public void incrementLoopCounter() { public boolean isAlive() { //latest - old heartbeat - if (LimelightHelpers.getHeartbeat(this.getName()) - LLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && LLHeartbeat != -1) { - return false; - } - else { - return true; - } + + if (LimelightHelpers.getHeartbeat(this.getName()) - LLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && LLHeartbeat != -1) { + return false; + } + else { + return true; + } + } public void updateLEDs() { switch (this.getName()) { - case "limelight-right" -> { if (!isAlive()) LEDController.isRightLLDead = true; } - - case "limelight-left" -> { if (!isAlive()) LEDController.isLeftLLDead = true; } - - case "limelight-back" -> { if (!isAlive()) LEDController.isBackLLDead = true; } + case "limelight-right" -> { + if (this.loopCounter == 50) { + if (!isAlive()) { + LEDController.isRightLLDead = true; + } + else { + LEDController.isRightLLDead = false; + } + + this.loopCounter = 0; + } + } + + case "limelight-left" -> { + if (this.loopCounter == 50) { + if (!isAlive()) { + LEDController.isLeftLLDead = true; + } + else { + LEDController.isLeftLLDead = false; + } + + this.loopCounter = 0; + } + } + + + case "limelight-back" -> { + if (this.loopCounter == 50) { + if (!isAlive()) { + LEDController.isBackLLDead = true; + } + else { + LEDController.isBackLLDead = false; + } + + this.loopCounter = 0; + } + } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index b004bf4a..7d618d38 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -232,6 +232,7 @@ public void periodicAfterScheduler() { DogLog.log("LED/heartbeat" + limelightName, LimelightHelpers.getHeartbeat(limelightName)); + Cameras.LimelightCameras[i].incrementLoopCounter(); Cameras.LimelightCameras[i].updateLEDs(); //boolean isDeadLatency = (prevRightLLLatency == LimelightHelpers.getLatency_Pipeline(limelightName)); //if (LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1 || isDeadLatency) { @@ -241,7 +242,7 @@ public void periodicAfterScheduler() { //Ensure this is below updateLEDs(), otherwise the cameras will never appear as dead Cameras.LimelightCameras[i].updateHeartBeat(); - Cameras.LimelightCameras[i].incrementLoopCounter(); + // Seed robot heading (used by MT2) From ac4e63d0b7e69027788a38eea9ac24c84afd4d98 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Wed, 3 Jun 2026 09:18:57 -0400 Subject: [PATCH 85/97] feat: added latency check back --- .../java/com/stuypulse/robot/constants/Cameras.java | 11 +++++++++-- .../robot/subsystems/vision/LimelightVision.java | 6 +----- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 5cb25070..1f06b3e7 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -47,6 +47,7 @@ public static class Camera { private String keyName; private double LLHeartbeat = -1; + private double LLLatency = -1; private int loopCounter = 0; private int rejectedCounterNotNull; @@ -129,14 +130,20 @@ public void updateHeartBeat() { LLHeartbeat = LimelightHelpers.getHeartbeat(this.getName()); } + public void updateLatency() { + LLLatency = LimelightHelpers.getLatency_Pipeline(this.name); + } + public void incrementLoopCounter() { loopCounter += 1; } public boolean isAlive() { //latest - old heartbeat - - if (LimelightHelpers.getHeartbeat(this.getName()) - LLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && LLHeartbeat != -1) { + boolean isDeadLatency = (LLLatency == LimelightHelpers.getLatency_Pipeline(this.name) && LLLatency != -1); + //TODO: double check that latency cannot be negative + boolean isDeadHeartbeat = LimelightHelpers.getHeartbeat(this.getName()) - LLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && LLHeartbeat != -1; + if (isDeadHeartbeat || isDeadLatency) { return false; } else { diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 7d618d38..4ce144a2 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -234,14 +234,10 @@ public void periodicAfterScheduler() { Cameras.LimelightCameras[i].incrementLoopCounter(); Cameras.LimelightCameras[i].updateLEDs(); - //boolean isDeadLatency = (prevRightLLLatency == LimelightHelpers.getLatency_Pipeline(limelightName)); - //if (LimelightHelpers.getHeartbeat(limelightName) - rightLLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && leftLLHeartbeat != -1 || isDeadLatency) { - // LEDController.isRightLLDead = true; - - // prevRightLLLatency = LimelightHelpers.getLatency_Pipeline(limelightName); //Ensure this is below updateLEDs(), otherwise the cameras will never appear as dead Cameras.LimelightCameras[i].updateHeartBeat(); + Cameras.LimelightCameras[i].updateLatency(); From c40f1778c9e9bf74a5e36a79bc769d1fd55ff5dd Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 3 Jun 2026 16:53:22 -0400 Subject: [PATCH 86/97] feat: testing changes. Changing LEDs during autons works. State + supplier based LED works. Cleaned up LED code works. Note to self: Find alternative to twinkle animation (that actually flashes not some weird movement that is seen w twinkle animation) and apply deadlineFor stuff to all other autons. --- src/main/java/com/stuypulse/robot/Robot.java | 1 + .../com/stuypulse/robot/RobotContainer.java | 54 ++++++++-------- .../robot/commands/leds/LEDApplyState.java | 1 - .../stuypulse/robot/constants/Settings.java | 2 + .../robot/subsystems/leds/LEDController.java | 64 ++++++++++++++----- .../swerve/CommandSwerveDrivetrain.java | 2 + .../subsystems/vision/LimelightVision.java | 1 + 7 files changed, 82 insertions(+), 43 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 3e7338db..e13e1c9d 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -46,6 +46,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.WaitCommand; public class Robot extends TimedRobot { diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1f2c15f5..107e20ab 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.auton.regular.CenterTwoCornerBC; import com.stuypulse.robot.commands.auton.regular.Depot; import com.stuypulse.robot.commands.auton.regular.LeftBump; import com.stuypulse.robot.commands.auton.regular.LeftFollow; @@ -21,6 +22,7 @@ import com.stuypulse.robot.commands.auton.regular.RightTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.auton.regular.ShallowSwipeDot; +import com.stuypulse.robot.commands.auton.regular.TwoCornerBC; import com.stuypulse.robot.commands.auton.test.PathfindTest; import com.stuypulse.robot.commands.auton.test.TestBC; import com.stuypulse.robot.commands.handoff.HandoffRun; @@ -104,7 +106,7 @@ public class RobotContainer { public interface EnabledSubsystems { - SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true); + SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", false); SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", true); SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", true); SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", true); @@ -294,7 +296,7 @@ private void configureButtonBindings() { .onTrue(new SuperstructureStow() .alongWith(new HandoffStop()) .alongWith(new SpindexerStop())) - .onTrue(new LEDApplyState(LedState.RESET).repeatedly().withTimeout(2.0)); + .onTrue(new LEDApplyState(LedState.RESET).withTimeout(2.0)); // Manual Left Corner Scoring driver.getLeftButton() @@ -470,38 +472,38 @@ public void configureAutons() { //BC RIGHT - // AutonConfig R_CN_FN_D = new AutonConfig("Right Corner-Near Far-Near Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); - // R_CN_FN_D.register(autonChooser); + AutonConfig R_CN_FN_D = new AutonConfig("Right Corner-Near Far-Near Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); + R_CN_FN_D.register(autonChooser); - // AutonConfig R_CN_NF_D = new AutonConfig("Right Corner-Near Near-Far Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot"); - // R_CN_NF_D.register(autonChooser); + AutonConfig R_CN_NF_D = new AutonConfig("Right Corner-Near Near-Far Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot"); + R_CN_NF_D.register(autonChooser); - // AutonConfig R_CN_FN_CD = new AutonConfig("Right Corner-Near Far-Near Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); - // R_CN_FN_CD.register(autonChooser); + AutonConfig R_CN_FN_CD = new AutonConfig("Right Corner-Near Far-Near Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); + R_CN_FN_CD.register(autonChooser); - // AutonConfig R_CN_NF_CD = new AutonConfig("Right Corner-Near Near-Far Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); - // R_CN_NF_CD.register(autonChooser); + AutonConfig R_CN_NF_CD = new AutonConfig("Right Corner-Near Near-Far Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); + R_CN_NF_CD.register(autonChooser); //BC LEFT - // AutonConfig L_CN_FN_D = new AutonConfig("Left Corner-Near Far-Near Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Dot"); - // L_CN_FN_D.register(autonChooser); + AutonConfig L_CN_FN_D = new AutonConfig("Left Corner-Near Far-Near Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Dot"); + L_CN_FN_D.register(autonChooser); - // AutonConfig L_CN_NF_D = new AutonConfig("Left Corner-Near Near-Far Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Dot"); - // L_CN_NF_D.register(autonChooser); + AutonConfig L_CN_NF_D = new AutonConfig("Left Corner-Near Near-Far Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Dot"); + L_CN_NF_D.register(autonChooser); - // AutonConfig L_CN_FN_CD = new AutonConfig("Left Corner-Near Far-Near Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); - // L_CN_FN_CD.register(autonChooser); + AutonConfig L_CN_FN_CD = new AutonConfig("Left Corner-Near Far-Near Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); + L_CN_FN_CD.register(autonChooser); - // AutonConfig L_CN_NF_CD = new AutonConfig("Left Corner-Near Near-Far Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); - // L_CN_NF_CD.register(autonChooser); + AutonConfig L_CN_NF_CD = new AutonConfig("Left Corner-Near Near-Far Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); + L_CN_NF_CD.register(autonChooser); // FOLLOWS AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java index 871dc98d..11b80f81 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj2.command.Command; public class LEDApplyState extends Command { - //This will not work as default command will override it. Either make the cached class the same as this OR (better solution) have a boolean that tells you if it is manually applied or not and if it is then default command dont change protected final LEDController leds; protected final Supplier state; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index d1f8d607..5be7079e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.controls.RainbowAnimation; import com.ctre.phoenix6.controls.SolidColor; +import com.ctre.phoenix6.controls.TwinkleAnimation; import com.ctre.phoenix6.signals.RGBWColor; import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.stuylib.network.SmartBoolean; @@ -383,6 +384,7 @@ public interface LED { public SolidColor solidColorRequest = new SolidColor(0, Settings.LED.LED_LENGTH - 1).withColor(new RGBWColor(Color.kRed)); public RainbowAnimation rainbowRequest = new RainbowAnimation(0, Settings.LED.LED_LENGTH - 1).withFrameRate(60).withSlot(0); + public TwinkleAnimation twinkleAnimation = new TwinkleAnimation(0, Settings.LED.LED_LENGTH - 1); public static RGBWColor rgbwConverter(Color color) { return new RGBWColor(color); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index e9621453..75398c8c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -11,16 +11,20 @@ import com.ctre.phoenix6.configs.LEDConfigs; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.SolidColor; +import com.ctre.phoenix6.controls.TwinkleAnimation; import com.ctre.phoenix6.hardware.CANdle; import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue; import com.ctre.phoenix6.signals.RGBWColor; import com.ctre.phoenix6.signals.StatusLedWhenActiveValue; import com.ctre.phoenix6.signals.StripTypeValue; import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import dev.doglog.DogLog; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDController extends SubsystemBase { @@ -34,8 +38,10 @@ public class LEDController extends SubsystemBase { public boolean backDeadAnimationCleared; public boolean rightDeadAnimationCleared; - // private Pose2d lastPoseOnAprilTag; - // private boolean initialPoseUpdated = false; + private Pose2d lastPoseOnAprilTag; + private boolean initialPoseUpdated; + + private boolean needToBeTwinkle; static { instance = new LEDController(); @@ -58,8 +64,13 @@ private LEDController() { isBackLLDead = false; isRightLLDead = false; + initialPoseUpdated = false; + needToBeTwinkle = false; + + lastPoseOnAprilTag = new Pose2d(); + leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); - // lastPoseOnAprilTag = new Pose2d(); + candleConfigs = new CANdleConfiguration() .withLED( @@ -118,15 +129,32 @@ public ControlRequest getAnimation() { //TODO: make branch for the distance flashing thing public void applyPattern() { - // if (initialPoseUpdated && - // lastPoseOnAprilTag.getTranslation() - // .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()) > Settings.LED.APRIL_TAG_DISTANCE_THRESHOLD) { - // - // } + if (initialPoseUpdated && + lastPoseOnAprilTag.getTranslation() + .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()) > Settings.LED.APRIL_TAG_DISTANCE_THRESHOLD) { + needToBeTwinkle = true; + } + else { + needToBeTwinkle = false; + } + + //TODO: potentially make this go both ways - if need be + boolean shouldClear = (cachedState.getAnimation() instanceof TwinkleAnimation && state.getAnimation() instanceof SolidColor) ? true : false; if (cachedState != state) { - SolidColor solidColor = (SolidColor) ledPattern; - solidColor.withColor(state.getColor()); + //not clearing animations + //instance of locks me out + if (shouldClear) leds.clearAllAnimations(); + ledPattern = needToBeTwinkle ? Settings.LED.twinkleAnimation : Settings.LED.solidColorRequest; + + if (ledPattern instanceof SolidColor) { + SolidColor solidColor = (SolidColor) ledPattern; + solidColor.withColor(state.getColor()); + } + else if (ledPattern instanceof TwinkleAnimation) { + TwinkleAnimation twinkleAnimation = (TwinkleAnimation) ledPattern; + twinkleAnimation.withColor(state.getColor()); + } cachedState = state; } @@ -138,12 +166,16 @@ public void changeState(LedState state) { public void periodicAfterScheduler() { - // if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || - // Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || - // Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { - // lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); - // initialPoseUpdated = true; - // } + if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || + Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || + Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { + lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); + initialPoseUpdated = true; + } + + DogLog.log("LED/last pose updated", this.lastPoseOnAprilTag); + DogLog.log("LED/distance from last pose", lastPoseOnAprilTag.getTranslation() + .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation())); if (RobotContainer.EnabledSubsystems.LEDS.get()) { applyPattern(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 95bd9041..7e75e9dd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -430,6 +430,8 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); } + + //TODO: save the pose here } public Pose2d getPose() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 4ce144a2..f66bb991 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -231,6 +231,7 @@ public void periodicAfterScheduler() { String limelightName = names[i]; DogLog.log("LED/heartbeat" + limelightName, LimelightHelpers.getHeartbeat(limelightName)); + DogLog.log("LED/raw fiducials" + limelightName, LimelightHelpers.getRawFiducials(limelightName).length); Cameras.LimelightCameras[i].incrementLoopCounter(); Cameras.LimelightCameras[i].updateLEDs(); From d7bde99556f190c1947bc7ec811eb1e54490261d Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Fri, 5 Jun 2026 16:50:15 -0400 Subject: [PATCH 87/97] clean: clean up logging --- .../com/stuypulse/robot/RobotContainer.java | 2 +- .../stuypulse/robot/constants/Cameras.java | 10 +++- .../robot/subsystems/handoff/HandoffImpl.java | 18 +++--- .../robot/subsystems/intake/IntakeImpl.java | 52 +++++++++-------- .../robot/subsystems/leds/LEDController.java | 1 - .../subsystems/spindexer/SpindexerImpl.java | 19 ++++--- .../superstructure/hood/HoodImpl.java | 14 ++--- .../superstructure/shooter/ShooterImpl.java | 56 +++++++++---------- .../superstructure/turret/TurretImpl.java | 34 +++++------ .../swerve/CommandSwerveDrivetrain.java | 32 +++++------ .../subsystems/vision/LimelightVision.java | 39 ++++--------- 11 files changed, 127 insertions(+), 150 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 107e20ab..c691abfb 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -619,7 +619,7 @@ public void periodicAfterScheduler() { superstructure.getCurrentDraw() + swerve.getTotalDriveSupplyCurrent() + swerve.getTotalSteerSupplyCurrent(); - DogLog.log("Robot/Total Current Draw", totalCurrentDraw); + DogLog.log("Robot/Total Current Draw", totalCurrentDraw, "Amps"); handoff.periodicAfterScheduler(); intake.periodicAfterScheduler(); diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 1f06b3e7..02798453 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -215,8 +215,8 @@ public void log() { DogLog.log(keyName + "# Rejected Invalid Position", rejectedCounterInvalidPosition); DogLog.log(keyName + "Heartbeat", LimelightHelpers.getHeartbeat(name)); - DogLog.log(keyName + "Temp (C)", LimelightHelpers.getLimelightDoubleArrayEntry(name, "hw").get()); - DogLog.log(keyName + "cpu usage", LimelightHelpers.getLimelightDoubleArrayEntry(name, "cpu").get()); + DogLog.log(keyName + "Temp", LimelightHelpers.getLimelightDoubleArrayEntry(name, "hw").get(), "Celsius"); + DogLog.log(keyName + "Cpu Usage", LimelightHelpers.getLimelightDoubleArrayEntry(name, "cpu").get(), "Percent"); DogLog.log(keyName + "Pose MT1", (Robot.isBlue() ? LimelightHelpers.getBotPoseEstimate_wpiBlue(name).pose : LimelightHelpers.getBotPoseEstimate_wpiRed(name).pose)); @@ -224,6 +224,12 @@ public void log() { ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).pose : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(name).pose)); DogLog.log(keyName + "Pipeline", LimelightHelpers.getCurrentPipelineIndex(name)); + + // this yaw is seems to be the robot yaw passed into the LL + DogLog.log(keyName + "Robot Yaw Passed In", LimelightHelpers.getIMUData(name).robotYaw); + // this is just the yaw of the internal imu + DogLog.log(keyName + "IMU Yaw", LimelightHelpers.getIMUData(name).Yaw); + DogLog.log(keyName + "Pipeline Latency", LimelightHelpers.getLatency_Pipeline(name)); result = LimelightHelpers.getLatestResults(name); DogLog.log(keyName + "Time since last boot", result.timestamp_LIMELIGHT_publish / 1000.0, "Seconds"); diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java index 0a5da6c1..a69dbcfb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -115,8 +115,6 @@ public double getFollowerRPM() { public void periodicAfterScheduler() { super.periodicAfterScheduler(); - // removed shouldNotShootIntoHub logic (no longer used) - if (EnabledSubsystems.HANDOFF.get() && getState() != HandoffState.STOP) { if (voltageOverride.isPresent()) { motorLead.setVoltage(voltageOverride.get()); @@ -132,18 +130,18 @@ public void periodicAfterScheduler() { motorFollow.stopMotor(); } - DogLog.log("Handoff/Lead Velocity", getLeaderRPM()); - DogLog.log("Handoff/Follow Velocity", getLeaderRPM()); + DogLog.log("Handoff/Lead Velocity", getLeaderRPM(), "RPM"); + DogLog.log("Handoff/Follow Velocity", getLeaderRPM(), "RPM"); if (Settings.DEBUG_MODE.get()) { - DogLog.log("Handoff/Lead Voltage", motorLeadVoltage.getValueAsDouble()); - DogLog.log("Handoff/Lead Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); - DogLog.log("Handoff/Lead Stator Current", motorLeadStatorCurrent.getValueAsDouble()); + DogLog.log("Handoff/Lead Voltage", motorLeadVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Handoff/Lead Supply Current", motorLeadSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Handoff/Lead Stator Current", motorLeadStatorCurrent.getValueAsDouble(), "Amps"); - DogLog.log("Handoff/Follow Voltage", motorLeadVoltage.getValueAsDouble()); - DogLog.log("Handoff/Follow Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); - DogLog.log("Handoff/Follow Stator Current", motorLeadStatorCurrent.getValueAsDouble()); + DogLog.log("Handoff/Follow Voltage", motorLeadVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Handoff/Follow Supply Current", motorLeadSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Handoff/Follow Stator Current", motorLeadStatorCurrent.getValueAsDouble(), "Amps"); if(Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log("Robot/CAN/Main/Handoff Lead Motor Connected? (ID " + String.valueOf(Ports.Handoff.MOTOR_LEAD) + ")", motorLead.isConnected()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 2987cf8c..5e48ddff 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -260,40 +260,39 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge // PIVOT DogLog.log("Intake/Pivot Pushdown Voltage Applied?", applyingPushdownCurrent); - DogLog.log("Intake/Pivot Closed Loop Error (deg)", - pivot.getClosedLoopError().getValueAsDouble() * 360.0); + DogLog.log("Intake/Pivot Closed Loop Error", + pivot.getClosedLoopError().getValueAsDouble() * 360.0, "Degrees"); if (Settings.DEBUG_MODE.get()) { DogLog.log("Intake/Voltage Override", pivotVoltageOverride.isPresent()); - DogLog.log("Intake/Pivot Temperature (C)", pivotTemperature.getValueAsDouble()); - DogLog.log("Intake/Leader Temperature (C)", - rollerLeaderTemperature.getValueAsDouble()); - DogLog.log("Intake/Follower Temperature (C)", - rollerFollowerTemperature.getValueAsDouble()); + DogLog.log("Intake/Pivot Temperature", pivotTemperature.getValueAsDouble(), "Celsius"); + DogLog.log("Intake/Leader Temperature", + rollerLeaderTemperature.getValueAsDouble(), "Celsius"); + DogLog.log("Intake/Follower Temperature", + rollerFollowerTemperature.getValueAsDouble(), "Celsius"); // Rolers - DogLog.log("Intake/Roller Leader Voltage (volts)", - rollerLeaderVoltage.getValueAsDouble()); - DogLog.log("Intake/Roller Leader Current (amps)", - rollerLeaderSupplyCurrent.getValueAsDouble()); - DogLog.log("Intake/Roller Leader Stator Current (amps)", - rollerLeaderStatorCurrent.getValueAsDouble()); - DogLog.log("Intake/Roller Follower Voltage (volts)", - rollerFollowerVoltage.getValueAsDouble()); - DogLog.log("Intake/Roller Follower Current (amps)", - rollerFollowerSupplyCurrent.getValueAsDouble()); - DogLog.log("Intake/Roller Follower Stator Current (amps)", - rollerFollowerStatorCurrent.getValueAsDouble()); + DogLog.log("Intake/Roller Leader Voltage", + rollerLeaderVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Intake/Roller Leader Current", + rollerLeaderSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Intake/Roller Leader Stator Current", + rollerLeaderStatorCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Intake/Roller Follower Voltage", + rollerFollowerVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Intake/Roller Follower Current", + rollerFollowerSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Intake/Roller Follower Stator Current", + rollerFollowerStatorCurrent.getValueAsDouble(), "Amps"); - // Pivot - DogLog.log("Intake/Pivot Voltage (volts)", pivotMotorVoltage.getValueAsDouble()); - DogLog.log("Intake/Pivot Supply Current (amps)", - pivotSupplyCurrent.getValueAsDouble()); - DogLog.log("Intake/Pivot Stator Current (amps)", - pivotStatorCurrent.getValueAsDouble()); + DogLog.log("Intake/Pivot Voltage", pivotMotorVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Intake/Pivot Supply Current", + pivotSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Intake/Pivot Stator Current", + pivotStatorCurrent.getValueAsDouble(), "Amps"); DogLog.log("Intake/Pivot is below pushdown Threshold", isPivotBelowPushDownThreshold.get()); - DogLog.log("Intake/Pivot Torque Current", pivotTorqueCurrent.getValueAsDouble()); + DogLog.log("Intake/Pivot Torque Current", pivotTorqueCurrent.getValueAsDouble(), "Amps"); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log("Robot/CAN/Main/Intake Pivot Motor Connected? (ID " @@ -305,7 +304,6 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge } Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); - } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 75398c8c..40877686 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -221,6 +221,5 @@ public void periodicAfterScheduler() { DogLog.log("LED/Is Back LL dead", isBackLLDead); DogLog.log("LED/Is Right LL dead", isRightLLDead); DogLog.log("LED/Is Left LL dead", isLeftLLDead); - } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index 7b8c65b8..8c4e2a7b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -117,11 +117,11 @@ public SpindexerImpl() { private double getLeaderRPM() { return leaderVelocity.getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.GEAR_RATIO; } - private double getFollowerRpm() { + + private double getFollowerRPM() { return followerVelocity.getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.GEAR_RATIO; } - private boolean spindexerUnjam() { if (!hasStartedStallTimer && Handoff.getInstance().isHandoffStalling()) { unjamTimer.start(); @@ -164,15 +164,16 @@ public void periodicAfterScheduler() { followerMotor.stopMotor(); } - DogLog.log("Spindexer/Leader Motor RPM", getLeaderRPM()); + DogLog.log("Spindexer/Leader RPM", getLeaderRPM(), "RPM"); + DogLog.log("Spindexer/Follower RPM", getFollowerRPM(), "RPM"); // SmartDashboard.putBoolean("Spindexer/Unjamming", unJamming); - DogLog.log("Spindexer/Leader Voltage (volts)", leaderMotorVoltage.getValueAsDouble()); - DogLog.log("Spindexer/Leader Supply Current (amps)", leaderSupplyCurrent.getValueAsDouble()); - DogLog.log("Spindexer/Leader Stator Current (amps)", leaderStatorCurrent.getValueAsDouble()); - DogLog.log("Spindexer/Follower Voltage (volts)", followerMotorVoltage.getValueAsDouble()); - DogLog.log("Spindexer/Follower Supply Current (amps)", followerSupplyCurrent.getValueAsDouble()); - DogLog.log("Spindexer/Follower Stator Current (amps)", followerStatorCurrent.getValueAsDouble()); + DogLog.log("Spindexer/Leader Voltage", leaderMotorVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Spindexer/Leader Supply Current", leaderSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Spindexer/Leader Stator Current", leaderStatorCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Spindexer/Follower Voltage", followerMotorVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Spindexer/Follower Supply Current", followerSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Spindexer/Follower Stator Current", followerStatorCurrent.getValueAsDouble(), "Amps"); // SmartDashboard.putBoolean("Spindexer/Should Stop?", shouldStop()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 21fc5de5..9f730fa1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -182,15 +182,15 @@ public void periodicAfterScheduler() { // SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); DogLog.log("Prematch Checks/Hood at Bottom?", getAngle().getDegrees() < Settings.Superstructure.Hood.REVERSE_SOFT_LIMIT.getDegrees()); - DogLog.log("Superstructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); - DogLog.log("Superstructure/Hood/Closed Loop Error (deg)", hoodMotorClosedLoopError.getValueAsDouble() * 360.0); - DogLog.log("Superstructure/Hood/Implemented Error (Degrees)", getTargetAngle().getDegrees() - getAngle().getDegrees()); + DogLog.log("Superstructure/Hood/Correct Hood Angle", getAbsoluteHoodAngleDeg(), "Degrees"); + DogLog.log("Superstructure/Hood/Closed Loop Error", hoodMotorClosedLoopError.getValueAsDouble() * 360.0, "Degrees"); + DogLog.log("Superstructure/Hood/Implemented Error", getTargetAngle().getDegrees() - getAngle().getDegrees(), "Degrees"); if (Settings.DEBUG_MODE.get()) { - DogLog.log("Superstructure/Hood/Applied Voltage (amps)", hoodMotorVoltage.getValueAsDouble()); - DogLog.log("Superstructure/Hood/Supply Current (amps)", hoodMotorSupplyCurrent.getValueAsDouble()); - DogLog.log("Superstructure/Hood/Stator Current (amps)", hoodMotorStatorCurrent.getValueAsDouble()); - DogLog.log("Superstructure/Hood/Raw Motor Encoder Value",hoodMotorStatorCurrent.getValueAsDouble()); + DogLog.log("Superstructure/Hood/Applied Voltage", hoodMotorVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Superstructure/Hood/Supply Current", hoodMotorSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Superstructure/Hood/Stator Current", hoodMotorStatorCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Superstructure/Hood/Raw Motor Encoder Value",hoodMotorStatorCurrent.getValueAsDouble(), "Degrees"); DogLog.log("Superstructure/Hood/is stalling", isStalling()); Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 0a332e22..532bd9f0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -153,34 +153,32 @@ public void periodicAfterScheduler() { shooterLeader.stopMotor(); } - DogLog.log("Superstructure/Shooter/Leader RPM", getLeaderRPM()); - DogLog.log("Superstructure/Shooter/Follower RPM", getFollowerRPM()); + DogLog.log("Superstructure/Shooter/Leader RPM", getLeaderRPM(), "RPM"); + DogLog.log("Superstructure/Shooter/Follower RPM", getFollowerRPM(), "RPM"); if (Settings.DEBUG_MODE.get()) { DogLog.log("InterpolationTesting/Shooter Applied Voltage", - shooterLeaderVoltage.getValueAsDouble()); - - DogLog.log("Superstructure/Shooter/Leader Voltage (volts)", - shooterLeaderVoltage.getValueAsDouble()); - DogLog.log("Superstructure/Shooter/Leader Supply Current (amps)", - shooterLeadSupplyCurrent.getValueAsDouble()); - DogLog.log("Superstructure/Shooter/Leader Stator Current (amps)", - shooterLeadStatorCurrent.getValueAsDouble()); - - DogLog.log("Superstructure/Shooter/Leader Motor Temp (C)", - shooterLeaderTemperature.getValueAsDouble()); - - DogLog.log("Superstructure/Shooter/Follower Voltage (volts)", - shooterFollowerVoltage.getValueAsDouble()); - DogLog.log("Superstructure/Shooter/Follower Supply Current (amps)", - shooterFollowSupplyCurrent.getValueAsDouble()); - DogLog.log("Superstructure/Shooter/Follower Stator Current (amps)", - shooterFollowStatorCurrent.getValueAsDouble()); + shooterLeaderVoltage.getValueAsDouble(), "Volts"); + + DogLog.log("Superstructure/Shooter/Leader Voltage", + shooterLeaderVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Superstructure/Shooter/Leader Supply Current", + shooterLeadSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Superstructure/Shooter/Leader Stator Current", + shooterLeadStatorCurrent.getValueAsDouble(), "Amps"); + + DogLog.log("Superstructure/Shooter/Leader Motor Temp", + shooterLeaderTemperature.getValueAsDouble(), "Celsius"); + + DogLog.log("Superstructure/Shooter/Follower Voltage", + shooterFollowerVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Superstructure/Shooter/Follower Supply Current", + shooterFollowSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Superstructure/Shooter/Follower Stator Current", + shooterFollowStatorCurrent.getValueAsDouble(), "Amps"); - DogLog.log("Superstructure/Shooter/Follower Motor Temp (C)", - shooterFollowerTemperature.getValueAsDouble()); - - + DogLog.log("Superstructure/Shooter/Follower Motor Temp", + shooterFollowerTemperature.getValueAsDouble(), "Celsius"); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log( @@ -195,10 +193,10 @@ public void periodicAfterScheduler() { Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); } - DogLog.log("InterpolationTesting/Shooter Closed Loop Error (RPM)", - shooterLeaderClosedLoopError.getValueAsDouble() * 60.0); + DogLog.log("InterpolationTesting/Shooter Closed Loop Error", + shooterLeaderClosedLoopError.getValueAsDouble() * 60.0, "RPM"); - DogLog.log("Superstructure/Shooter/Implemented Error (RPM)", getTargetRPM() - getLeaderRPM()); + DogLog.log("Superstructure/Shooter/Implemented Error", getTargetRPM() - getLeaderRPM(), "RPM"); } private void setVoltageOverride(Optional voltageOverride) { @@ -222,9 +220,9 @@ public SysIdRoutine getShooterSysIdRoutine() { public double getCurrentDraw() { return Double.max(0, shooterLeadSupplyCurrent.getValueAsDouble()) + Double.max(0, shooterFollowSupplyCurrent.getValueAsDouble()); - } + } - @Override + @Override public boolean isShooting() { return currentlyShooting.get(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 05119635..56fba4db 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -254,9 +254,6 @@ public void periodicAfterScheduler() { if (isWrapping) { slot = 1; } - // else if (!deltaIsSignificant) { - // slot = 2; - // } if (EnabledSubsystems.TURRET.get()) { if (voltageOverride.isPresent()) { @@ -281,26 +278,26 @@ public void periodicAfterScheduler() { turretMotor.stopMotor(); } - DogLog.log("Superstructure/Turret/Relative Encoder Position (deg)", - turretMotorPos.getValueAsDouble() * 360.0); - DogLog.log("Superstructure/Turret/Closed Loop Error (deg)", - turretMotorClosedLoopError.getValueAsDouble() * 360.0); + DogLog.log("Superstructure/Turret/Relative Encoder Position", + turretMotorPos.getValueAsDouble() * 360.0, "Degrees"); + DogLog.log("Superstructure/Turret/Closed Loop Error", + turretMotorClosedLoopError.getValueAsDouble() * 360.0, "Degrees"); - DogLog.log("Superstructure/Turret/Encoder18t Abs Position (Rot)", - encoder18tPos.getValueAsDouble()); - DogLog.log("Superstructure/Turret/Encoder17t Abs Position (Rot)", - encoder17tPos.getValueAsDouble()); + DogLog.log("Superstructure/Turret/Encoder18t Abs Position", + encoder18tPos.getValueAsDouble(), "Rotations"); + DogLog.log("Superstructure/Turret/Encoder17t Abs Position", + encoder17tPos.getValueAsDouble(), "Rotations"); - DogLog.log("Superstructure/Turret/Voltage (volts)", - turretMotorVoltage.getValueAsDouble()); + DogLog.log("Superstructure/Turret/Voltage", + turretMotorVoltage.getValueAsDouble(), "Volts"); - DogLog.log("Superstructure/Turret/Wrapped Target Angle (deg)", prevActualTargetAngle); + DogLog.log("Superstructure/Turret/Wrapped Target Angle", prevActualTargetAngle, "Degrees"); if (Settings.DEBUG_MODE.get()) { - DogLog.log("Superstructure/Turret/Stator Current (amps)", - turretMotorStatorCurrent.getValueAsDouble()); - DogLog.log("Superstructure/Turret/Supply Current (amps)", - turretMotorSupplyCurrent.getValueAsDouble()); + DogLog.log("Superstructure/Turret/Stator Current", + turretMotorStatorCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Superstructure/Turret/Supply Current", + turretMotorSupplyCurrent.getValueAsDouble(), "Amps"); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log( @@ -312,7 +309,6 @@ public void periodicAfterScheduler() { + String.valueOf(Ports.Superstructure.Turret.ENCODER18T) + ")", encoder18t.isConnected()); } Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); - } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 7e75e9dd..1468532a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -755,15 +755,15 @@ public void periodicAfterScheduler() { ChassisSpeeds chassisSpeeds = getChassisSpeeds(); Vector2D fieldRelativeSpeeds = getFieldRelativeSpeeds(); - DogLog.log("Swerve/Velocity Robot Relative X (m per s)", chassisSpeeds.vxMetersPerSecond); - DogLog.log("Swerve/Velocity Robot Relative Y (m per s)", chassisSpeeds.vyMetersPerSecond); + DogLog.log("Swerve/Velocity Robot Relative X", chassisSpeeds.vxMetersPerSecond, "Meters/Second"); + DogLog.log("Swerve/Velocity Robot Relative Y", chassisSpeeds.vyMetersPerSecond, "Meters/Second"); - DogLog.log("Swerve/Velocity Field Relative X (m per s)", fieldRelativeSpeeds.x); - DogLog.log("Swerve/Field Relative Rotation", pose.getRotation().getDegrees()); - DogLog.log("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); + DogLog.log("Swerve/Velocity Field Relative X", fieldRelativeSpeeds.x, "Meters/Second"); + DogLog.log("Swerve/Field Relative Rotation", pose.getRotation().getDegrees(), "Degrees"); + DogLog.log("Swerve/Velocity Field Relative Y", getFieldRelativeSpeeds().y, "Meters/Second"); - DogLog.log("Swerve/Angular Velocity (rad per s)", chassisSpeeds.omegaRadiansPerSecond); - DogLog.log("Swerve/Distance From Hub (meters)", Field.HUB_CENTER.getTranslation().getDistance(pose.getTranslation())); + DogLog.log("Swerve/Angular Velocity", chassisSpeeds.omegaRadiansPerSecond, "Radians/Second"); + DogLog.log("Swerve/Distance From Hub", Field.HUB_CENTER.getTranslation().getDistance(pose.getTranslation()), "Meters"); DogLog.log("Swerve/Pose", pose); @@ -781,27 +781,27 @@ public void periodicAfterScheduler() { // will confirm whether we are even getting data DogLog.log("Superstructure/Turret/Dist From Hub", - turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); + turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation()), "Meters"); DogLog.log("InterpolationTesting/Turret Dist From Hub", - turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); + turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation()), "Meters"); DogLog.log("InterpolationTesting/Turret Dist From Ferry Zone", turretPose.getTranslation() - .getDistance(Field.getFerryZonePose(pose.getTranslation()).getTranslation())); + .getDistance(Field.getFerryZonePose(pose.getTranslation()).getTranslation()), "Meters"); for (int i = 0; i < 4; i++) { String prefix = "Swerve/Modules/Module " + i; SwerveModuleState current = getModule(i).getCurrentState(); SwerveModuleState target = getModule(i).getTargetState(); - DogLog.log(prefix + "/Speed (m per s)", current.speedMetersPerSecond); - DogLog.log(prefix + "/Target Speed (m per s)", target.speedMetersPerSecond); - DogLog.log(prefix + "/Angle (deg)", current.angle.getDegrees() % 360); - DogLog.log(prefix + "/Target Angle (deg)", target.angle.getDegrees() % 360); + DogLog.log(prefix + "/Speed", current.speedMetersPerSecond, "Meters/Second"); + DogLog.log(prefix + "/Target Speed", target.speedMetersPerSecond, "Meters/Second"); + DogLog.log(prefix + "/Angle", current.angle.getDegrees() % 360, "Degrees"); + DogLog.log(prefix + "/Target Angle", target.angle.getDegrees() % 360, "Degrees"); if (Settings.DEBUG_MODE.get()) { DogLog.log(prefix + "/Stator Current", - getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble()); + getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble(), "Amps"); DogLog.log(prefix + "/Supply Current", - getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble()); + getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble(), "Amps"); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index f66bb991..a6b89ffd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -230,17 +230,12 @@ public void periodicAfterScheduler() { if (Cameras.LimelightCameras[i].isEnabled()) { String limelightName = names[i]; - DogLog.log("LED/heartbeat" + limelightName, LimelightHelpers.getHeartbeat(limelightName)); - DogLog.log("LED/raw fiducials" + limelightName, LimelightHelpers.getRawFiducials(limelightName).length); - Cameras.LimelightCameras[i].incrementLoopCounter(); Cameras.LimelightCameras[i].updateLEDs(); //Ensure this is below updateLEDs(), otherwise the cameras will never appear as dead Cameras.LimelightCameras[i].updateHeartBeat(); Cameras.LimelightCameras[i].updateLatency(); - - // Seed robot heading (used by MT2) LimelightHelpers.SetRobotOrientation( @@ -254,22 +249,18 @@ public void periodicAfterScheduler() { ); PoseEstimate poseEstimate; - PoseEstimate poseEstimateMT1 = Robot.isBlue() - ? LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName) - : LimelightHelpers.getBotPoseEstimate_wpiRed(limelightName); - - PoseEstimate poseEstimateMT2 = Robot.isBlue() - ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName) - : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(limelightName); // MegaTag switching if (megaTagMode == MegaTagMode.MEGATAG1) { - poseEstimate = poseEstimateMT1; + poseEstimate = Robot.isBlue() + ? LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName) + : LimelightHelpers.getBotPoseEstimate_wpiRed(limelightName); } else { - poseEstimate = poseEstimateMT2; + poseEstimate = Robot.isBlue() + ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName) + : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(limelightName); } - // Adding to pose estimator boolean notNull = false; boolean withinAngularVelocityTolerance = false; @@ -309,15 +300,10 @@ public void periodicAfterScheduler() { DogLog.log("Vision/Within Angular Velocity Tolerance", withinAngularVelocityTolerance); DogLog.log("Vision/Not Null", notNull); - DogLog.log("Vision/Pose X Component", robotPose.getX()); - DogLog.log("Vision/Pose Y Component", robotPose.getY()); - DogLog.log("Vision/Pose Theta (Degrees)", robotPose.getRotation().getDegrees()); - DogLog.log("Vision/Pose Estimate X " + limelightName, poseEstimate.pose.getX()); - DogLog.log("Vision/Pose Estimate Y " + limelightName, poseEstimate.pose.getY()); - DogLog.log("Vision/Pose Estimate Theta " + limelightName, poseEstimate.pose.getRotation().getDegrees()); - - DogLog.log("Vision/" + Cameras.LimelightCameras[i].getName() + "/Pose MT1", poseEstimateMT1.pose); - DogLog.log("Vision/" + Cameras.LimelightCameras[i].getName() + "/Pose MT2", poseEstimateMT2.pose); + DogLog.log("Vision/Pose", robotPose); + DogLog.log("Vision/Pose X Component", robotPose.getX(), "Meters"); + DogLog.log("Vision/Pose Y Component", robotPose.getY(), "Meters"); + DogLog.log("Vision/Pose Theta", robotPose.getRotation().getDegrees(), "Degrees"); DogLog.log("Vision/" + names[i] + " Has Data", true); @@ -327,11 +313,6 @@ public void periodicAfterScheduler() { } DogLog.log("Vision/MegaTag Mode", megaTagMode.toString()); - // this yaw is seems to be the robot yaw passed into the LL - DogLog.log("Vision/Limelight Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); - // this is just the yaw of the internal imu - DogLog.log("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); - DogLog.log("Vision/latency_pipeline " + limelightName , LimelightHelpers.getLatency_Pipeline(limelightName)); Cameras.LimelightCameras[i].log(); } From 53720cec44fa9b9de08b5ae48725a36669bf8a17 Mon Sep 17 00:00:00 2001 From: Danx3mer Date: Fri, 19 Jun 2026 21:34:04 -0400 Subject: [PATCH 88/97] Revert "feat: testing changes. Changing LEDs during autons works. State + supplier based LED works. Cleaned up LED code works. Note to self: Find alternative to twinkle animation (that actually flashes not some weird movement that is seen w twinkle animation) and apply deadlineFor stuff to all other autons." This reverts commit c40f1778c9e9bf74a5e36a79bc769d1fd55ff5dd. --- src/main/java/com/stuypulse/robot/Robot.java | 1 - .../com/stuypulse/robot/RobotContainer.java | 54 ++++++++-------- .../robot/commands/leds/LEDApplyState.java | 1 + .../stuypulse/robot/constants/Settings.java | 2 - .../robot/subsystems/leds/LEDController.java | 64 +++++-------------- .../swerve/CommandSwerveDrivetrain.java | 2 - 6 files changed, 43 insertions(+), 81 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index e13e1c9d..3e7338db 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -46,7 +46,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.WaitCommand; public class Robot extends TimedRobot { diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c691abfb..dd01d668 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,7 +7,6 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.auton.regular.CenterTwoCornerBC; import com.stuypulse.robot.commands.auton.regular.Depot; import com.stuypulse.robot.commands.auton.regular.LeftBump; import com.stuypulse.robot.commands.auton.regular.LeftFollow; @@ -22,7 +21,6 @@ import com.stuypulse.robot.commands.auton.regular.RightTwoCornerVariant; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.auton.regular.ShallowSwipeDot; -import com.stuypulse.robot.commands.auton.regular.TwoCornerBC; import com.stuypulse.robot.commands.auton.test.PathfindTest; import com.stuypulse.robot.commands.auton.test.TestBC; import com.stuypulse.robot.commands.handoff.HandoffRun; @@ -106,7 +104,7 @@ public class RobotContainer { public interface EnabledSubsystems { - SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", false); + SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true); SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", true); SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", true); SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", true); @@ -296,7 +294,7 @@ private void configureButtonBindings() { .onTrue(new SuperstructureStow() .alongWith(new HandoffStop()) .alongWith(new SpindexerStop())) - .onTrue(new LEDApplyState(LedState.RESET).withTimeout(2.0)); + .onTrue(new LEDApplyState(LedState.RESET).repeatedly().withTimeout(2.0)); // Manual Left Corner Scoring driver.getLeftButton() @@ -472,38 +470,38 @@ public void configureAutons() { //BC RIGHT - AutonConfig R_CN_FN_D = new AutonConfig("Right Corner-Near Far-Near Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); - R_CN_FN_D.register(autonChooser); + // AutonConfig R_CN_FN_D = new AutonConfig("Right Corner-Near Far-Near Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); + // R_CN_FN_D.register(autonChooser); - AutonConfig R_CN_NF_D = new AutonConfig("Right Corner-Near Near-Far Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot"); - R_CN_NF_D.register(autonChooser); + // AutonConfig R_CN_NF_D = new AutonConfig("Right Corner-Near Near-Far Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot"); + // R_CN_NF_D.register(autonChooser); - AutonConfig R_CN_FN_CD = new AutonConfig("Right Corner-Near Far-Near Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); - R_CN_FN_CD.register(autonChooser); + // AutonConfig R_CN_FN_CD = new AutonConfig("Right Corner-Near Far-Near Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); + // R_CN_FN_CD.register(autonChooser); - AutonConfig R_CN_NF_CD = new AutonConfig("Right Corner-Near Near-Far Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); - R_CN_NF_CD.register(autonChooser); + // AutonConfig R_CN_NF_CD = new AutonConfig("Right Corner-Near Near-Far Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); + // R_CN_NF_CD.register(autonChooser); //BC LEFT - AutonConfig L_CN_FN_D = new AutonConfig("Left Corner-Near Far-Near Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Dot"); - L_CN_FN_D.register(autonChooser); + // AutonConfig L_CN_FN_D = new AutonConfig("Left Corner-Near Far-Near Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Dot"); + // L_CN_FN_D.register(autonChooser); - AutonConfig L_CN_NF_D = new AutonConfig("Left Corner-Near Near-Far Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Dot"); - L_CN_NF_D.register(autonChooser); + // AutonConfig L_CN_NF_D = new AutonConfig("Left Corner-Near Near-Far Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Dot"); + // L_CN_NF_D.register(autonChooser); - AutonConfig L_CN_FN_CD = new AutonConfig("Left Corner-Near Far-Near Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); - L_CN_FN_CD.register(autonChooser); + // AutonConfig L_CN_FN_CD = new AutonConfig("Left Corner-Near Far-Near Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); + // L_CN_FN_CD.register(autonChooser); - AutonConfig L_CN_NF_CD = new AutonConfig("Left Corner-Near Near-Far Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, - "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); - L_CN_NF_CD.register(autonChooser); + // AutonConfig L_CN_NF_CD = new AutonConfig("Left Corner-Near Near-Far Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); + // L_CN_NF_CD.register(autonChooser); // FOLLOWS AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java index 11b80f81..871dc98d 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.Command; public class LEDApplyState extends Command { + //This will not work as default command will override it. Either make the cached class the same as this OR (better solution) have a boolean that tells you if it is manually applied or not and if it is then default command dont change protected final LEDController leds; protected final Supplier state; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 5be7079e..d1f8d607 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.controls.RainbowAnimation; import com.ctre.phoenix6.controls.SolidColor; -import com.ctre.phoenix6.controls.TwinkleAnimation; import com.ctre.phoenix6.signals.RGBWColor; import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.stuylib.network.SmartBoolean; @@ -384,7 +383,6 @@ public interface LED { public SolidColor solidColorRequest = new SolidColor(0, Settings.LED.LED_LENGTH - 1).withColor(new RGBWColor(Color.kRed)); public RainbowAnimation rainbowRequest = new RainbowAnimation(0, Settings.LED.LED_LENGTH - 1).withFrameRate(60).withSlot(0); - public TwinkleAnimation twinkleAnimation = new TwinkleAnimation(0, Settings.LED.LED_LENGTH - 1); public static RGBWColor rgbwConverter(Color color) { return new RGBWColor(color); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 40877686..4896183a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -11,20 +11,16 @@ import com.ctre.phoenix6.configs.LEDConfigs; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.SolidColor; -import com.ctre.phoenix6.controls.TwinkleAnimation; import com.ctre.phoenix6.hardware.CANdle; import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue; import com.ctre.phoenix6.signals.RGBWColor; import com.ctre.phoenix6.signals.StatusLedWhenActiveValue; import com.ctre.phoenix6.signals.StripTypeValue; import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import dev.doglog.DogLog; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDController extends SubsystemBase { @@ -38,10 +34,8 @@ public class LEDController extends SubsystemBase { public boolean backDeadAnimationCleared; public boolean rightDeadAnimationCleared; - private Pose2d lastPoseOnAprilTag; - private boolean initialPoseUpdated; - - private boolean needToBeTwinkle; + // private Pose2d lastPoseOnAprilTag; + // private boolean initialPoseUpdated = false; static { instance = new LEDController(); @@ -64,13 +58,8 @@ private LEDController() { isBackLLDead = false; isRightLLDead = false; - initialPoseUpdated = false; - needToBeTwinkle = false; - - lastPoseOnAprilTag = new Pose2d(); - leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); - + // lastPoseOnAprilTag = new Pose2d(); candleConfigs = new CANdleConfiguration() .withLED( @@ -129,32 +118,15 @@ public ControlRequest getAnimation() { //TODO: make branch for the distance flashing thing public void applyPattern() { - if (initialPoseUpdated && - lastPoseOnAprilTag.getTranslation() - .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()) > Settings.LED.APRIL_TAG_DISTANCE_THRESHOLD) { - needToBeTwinkle = true; - } - else { - needToBeTwinkle = false; - } - - //TODO: potentially make this go both ways - if need be - boolean shouldClear = (cachedState.getAnimation() instanceof TwinkleAnimation && state.getAnimation() instanceof SolidColor) ? true : false; + // if (initialPoseUpdated && + // lastPoseOnAprilTag.getTranslation() + // .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()) > Settings.LED.APRIL_TAG_DISTANCE_THRESHOLD) { + // + // } if (cachedState != state) { - //not clearing animations - //instance of locks me out - if (shouldClear) leds.clearAllAnimations(); - ledPattern = needToBeTwinkle ? Settings.LED.twinkleAnimation : Settings.LED.solidColorRequest; - - if (ledPattern instanceof SolidColor) { - SolidColor solidColor = (SolidColor) ledPattern; - solidColor.withColor(state.getColor()); - } - else if (ledPattern instanceof TwinkleAnimation) { - TwinkleAnimation twinkleAnimation = (TwinkleAnimation) ledPattern; - twinkleAnimation.withColor(state.getColor()); - } + SolidColor solidColor = (SolidColor) ledPattern; + solidColor.withColor(state.getColor()); cachedState = state; } @@ -166,16 +138,12 @@ public void changeState(LedState state) { public void periodicAfterScheduler() { - if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || - Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || - Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { - lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); - initialPoseUpdated = true; - } - - DogLog.log("LED/last pose updated", this.lastPoseOnAprilTag); - DogLog.log("LED/distance from last pose", lastPoseOnAprilTag.getTranslation() - .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation())); + // if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || + // Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || + // Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { + // lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); + // initialPoseUpdated = true; + // } if (RobotContainer.EnabledSubsystems.LEDS.get()) { applyPattern(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 1468532a..b9d437e7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -430,8 +430,6 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); } - - //TODO: save the pose here } public Pose2d getPose() { From 8a049bfffa5e479fc041bdcae6a09113aa356d40 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Sat, 20 Jun 2026 07:08:15 -0400 Subject: [PATCH 89/97] feat: auton changes for duel. Began cleaning up paths (only grouped right and left for now) --- .../autos/BC Left Corner Bite CD.auto | 2 +- .../deploy/pathplanner/autos/BC Slow.auto | 43 ++++ .../deploy/pathplanner/autos/Champs + NY.auto | 43 ++++ src/main/deploy/pathplanner/autos/Champs.auto | 43 ++++ src/main/deploy/pathplanner/autos/NY.auto | 43 ++++ .../Champs Left Bite Score To Score.path | 145 ++++++++++++ .../paths/Champs Left Score To Corner.path | 54 +++++ .../paths/Champs Left Shallow To Score.path | 59 +++++ .../paths/Champs Left To Shallow.path | 81 +++++++ .../Champs Right Bite Score To Score.path | 145 ++++++++++++ ...path => Champs Right Score To Corner.path} | 32 +-- ...ath => Champs Right Shallow To Score.path} | 35 +-- .../paths/Champs Right To Shallow.path | 81 +++++++ .../Left Corner Bite Anti Collision.path | 4 +- .../pathplanner/paths/Left Corner Bite.path | 8 +- ...t2.path => Left Corner To Center Dot.path} | 35 ++- ...t v2.path => Left Corner To Dot Turn.path} | 0 .../Left NZ To Score Anti Collision.path | 2 +- .../pathplanner/paths/Left NZ To Score.path | 8 +- .../paths/Left Score To NZ (F).path | 8 +- .../paths/Left Shallow To Score.path | 4 +- .../pathplanner/paths/Left To Shallow.path | 2 +- .../pathplanner/paths/Left Trench To NZ.path | 8 +- .../paths/NY Left NZ To Score.path | 79 +++++++ .../paths/NY Left Score To NZ (F).path | 59 +++++ .../paths/NY Left Score To Score.path | 127 +++++++++++ .../paths/NY Left Trench To NZ.path | 73 ++++++ ...Dot XXX.path => NY Right NZ To Score.path} | 48 ++-- ...pt1.path => NY Right Score To NZ (F).path} | 35 +-- .../paths/NY Right Score To Score.path | 111 +++++++++ .../paths/NY Right Trench To NZ.path | 73 ++++++ .../Right Corner Bite Anti Collision.path | 2 +- .../pathplanner/paths/Right Corner Bite.path | 8 +- .../paths/Right Corner To Center Dot.path | 50 ++--- ...Dot.path => Right Corner To Dot Turn.path} | 18 +- .../paths/Right Corner To Dot.path | 64 ++++-- .../paths/Right Corner to Dot v2.path | 97 -------- .../Right NZ To Score Anti Collision.path | 2 +- .../pathplanner/paths/Right NZ To Score.path | 8 +- .../paths/Right Score To NZ (F).path | 8 +- .../paths/Right Score To Score NY.path | 8 +- .../paths/Right Shallow To Score.path | 2 +- .../pathplanner/paths/Right To Shallow.path | 2 +- .../pathplanner/paths/Right Trench To NZ.path | 8 +- src/main/deploy/pathplanner/settings.json | 7 +- .../com/stuypulse/robot/RobotContainer.java | 212 ++++++++++-------- .../BCAuton.java} | 6 +- .../{regular => deprecated}/LeftFollow.java | 5 +- .../ShallowSwipeDot.java | 4 +- .../regular/{LeftBump.java => Bump.java} | 6 +- .../auton/regular/CenterTwoCornerBC.java | 82 ------- .../auton/regular/RightTwoCornerShallow.java | 94 -------- .../{LeftTwoCorner.java => TwoCorner.java} | 6 +- ...rnerShallow.java => TwoCornerShallow.java} | 16 +- 54 files changed, 1630 insertions(+), 575 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/BC Slow.auto create mode 100644 src/main/deploy/pathplanner/autos/Champs + NY.auto create mode 100644 src/main/deploy/pathplanner/autos/Champs.auto create mode 100644 src/main/deploy/pathplanner/autos/NY.auto create mode 100644 src/main/deploy/pathplanner/paths/Champs Left Bite Score To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Champs Left Score To Corner.path create mode 100644 src/main/deploy/pathplanner/paths/Champs Left Shallow To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Champs Left To Shallow.path create mode 100644 src/main/deploy/pathplanner/paths/Champs Right Bite Score To Score.path rename src/main/deploy/pathplanner/paths/{Right Corner To Center Dot pt2.path => Champs Right Score To Corner.path} (60%) rename src/main/deploy/pathplanner/paths/{Left Corner To Center Dot pt1.path => Champs Right Shallow To Score.path} (58%) create mode 100644 src/main/deploy/pathplanner/paths/Champs Right To Shallow.path rename src/main/deploy/pathplanner/paths/{Left Corner To Center Dot pt2.path => Left Corner To Center Dot.path} (61%) rename src/main/deploy/pathplanner/paths/{Left Corner to Dot v2.path => Left Corner To Dot Turn.path} (100%) create mode 100644 src/main/deploy/pathplanner/paths/NY Left NZ To Score.path create mode 100644 src/main/deploy/pathplanner/paths/NY Left Score To NZ (F).path create mode 100644 src/main/deploy/pathplanner/paths/NY Left Score To Score.path create mode 100644 src/main/deploy/pathplanner/paths/NY Left Trench To NZ.path rename src/main/deploy/pathplanner/paths/{Left Corner To Center Dot XXX.path => NY Right NZ To Score.path} (61%) rename src/main/deploy/pathplanner/paths/{Right Corner To Center Dot pt1.path => NY Right Score To NZ (F).path} (58%) create mode 100644 src/main/deploy/pathplanner/paths/NY Right Score To Score.path create mode 100644 src/main/deploy/pathplanner/paths/NY Right Trench To NZ.path rename src/main/deploy/pathplanner/paths/{Left Corner To Dot.path => Right Corner To Dot Turn.path} (78%) delete mode 100644 src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path rename src/main/java/com/stuypulse/robot/commands/auton/{regular/TwoCornerBC.java => deprecated/BCAuton.java} (95%) rename src/main/java/com/stuypulse/robot/commands/auton/{regular => deprecated}/LeftFollow.java (92%) rename src/main/java/com/stuypulse/robot/commands/auton/{regular => deprecated}/ShallowSwipeDot.java (96%) rename src/main/java/com/stuypulse/robot/commands/auton/regular/{LeftBump.java => Bump.java} (91%) delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java rename src/main/java/com/stuypulse/robot/commands/auton/regular/{LeftTwoCorner.java => TwoCorner.java} (96%) rename src/main/java/com/stuypulse/robot/commands/auton/regular/{LeftTwoCornerShallow.java => TwoCornerShallow.java} (92%) diff --git a/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto b/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto index a1a050dd..06fa4b7c 100644 --- a/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto +++ b/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto @@ -37,7 +37,7 @@ { "type": "path", "data": { - "pathName": "Left Corner To Center Dot XXX" + "pathName": null } } ] diff --git a/src/main/deploy/pathplanner/autos/BC Slow.auto b/src/main/deploy/pathplanner/autos/BC Slow.auto new file mode 100644 index 00000000..85471c5f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Slow.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Corner Bite Anti Collision" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Score Anti Collision" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Right Score To Score NY" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Duel", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Champs + NY.auto b/src/main/deploy/pathplanner/autos/Champs + NY.auto new file mode 100644 index 00000000..fe2fdcc4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Champs + NY.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Champs Right To Shallow" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Shallow To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Score NY" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Duel", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Champs.auto b/src/main/deploy/pathplanner/autos/Champs.auto new file mode 100644 index 00000000..7d24a972 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Champs.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Champs Right To Shallow" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Shallow To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Bite Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Duel", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/NY.auto b/src/main/deploy/pathplanner/autos/NY.auto new file mode 100644 index 00000000..a62cb56b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/NY.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "NY Right Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "NY Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "NY Right Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Duel", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Left Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Champs Left Bite Score To Score.path new file mode 100644 index 00000000..f65f9f2d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Left Bite Score To Score.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.3075858778625955, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 7.564404708105101, + "y": 7.509029957203994 + }, + "isLocked": false, + "linkedName": "Left Corner" + }, + { + "anchor": { + "x": 7.7265763195435095, + "y": 5.904636233951498 + }, + "prevControl": { + "x": 7.687760342368046, + "y": 7.884251069900142 + }, + "nextControl": { + "x": 7.76360928403665, + "y": 4.015955044801327 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.794992867332382, + "y": 3.782696148359487 + }, + "prevControl": { + "x": 7.6166741233975355, + "y": 3.782696148359487 + }, + "nextControl": { + "x": 5.966918687589157, + "y": 3.782696148359487 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.109243937232525, + "y": 6.059900142653353 + }, + "prevControl": { + "x": 6.143981742457801, + "y": 4.624070860008561 + }, + "nextControl": { + "x": 6.070427960057062, + "y": 7.664293865905849 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.328549618320611, + "y": 7.591536259541984 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.307036247334758, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.8272921108741973, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.2366737739872133, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.7421203438395327, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.6183368869935886, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.0533049040511733, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.3176972281449895, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8835202761000936, + "maxWaypointRelativePos": 3.057808455565133, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Left Score To Corner.path b/src/main/deploy/pathplanner/paths/Champs Left Score To Corner.path new file mode 100644 index 00000000..082d1fcb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Left Score To Corner.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 3.2238741384420084, + "y": 7.349786421407005 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 3.3075858778625955, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 3.553822420306369, + "y": 7.484121823389645 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Corner" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.1, + "maxAcceleration": 4.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score.path b/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score.path new file mode 100644 index 00000000..3a39c558 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 5.374151212553495 + }, + "prevControl": null, + "nextControl": { + "x": 6.235545760690833, + "y": 7.811409368840588 + }, + "isLocked": false, + "linkedName": "Left Shallow" + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.085868320610687, + "y": 7.650114503816795 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3816631130063977, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Left To Shallow.path b/src/main/deploy/pathplanner/paths/Champs Left To Shallow.path new file mode 100644 index 00000000..53fa076b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Left To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 6.639728958630526, + "y": 7.677232524964337 + }, + "isLocked": false, + "linkedName": "Left Trench Start" + }, + { + "anchor": { + "x": 7.569, + "y": 5.374151212553495 + }, + "prevControl": { + "x": 7.361981455064195, + "y": 7.133808844507846 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Shallow" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.9253731343283487, + "rotationDegrees": -55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6867989646246767, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Champs Right Bite Score To Score.path new file mode 100644 index 00000000..238c139c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Right Bite Score To Score.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 5.2357375178316685, + "y": 0.5719386590584888 + }, + "isLocked": false, + "linkedName": "Right Corner" + }, + { + "anchor": { + "x": 7.739514978601996, + "y": 1.0008844507845935 + }, + "prevControl": { + "x": 7.7167792788333145, + "y": 0.31502417442937847 + }, + "nextControl": { + "x": 7.817146932952923, + "y": 3.3427817403708993 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.002011412268189, + "y": 3.925021398002854 + }, + "prevControl": { + "x": 8.114526380651917, + "y": 3.9034191656070543 + }, + "nextControl": { + "x": 5.669329529243937, + "y": 3.9508987161198283 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.031611982881596, + "y": 2.0877318116975756 + }, + "prevControl": { + "x": 6.009732033987853, + "y": 3.174435940086786 + }, + "nextControl": { + "x": 6.070427960057061, + "y": 0.15987161198288047 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 6.147977175463622, + "y": 0.5460613409415126 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5714285714285716, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.9722814498933815, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.3475479744136523, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.936034115138597, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.4818763326225852, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.0618336886993425, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.4968017057569094, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0353753235547816, + "maxWaypointRelativePos": 3.1406384814495194, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt2.path b/src/main/deploy/pathplanner/paths/Champs Right Score To Corner.path similarity index 60% rename from src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt2.path rename to src/main/deploy/pathplanner/paths/Champs Right Score To Corner.path index 07002c83..adfdfdba 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt2.path +++ b/src/main/deploy/pathplanner/paths/Champs Right Score To Corner.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 6.27, - "y": 0.57 + "x": 3.612, + "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 6.436401056802655, - "y": 0.7829836402426446 + "x": 3.199971130397284, + "y": 0.6068359978960554 }, "isLocked": false, - "linkedName": null + "linkedName": "Right Trench Score" }, { "anchor": { - "x": 8.289, - "y": 4.064233333333333 + "x": 3.282, + "y": 0.559 }, "prevControl": { - "x": 8.135084631168585, - "y": 3.8672306449316527 + "x": 3.5164317899184905, + "y": 0.4721568317274595 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Right Corner" } ], "rotationTargets": [], @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.19, - "maxAcceleration": 10.0, + "maxVelocity": 0.1, + "maxAcceleration": 4.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 0.0 }, "reversed": false, - "folder": "BC To Dot", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt1.path b/src/main/deploy/pathplanner/paths/Champs Right Shallow To Score.path similarity index 58% rename from src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt1.path rename to src/main/deploy/pathplanner/paths/Champs Right Shallow To Score.path index 98a93f05..c8212757 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt1.path +++ b/src/main/deploy/pathplanner/paths/Champs Right Shallow To Score.path @@ -3,32 +3,37 @@ "waypoints": [ { "anchor": { - "x": 3.31, - "y": 7.44 + "x": 7.569, + "y": 2.7346647646219684 }, "prevControl": null, "nextControl": { - "x": 3.5785777777777783, - "y": 7.4359777777777785 + "x": 6.1198701854493605, + "y": 0.34101283880171307 }, "isLocked": false, - "linkedName": null + "linkedName": "Right Shallow NZ" }, { "anchor": { - "x": 6.27, - "y": 7.44 + "x": 3.612, + "y": 0.559 }, "prevControl": { - "x": 6.02, - "y": 7.44 + "x": 6.018590584878744, + "y": 0.3649201141226812 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 } ], - "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -41,14 +46,14 @@ "unlimited": false }, "goalEndState": { - "velocity": 3.8, + "velocity": 0, "rotation": 0.0 }, "reversed": false, - "folder": "BC To Dot", + "folder": "Shallow", "idealStartingState": { - "velocity": 0.0, - "rotation": 0.0 + "velocity": 0, + "rotation": 90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Right To Shallow.path b/src/main/deploy/pathplanner/paths/Champs Right To Shallow.path new file mode 100644 index 00000000..9719aeb3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Right To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 6.939437022900763, + "y": 0.36130725190839597 + }, + "isLocked": false, + "linkedName": "Right Trench Start" + }, + { + "anchor": { + "x": 7.569, + "y": 2.7346647646219684 + }, + "prevControl": { + "x": 7.602473282442749, + "y": 1.5045216348509767 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Shallow NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.20916905444126394, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.5200573065902583, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.7736389684813755, + "rotationDegrees": 55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6143226919758464, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path b/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path index 27f664c2..5494bd8c 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 7.267033333333333, - "y": 7.940311111111112 + "y": 7.940311111111111 }, "nextControl": null, "isLocked": false, @@ -72,7 +72,7 @@ "rotation": -90.0 }, "reversed": false, - "folder": "Anti-Anti Collision", + "folder": "Anti Collision", "idealStartingState": { "velocity": 0, "rotation": -90.0 diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite.path b/src/main/deploy/pathplanner/paths/Left Corner Bite.path index e29ed1fc..2f188ae5 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.036133333333334, - "y": 4.62941111111111 + "x": 8.248481613285884, + "y": 4.621376037959667 }, "prevControl": { - "x": 7.539166666666667, - "y": 7.591722222222222 + "x": 7.751514946619217, + "y": 7.583687149070779 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt2.path b/src/main/deploy/pathplanner/paths/Left Corner To Center Dot.path similarity index 61% rename from src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt2.path rename to src/main/deploy/pathplanner/paths/Left Corner To Center Dot.path index caaf09af..12fedec1 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner To Center Dot pt2.path +++ b/src/main/deploy/pathplanner/paths/Left Corner To Center Dot.path @@ -3,13 +3,29 @@ "waypoints": [ { "anchor": { - "x": 6.27, - "y": 7.44 + "x": 3.282, + "y": 7.441 }, "prevControl": null, "nextControl": { - "x": 6.436401056802655, - "y": 7.227016359757356 + "x": 6.282, + "y": 7.441 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.838, + "y": 7.179 + }, + "prevControl": { + "x": 6.7323454345648255, + "y": 7.405576946759163 + }, + "nextControl": { + "x": 7.209490856875041, + "y": 6.382335286525449 }, "isLocked": false, "linkedName": null @@ -20,15 +36,20 @@ "y": 4.064233333333333 }, "prevControl": { - "x": 8.135084631168585, - "y": 4.261236021735014 + "x": 8.164, + "y": 4.2807396842794425 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], diff --git a/src/main/deploy/pathplanner/paths/Left Corner to Dot v2.path b/src/main/deploy/pathplanner/paths/Left Corner To Dot Turn.path similarity index 100% rename from src/main/deploy/pathplanner/paths/Left Corner to Dot v2.path rename to src/main/deploy/pathplanner/paths/Left Corner To Dot Turn.path diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path b/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path index f4c7c772..703a4b27 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path @@ -70,7 +70,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Anti-Anti Collision", + "folder": "Anti Collision", "idealStartingState": { "velocity": 0.0, "rotation": -90.0 diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 523fd574..2415dfc2 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.036133333333334, - "y": 4.62941111111111 + "x": 8.248481613285884, + "y": 4.621376037959667 }, "prevControl": null, "nextControl": { - "x": 6.389322222222223, - "y": 4.580688888888889 + "x": 6.601670502174773, + "y": 4.572653815737446 }, "isLocked": false, "linkedName": "Left NZ" diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index a7cb3bb8..b8c3689a 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.036133333333334, - "y": 4.62941111111111 + "x": 8.248481613285884, + "y": 4.621376037959667 }, "prevControl": { - "x": 7.647973561578697, - "y": 7.204204263750197 + "x": 7.860321841531247, + "y": 7.196169190598754 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Shallow To Score.path b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path index 86f7bf60..3a39c558 100644 --- a/src/main/deploy/pathplanner/paths/Left Shallow To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 6.235545760690834, + "x": 6.235545760690833, "y": 7.811409368840588 }, "isLocked": false, @@ -50,7 +50,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Non-Collision", + "folder": "Shallow", "idealStartingState": { "velocity": 0, "rotation": -90.0 diff --git a/src/main/deploy/pathplanner/paths/Left To Shallow.path b/src/main/deploy/pathplanner/paths/Left To Shallow.path index de8407c0..53fa076b 100644 --- a/src/main/deploy/pathplanner/paths/Left To Shallow.path +++ b/src/main/deploy/pathplanner/paths/Left To Shallow.path @@ -72,7 +72,7 @@ "rotation": -90.0 }, "reversed": false, - "folder": "Non-Collision", + "folder": "Shallow", "idealStartingState": { "velocity": 0, "rotation": -90.0 diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 2b6b9aa3..3568773d 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.036133333333334, - "y": 4.62941111111111 + "x": 8.248481613285884, + "y": 4.621376037959667 }, "prevControl": { - "x": 7.984378697099382, - "y": 7.450038785861467 + "x": 8.196726977051933, + "y": 7.442003712710024 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/NY Left NZ To Score.path b/src/main/deploy/pathplanner/paths/NY Left NZ To Score.path new file mode 100644 index 00000000..52c62217 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Left NZ To Score.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.248481613285884, + "y": 4.621376037959667 + }, + "prevControl": null, + "nextControl": { + "x": 5.7738671411625155, + "y": 4.589098457888493 + }, + "isLocked": false, + "linkedName": "Left NZ" + }, + { + "anchor": { + "x": 6.052395038167939, + "y": 6.378129770992366 + }, + "prevControl": { + "x": 6.055976129616249, + "y": 5.761441920527078 + }, + "nextControl": { + "x": 6.044550641940085, + "y": 7.7289871611982885 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.018673323823109, + "y": 7.444336661911555 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.3432835820895521, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NY Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/NY Left Score To NZ (F).path new file mode 100644 index 00000000..4bd6d40f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Left Score To NZ (F).path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 6.846747503566332, + "y": 7.440906488549619 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 8.248481613285884, + "y": 4.621376037959667 + }, + "prevControl": { + "x": 7.860321841531247, + "y": 7.196169190598754 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1902339776195354, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NY Left Score To Score.path b/src/main/deploy/pathplanner/paths/NY Left Score To Score.path new file mode 100644 index 00000000..8a3f8436 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Left Score To Score.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 6.717360912981454, + "y": 7.521968616262482 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 5.863409415121255, + "y": 5.244764621968616 + }, + "prevControl": { + "x": 5.962233970579355, + "y": 7.616553952963002 + }, + "nextControl": { + "x": 5.824593437945791, + "y": 4.31318116975749 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.937318116975749, + "y": 4.571954350927247 + }, + "prevControl": { + "x": 6.567013040113212, + "y": 4.295700209513629 + }, + "nextControl": { + "x": 7.5790326308920575, + "y": 5.0506847360913 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0667047075606275, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 7.3644732636065955, + "y": 7.433121689698743 + }, + "nextControl": { + "x": 5.087089871611983, + "y": 7.49266112478357 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 5.294108416547789, + "y": 7.399604078672524 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1891117478510029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.5, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.2, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NY Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/NY Left Trench To NZ.path new file mode 100644 index 00000000..3568773d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Left Trench To NZ.path @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 8.517461447212337, + "y": 7.70926453143535 + }, + "isLocked": false, + "linkedName": "Left Trench Start" + }, + { + "anchor": { + "x": 8.248481613285884, + "y": 4.621376037959667 + }, + "prevControl": { + "x": 8.196726977051933, + "y": 7.442003712710024 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.140401146131807, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.55, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Center Dot XXX.path b/src/main/deploy/pathplanner/paths/NY Right NZ To Score.path similarity index 61% rename from src/main/deploy/pathplanner/paths/Left Corner To Center Dot XXX.path rename to src/main/deploy/pathplanner/paths/NY Right NZ To Score.path index 194ab52d..74fbafa9 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner To Center Dot XXX.path +++ b/src/main/deploy/pathplanner/paths/NY Right NZ To Score.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 3.282, - "y": 7.441 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": null, "nextControl": { - "x": 5.209691738594327, - "y": 7.581929716399507 + "x": 5.763107947805457, + "y": 3.5131791221826814 }, "isLocked": false, - "linkedName": null + "linkedName": "Right NZ" }, { "anchor": { - "x": 6.20456226879871, - "y": 6.203 + "x": 6.053851272542522, + "y": 1.6151808747904899 }, "prevControl": { - "x": 6.237003699136868, - "y": 7.419722564734895 + "x": 6.040480921648686, + "y": 2.6846376869648685 }, "nextControl": { - "x": 6.174004896303544, - "y": 5.056939414929412 + "x": 6.070427960057061, + "y": 0.289258202567761 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.27, - "y": 4.067 + "x": 3.6120827389443653, + "y": 0.559 }, "prevControl": { - "x": 5.685499383471953, - "y": 4.089069050550844 + "x": 6.290385164051354, + "y": 0.5848773181169763 }, "nextControl": null, "isLocked": false, @@ -46,16 +46,12 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.23, - "rotationDegrees": 0.0 + "waypointRelativePos": 0.23445825932504563, + "rotationDegrees": 90.0 }, { - "waypointRelativePos": 1.109909909909912, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 1.754954954954955, - "rotationDegrees": 0.4456470247826539 + "waypointRelativePos": 1.488272921108742, + "rotationDegrees": 0.0 } ], "constraintZones": [], @@ -71,13 +67,13 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 90.0 + "rotation": 0.0 }, "reversed": false, - "folder": "BC To Dot", + "folder": "To Score", "idealStartingState": { "velocity": 0.0, - "rotation": 0.0 + "rotation": 90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt1.path b/src/main/deploy/pathplanner/paths/NY Right Score To NZ (F).path similarity index 58% rename from src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt1.path rename to src/main/deploy/pathplanner/paths/NY Right Score To NZ (F).path index a483881a..82c6f9a0 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot pt1.path +++ b/src/main/deploy/pathplanner/paths/NY Right Score To NZ (F).path @@ -3,32 +3,37 @@ "waypoints": [ { "anchor": { - "x": 3.28, - "y": 0.56 + "x": 3.612, + "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 3.548577777777778, - "y": 0.5559777777777778 + "x": 7.183069900142653, + "y": 0.5460613409415126 }, "isLocked": false, - "linkedName": null + "linkedName": "Right Trench Score" }, { "anchor": { - "x": 6.27, - "y": 0.57 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": { - "x": 6.02, - "y": 0.57 + "x": 8.185967783694872, + "y": 0.697048513985274 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Right NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.13936927772126403, + "rotationDegrees": 0.0 } ], - "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -41,13 +46,13 @@ "unlimited": false }, "goalEndState": { - "velocity": 3.8, - "rotation": 0.0 + "velocity": 0, + "rotation": 90.0 }, "reversed": false, - "folder": "BC To Dot", + "folder": "To NZ", "idealStartingState": { - "velocity": 0.0, + "velocity": 0, "rotation": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/NY Right Score To Score.path b/src/main/deploy/pathplanner/paths/NY Right Score To Score.path new file mode 100644 index 00000000..d847f06a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Right Score To Score.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.244952924393722, + "y": 0.6236932952924387 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.850470756062768, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 5.850470756062768, + "y": 0.3280741797432247 + }, + "nextControl": { + "x": 5.850470756062768, + "y": 4.150659142168011 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.302881844380403, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 7.215816731986725, + "y": 3.83143356936277 + }, + "nextControl": { + "x": 7.525057636887608, + "y": 0.34949567723342856 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 7.673006390654899, + "y": 0.520948709769672 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17051509769094172, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.644760213143872, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.289978678038381, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.6703967446591785, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NY Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/NY Right Trench To NZ.path new file mode 100644 index 00000000..13b73e23 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Right Trench To NZ.path @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 9.06721902017291, + "y": 0.41484149855907726 + }, + "isLocked": false, + "linkedName": "Right Trench Start" + }, + { + "anchor": { + "x": 8.237722419928826, + "y": 3.4271055753262156 + }, + "prevControl": { + "x": 8.14623827007292, + "y": 0.4342669586115182 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.12255772646536249, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.55, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path b/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path index 97fcc686..9e9109a1 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path @@ -72,7 +72,7 @@ "rotation": 90.0 }, "reversed": false, - "folder": "Anti-Anti Collision", + "folder": "Anti Collision", "idealStartingState": { "velocity": 0, "rotation": 90.0 diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite.path b/src/main/deploy/pathplanner/paths/Right Corner Bite.path index 5772cb4b..c6fec780 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.909455555555555, - "y": 3.508799999999999 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": { - "x": 7.948433333333332, - "y": 2.2030444444444446 + "x": 8.276700197706603, + "y": 2.121350019770661 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path index 6e2fd9e1..a68ee711 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 3.48, + "x": 3.282, "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 5.818616522811345, - "y": 0.531325524044389 + "x": 6.282, + "y": 0.559 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.20456226879871, - "y": 1.7965413070243335 + "x": 6.837566666666667, + "y": 0.8193333333333328 }, "prevControl": { - "x": 6.193748458692971, - "y": 0.6502774352651044 + "x": 6.731912101231492, + "y": 0.5927563865741706 }, "nextControl": { - "x": 6.2118058772904625, - "y": 2.564363807519222 + "x": 7.209057523541707, + "y": 1.6159980468078845 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.27, - "y": 4.067 + "x": 8.289, + "y": 4.064233333333333 }, "prevControl": { - "x": 5.685499383471953, - "y": 4.089069050550844 + "x": 8.164, + "y": 3.8477269823872233 }, "nextControl": null, "isLocked": false, @@ -46,20 +46,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.23, + "waypointRelativePos": 0.5, "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1.109909909909912, - "rotationDegrees": 90.0 - }, - { - "waypointRelativePos": 1.6432432432432433, - "rotationDegrees": 0.4456470247826539 - }, - { - "waypointRelativePos": 1.7, - "rotationDegrees": -10.05657822827732 } ], "constraintZones": [], @@ -70,17 +58,17 @@ "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, - "unlimited": false + "nominalVoltage": 12.0, + "unlimited": true }, "goalEndState": { - "velocity": 0.0, - "rotation": 180.0 + "velocity": 0, + "rotation": -119.99999999999999 }, "reversed": false, "folder": "BC To Dot", "idealStartingState": { - "velocity": 0.0, + "velocity": 0, "rotation": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot Turn.path similarity index 78% rename from src/main/deploy/pathplanner/paths/Left Corner To Dot.path rename to src/main/deploy/pathplanner/paths/Right Corner To Dot Turn.path index 8e3881c6..cbfe0b65 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner To Dot.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot Turn.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.31, - "y": 7.44 + "x": 3.28, + "y": 0.56 }, "prevControl": null, "nextControl": { - "x": 4.835616333629863, - "y": 7.4478979018164315 + "x": 4.440424253599775, + "y": 0.56 }, "isLocked": false, "linkedName": null @@ -17,11 +17,11 @@ { "anchor": { "x": 8.254, - "y": 7.441 + "y": 0.559 }, "prevControl": { - "x": 7.931262293036603, - "y": 7.447711041684223 + "x": 8.003999999999998, + "y": 0.559 }, "nextControl": null, "isLocked": false, @@ -32,10 +32,6 @@ { "waypointRelativePos": 0.58, "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.59, - "rotationDegrees": -2.0 } ], "constraintZones": [], diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path index cbfe0b65..723eaf99 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.28, - "y": 0.56 + "x": 3.308, + "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 4.440424253599775, - "y": 0.56 + "x": 6.911445078005017, + "y": -0.11712412737826428 }, "isLocked": false, "linkedName": null @@ -17,11 +17,27 @@ { "anchor": { "x": 8.254, - "y": 0.559 + "y": 1.927 }, "prevControl": { - "x": 8.003999999999998, - "y": 0.559 + "x": 6.868618724968162, + "y": 2.827330417029173 + }, + "nextControl": { + "x": 8.63228091821551, + "y": 1.6811631152454245 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 0.759 + }, + "prevControl": { + "x": 8.254, + "y": 1.0090000000000008 }, "nextControl": null, "isLocked": false, @@ -30,11 +46,33 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.58, - "rotationDegrees": 0.0 + "waypointRelativePos": 0.3837953091684457, + "rotationDegrees": 35.0 + }, + { + "waypointRelativePos": 0.6652452025586366, + "rotationDegrees": 33.0 + }, + { + "waypointRelativePos": 1.35, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.5, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.0, + "unlimited": false + } } ], - "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -43,11 +81,11 @@ "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 90.0 }, "reversed": false, "folder": "BC To Dot", @@ -55,5 +93,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path b/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path deleted file mode 100644 index 723eaf99..00000000 --- a/src/main/deploy/pathplanner/paths/Right Corner to Dot v2.path +++ /dev/null @@ -1,97 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.308, - "y": 0.559 - }, - "prevControl": null, - "nextControl": { - "x": 6.911445078005017, - "y": -0.11712412737826428 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.254, - "y": 1.927 - }, - "prevControl": { - "x": 6.868618724968162, - "y": 2.827330417029173 - }, - "nextControl": { - "x": 8.63228091821551, - "y": 1.6811631152454245 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.254, - "y": 0.759 - }, - "prevControl": { - "x": 8.254, - "y": 1.0090000000000008 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3837953091684457, - "rotationDegrees": 35.0 - }, - { - "waypointRelativePos": 0.6652452025586366, - "rotationDegrees": 33.0 - }, - { - "waypointRelativePos": 1.35, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.5, - "maxWaypointRelativePos": 2.0, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.19, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "BC To Dot", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path b/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path index 7bf6063d..01f7654f 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path @@ -70,7 +70,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Anti-Anti Collision", + "folder": "Anti Collision", "idealStartingState": { "velocity": 0.0, "rotation": 90.0 diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 263ff2ce..27bdc626 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.909455555555555, - "y": 3.508799999999999 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": null, "nextControl": { - "x": 6.204177777777778, - "y": 3.4113555555555553 + "x": 6.532444642151049, + "y": 3.329661130881772 }, "isLocked": false, "linkedName": "Right NZ" diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 5be40485..a7c55caf 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.909455555555555, - "y": 3.508799999999999 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": { - "x": 7.857700919321601, - "y": 0.7787429386590574 + "x": 8.185967783694872, + "y": 0.697048513985274 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score NY.path b/src/main/deploy/pathplanner/paths/Right Score To Score NY.path index 5dc3f595..8ebe272a 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score NY.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score NY.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.612, + "x": 3.282, "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 6.574952924393722, + "x": 6.244952924393722, "y": 0.6236932952924387 }, "isLocked": false, - "linkedName": "Right Trench Score" + "linkedName": null }, { "anchor": { @@ -52,7 +52,7 @@ "y": 0.559 }, "prevControl": { - "x": 7.673006390654899, + "x": 7.673006390654898, "y": 0.520948709769672 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path index 151d3989..f57f83cb 100644 --- a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path @@ -50,7 +50,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Non-Collision", + "folder": "Shallow", "idealStartingState": { "velocity": 0, "rotation": 90.0 diff --git a/src/main/deploy/pathplanner/paths/Right To Shallow.path b/src/main/deploy/pathplanner/paths/Right To Shallow.path index 4aedf86c..9719aeb3 100644 --- a/src/main/deploy/pathplanner/paths/Right To Shallow.path +++ b/src/main/deploy/pathplanner/paths/Right To Shallow.path @@ -72,7 +72,7 @@ "rotation": 90.0 }, "reversed": false, - "folder": "Non-Collision", + "folder": "Shallow", "idealStartingState": { "velocity": 0.0, "rotation": 90.0 diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 18200b70..e5cb1843 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.909455555555555, - "y": 3.508799999999999 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": { - "x": 7.81797140569965, - "y": 0.5159613832853016 + "x": 8.14623827007292, + "y": 0.4342669586115182 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 4c890d06..f52289fc 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,20 +3,21 @@ "robotLength": 0.762, "holonomicMode": true, "pathFolders": [ - "Anti-Anti Collision", + "Anti Collision", "BC To Dot", "BC modified", "Bump Stuff", "Follow", - "Non-Collision", + "Shallow", "PathFinder Test", "To Depot", "To NZ", "To Score" ], "autoFolders": [ + "BC Extras", "BC Main", - "BC Extras" + "Duel" ], "defaultMaxVel": 4.19, "defaultMaxAccel": 10.0, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index dd01d668..dbc7bc89 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,22 +7,8 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.auton.regular.Depot; -import com.stuypulse.robot.commands.auton.regular.LeftBump; -import com.stuypulse.robot.commands.auton.regular.LeftFollow; -import com.stuypulse.robot.commands.auton.regular.LeftTwoCorner; -import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerShallow; -import com.stuypulse.robot.commands.auton.regular.LeftTwoCornerVariant; -import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; -import com.stuypulse.robot.commands.auton.regular.RightBump; -import com.stuypulse.robot.commands.auton.regular.RightFollow; -import com.stuypulse.robot.commands.auton.regular.RightTwoCorner; -import com.stuypulse.robot.commands.auton.regular.RightTwoCornerShallow; -import com.stuypulse.robot.commands.auton.regular.RightTwoCornerVariant; -import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; -import com.stuypulse.robot.commands.auton.regular.ShallowSwipeDot; -import com.stuypulse.robot.commands.auton.test.PathfindTest; -import com.stuypulse.robot.commands.auton.test.TestBC; +import com.stuypulse.robot.commands.auton.regular.TwoCorner; +import com.stuypulse.robot.commands.auton.regular.TwoCornerShallow; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hood.HomingRoutineLower; @@ -400,73 +386,115 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); // DEPOT - AutonConfig DEPOT_ONLY = new AutonConfig("Depot Only", Depot::new, prevWaitTimeOne, prevWaitTimeTwo, - "Center Hub To Depot"); - DEPOT_ONLY.register(autonChooser); - - AutonConfig LEFT_BUMP = new AutonConfig("Left Bump", LeftBump::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Bump To Score (Start)", "Left Bump To Score", "Left Bump Score To Depot"); - LEFT_BUMP.register(autonChooser); - - AutonConfig RIGHT_BUMP = new AutonConfig("Right Bump", RightBump::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Bump To Score (Start)", "Right Bump To Score", "Right Bump Score To Depot"); - RIGHT_BUMP.register(autonChooser); - - // TWO CYCLES (TRENCH) - AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Trench To NZ", "Left NZ To Score", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); - LEFT_TWO_CYCLE.register(autonChooser); - - AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); - RIGHT_TWO_CYCLE.register(autonChooser); - - AutonConfig BC_TEST = new AutonConfig("BC Test", TestBC::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner to Dot"); - BC_TEST.register(autonChooser); - - // TWO CYCLES (CORNER) - AutonConfig L_CN_FN = new AutonConfig("Left Corner-Near Far-Near", LeftTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); - L_CN_FN.register(autonChooser); - - AutonConfig R_CN_FN = new AutonConfig("Right Corner-Near Far-Near", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); - R_CN_FN.register(autonChooser); - - AutonConfig L_FNS_FN = new AutonConfig("Left Far-Near Shallow Far-Near", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left To Shallow", "Left Shallow To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); - L_FNS_FN.register(autonChooser); - - AutonConfig R_FNS_FN = new AutonConfig("Right Far-Near Shallow Far-Near", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right To Shallow", "Right Shallow To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); - R_FNS_FN.register(autonChooser); - - AutonConfig L_CN_NF = new AutonConfig("Left Corner-Near Near-Far", LeftTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); - L_CN_NF.register(autonChooser); - - AutonConfig R_CN_NF = new AutonConfig("Right Corner-Near Near-Far", RightTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); - R_CN_NF.register(autonChooser); - - AutonConfig L_CNL_D = new AutonConfig("Left Corner-Near Long Dot", ShallowSwipeDot::new, - "BC Left To Shallow", "BC Left Shallow To Score", "Left Score To Corner", "Left Corner To Dot Straight" - ); - L_CNL_D.register(autonChooser); - - AutonConfig R_CNL_D = new AutonConfig("Right Corner-Near Long Dot", ShallowSwipeDot::new, - "BC Right To Shallow", "BC Right Shallow To Score", "Right Score To Corner", "Right Corner To Dot Straight" - ); - R_CNL_D.register(autonChooser); - - AutonConfig R_CN_NFS = new AutonConfig("Right Corner-Near Near-Far-Short", RightTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "BC Right Score To Score NY", "Right Score To Corner", "Right Score To NZ (F)"); - R_CN_NFS.register(autonChooser); - - AutonConfig L_CN_NFS = new AutonConfig("Left Corner-Near Near-Far-Short", LeftTwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "BC Left Score To Score NY", "Left Score To Corner", "Left Score To NZ (F)"); - L_CN_NFS.register(autonChooser); - + // AutonConfig DEPOT_ONLY = new AutonConfig("Depot Only", Depot::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Center Hub To Depot"); + // DEPOT_ONLY.register(autonChooser); + + // AutonConfig LEFT_BUMP = new AutonConfig("Left Bump", Bump::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left Bump To Score (Start)", "Left Bump To Score", "Left Bump Score To Depot"); + // LEFT_BUMP.register(autonChooser); + + // AutonConfig RIGHT_BUMP = new AutonConfig("Right Bump", RightBump::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Bump To Score (Start)", "Right Bump To Score", "Right Bump Score To Depot"); + // RIGHT_BUMP.register(autonChooser); + + // // TWO CYCLES (TRENCH) + // AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left Trench To NZ", "Left NZ To Score", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + // LEFT_TWO_CYCLE.register(autonChooser); + + // AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + // RIGHT_TWO_CYCLE.register(autonChooser); + + // AutonConfig BC_TEST = new AutonConfig("BC Test", TestBC::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner to Dot"); + // BC_TEST.register(autonChooser); + + // // TWO CYCLES (CORNER) + // AutonConfig L_CN_FN = new AutonConfig("Left Corner-Near Far-Near", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + // L_CN_FN.register(autonChooser); + + // AutonConfig R_CN_FN = new AutonConfig("Right Corner-Near Far-Near", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + // R_CN_FN.register(autonChooser); + + // AutonConfig L_FNS_FN = new AutonConfig("Left Far-Near Shallow Far-Near", TwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left To Shallow", "Left Shallow To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + // L_FNS_FN.register(autonChooser); + + // AutonConfig R_FNS_FN = new AutonConfig("Right Far-Near Shallow Far-Near", TwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right To Shallow", "Right Shallow To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + // R_FNS_FN.register(autonChooser); + + // AutonConfig L_CN_NF = new AutonConfig("Left Corner-Near Near-Far", LeftTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + // L_CN_NF.register(autonChooser); + + // AutonConfig R_CN_NF = new AutonConfig("Right Corner-Near Near-Far", RightTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + // R_CN_NF.register(autonChooser); + + // AutonConfig L_CNL_D = new AutonConfig("Left Corner-Near Long Dot", ShallowSwipeDot::new, + // "BC Left To Shallow", "BC Left Shallow To Score", "Left Score To Corner", "Left Corner To Dot Straight" + // ); + // L_CNL_D.register(autonChooser); + + // AutonConfig R_CNL_D = new AutonConfig("Right Corner-Near Long Dot", ShallowSwipeDot::new, + // "BC Right To Shallow", "BC Right Shallow To Score", "Right Score To Corner", "Right Corner To Dot Straight" + // ); + // R_CNL_D.register(autonChooser); + + //RAN AT BATTLE CRY + // AutonConfig R_CN_NFS = new AutonConfig("Right Corner-Near Near-Far-Short", TwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "BC Right Score To Score NY", "Right Score To Corner", "Right Score To NZ (F)"); + // R_CN_NFS.register(autonChooser); + + // AutonConfig L_CN_NFS = new AutonConfig("Left Corner-Near Near-Far-Short", TwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "BC Left Score To Score NY", "Left Score To Corner", "Left Score To NZ (F)"); + // L_CN_NFS.register(autonChooser); + + //might be a duplicate of Right Far Near Shallow Far Near - if no changes to that were made + AutonConfig Right_Champs = new AutonConfig("Right Champs", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "Champs Right To Shallow", "Champs Right Shallow To Score", "Champs Right Bite Score To Score", "Champs Right Score To Corner"); + Right_Champs.register(autonChooser); + + AutonConfig Left_Champs = new AutonConfig("Left Champs", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "Champs Left To Shallow", "Champs Left Shallow To Score", "Champs Left Bite Score To Score", "Champs Left Score To Corner"); + Left_Champs.register(autonChooser); + + AutonConfig Right_NY = new AutonConfig("Right NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "NY Right Trench To NZ", "NY Right NZ To Score", "NY Right Score To Score", "Right Score To Corner"); + Right_NY.register(autonChooser); + + AutonConfig Left_NY = new AutonConfig("Left NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "NY Left Trench To NZ", "NY Left NZ To Score", "NY Left Score To Score", "Left Score To Corner"); + Left_NY.register(autonChooser); + + AutonConfig Right_Champs_NY = new AutonConfig("Right Champs NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "Champs Right To Shallow", "Champs Right Shallow To Score", "NY Right Score To Score", "Right Score To Corner"); + Right_Champs_NY.register(autonChooser); + + AutonConfig Left_Champs_NY = new AutonConfig("Left Champs NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "Champs Left To Shallow", "Champs Left Shallow To Score", "NY Left Score To Score", "Left Score To Corner"); + Left_Champs_NY.register(autonChooser); + + //BC Score To Score NY is a shorened version of NY and w the slow down + AutonConfig Right_BC = new AutonConfig("Right BC", TwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "BC Right Score To Score NY", "Right Score To Corner"); + Right_BC.register(autonChooser); + + AutonConfig Left_BC = new AutonConfig("Left BC", TwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "BC Left Score To Score NY", "Left Score To Corner"); + Left_BC.register(autonChooser); + + // AutonConfig Exp_Right_Champs = new AutonConfig("Exp Right Champs", MasterAuton::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Champs Right To Shallow", "Champs Right Shallow To Score", "Champs Right Score To Corner", "Champs Right Bite Score To Score"); + // Right_Champs.register(autonChooser); + + // AutonConfig Exp_Left_Champs = new AutonConfig("Exp Left Champs", MasterAuton::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Champs Left To Shallow", "Champs Left Shallow To Score", "Champs Left Score To Corner", "Champs Left Bite Score To Score"); + // Left_Champs.register(autonChooser); //BC RIGHT @@ -504,20 +532,20 @@ public void configureAutons() { // L_CN_NF_CD.register(autonChooser); // FOLLOWS - AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Follow To Bump", "Left Follow To Score", "Left Corner To Depot"); - LEFT_FOLLOW.register(autonChooser); + // AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left Follow To Bump", "Left Follow To Score", "Left Corner To Depot"); + // LEFT_FOLLOW.register(autonChooser); - AutonConfig RIGHT_FOLLOW = new AutonConfig("Right Follow", RightFollow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Follow To Bump", "Right Follow To Score"); - RIGHT_FOLLOW.register(autonChooser); + // AutonConfig RIGHT_FOLLOW = new AutonConfig("Right Follow", RightFollow::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Follow To Bump", "Right Follow To Score"); + // RIGHT_FOLLOW.register(autonChooser); // AutonConfig EMPTY_TEST = new AutonConfig("Empty Test", EmptyTest::new, prevWaitTimeOne, prevWaitTimeTwo, // "Right Trench Score To Corner"); // EMPTY_TEST.register(autonChooser); - AutonConfig PATH_FIND_TEST = new AutonConfig("Path Find Test", PathfindTest::new, prevWaitTimeOne, prevWaitTimeTwo, - "Straight One", "Straight Two"); - PATH_FIND_TEST.register(autonChooser); + // AutonConfig PATH_FIND_TEST = new AutonConfig("Path Find Test", PathfindTest::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Straight One", "Straight Two"); + // PATH_FIND_TEST.register(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/deprecated/BCAuton.java similarity index 95% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java rename to src/main/java/com/stuypulse/robot/commands/auton/deprecated/BCAuton.java index 386b0924..6dfe97b1 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerBC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/deprecated/BCAuton.java @@ -3,7 +3,7 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; +package com.stuypulse.robot.commands.auton.deprecated; import java.util.Set; @@ -29,9 +29,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class TwoCornerBC extends SequentialCommandGroup { +public class BCAuton extends SequentialCommandGroup { - public TwoCornerBC(PathPlannerPath... paths) { + public BCAuton(PathPlannerPath... paths) { addCommands( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/deprecated/LeftFollow.java similarity index 92% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java rename to src/main/java/com/stuypulse/robot/commands/auton/deprecated/LeftFollow.java index 822385c7..a094c5d9 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/deprecated/LeftFollow.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.regular; +package com.stuypulse.robot.commands.auton.deprecated; import java.util.Set; @@ -10,11 +10,8 @@ import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.constants.Gains.Spindexer; -import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/ShallowSwipeDot.java b/src/main/java/com/stuypulse/robot/commands/auton/deprecated/ShallowSwipeDot.java similarity index 96% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/ShallowSwipeDot.java rename to src/main/java/com/stuypulse/robot/commands/auton/deprecated/ShallowSwipeDot.java index f945cb7a..01378e9a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/ShallowSwipeDot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/deprecated/ShallowSwipeDot.java @@ -3,7 +3,7 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; +package com.stuypulse.robot.commands.auton.deprecated; import java.util.Set; @@ -27,7 +27,7 @@ import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class ShallowSwipeDot extends SequentialCommandGroup { - + //BC AUTON - don't know where to put public ShallowSwipeDot(PathPlannerPath... paths) { addCommands( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/Bump.java similarity index 91% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/Bump.java index e62711b5..93759309 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/Bump.java @@ -10,10 +10,8 @@ import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -23,9 +21,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class LeftBump extends SequentialCommandGroup { +public class Bump extends SequentialCommandGroup { - public LeftBump(PathPlannerPath... paths) { + public Bump(PathPlannerPath... paths) { addCommands( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java deleted file mode 100644 index 7a71dd94..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/CenterTwoCornerBC.java +++ /dev/null @@ -1,82 +0,0 @@ -/** ********************** PROJECT TRIBECBOT ************************ */ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ - /* Use of this source code is governed by an MIT-style license */ - /* that can be found in the repository LICENSE file. */ -/** ************************************************************ */ -package com.stuypulse.robot.commands.auton.regular; - -import java.util.Set; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.leds.LEDApplyState; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; -import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.subsystems.leds.LEDController.LedState; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class CenterTwoCornerBC extends SequentialCommandGroup { - - public CenterTwoCornerBC(PathPlannerPath... paths) { - - addCommands( - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).deadlineFor( - new WaitCommand(0.2).andThen(new IntakeDeploy()), - new LEDApplyState(LedState.AUTON_COLOR_ONE) - ), - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).deadlineFor( - new SuperstructureAutoInterpolation(), - new LEDApplyState(LedState.AUTON_COLOR_TWO) - ), - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).withTimeout(1.5).deadlineFor( - new LEDApplyState(LedState.AUTON_COLOR_ONE), - new HandoffRun(), - new SpindexerRun(), - new IntakeAutoDigest() - ),//.withTimeout(1.5), moved to the path up top, for the deadline for - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), - // NZ Trip 2 - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).deadlineFor( - new LEDApplyState(LedState.AUTON_COLOR_TWO), - new HandoffStop(), - new SpindexerStop() - ), - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new WaitCommand(5).deadlineFor( //deadline for accounts for LED Apply States - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new LEDApplyState(LedState.AUTON_COLOR_ONE), - new HandoffRun(), - new SpindexerRun(), - new IntakeAutoDigest() - ), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure SOTM is over - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).deadlineFor(new LEDApplyState(LedState.AUTON_COLOR_TWO)), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]).deadlineFor(new LEDApplyState(LedState.AUTON_COLOR_ONE)), - - new SwerveXMode() - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java deleted file mode 100644 index 24e84ad9..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerShallow.java +++ /dev/null @@ -1,94 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.auton.regular; - -import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.intake.IntakeDigest; -import com.stuypulse.robot.commands.intake.IntakeOuttake; -import com.stuypulse.robot.commands.intake.IntakeSetState; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; -import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.swerve.SwerveResetHeading; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.subsystems.intake.Intake.PivotState; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -import java.util.Set; - -import com.pathplanner.lib.path.PathPlannerPath; - -public class RightTwoCornerShallow extends SequentialCommandGroup { - - public RightTwoCornerShallow(PathPlannerPath... paths) { - - addCommands( - - new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - - Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), - - // NZ Trip 1 - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.2).andThen(new IntakeDeploy()) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)), - new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) - ), - new SuperstructureAutoInterpolation().alongWith(new IntakeSetState(PivotState.DEPLOY)), - new IntakeOuttake(), - new WaitCommand(0.2), - new IntakeDeploy(), - - // NZ Trip 2 - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffStop(), - new SpindexerStop() - ), - - new SuperstructureSOTM(), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffRun(), - new SpindexerRun(), - new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().withTimeout(15.0)), - new WaitCommand(15.0) - ) - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCorner.java similarity index 96% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCorner.java index c7f5043c..b171a196 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCorner.java @@ -31,9 +31,9 @@ import com.pathplanner.lib.path.PathPlannerPath; -public class LeftTwoCorner extends SequentialCommandGroup { - - public LeftTwoCorner(PathPlannerPath... paths) { +public class TwoCorner extends SequentialCommandGroup { + //Champs sequence + public TwoCorner(PathPlannerPath... paths) { addCommands( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerShallow.java similarity index 92% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerShallow.java index ed344397..3e359c7a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerShallow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerShallow.java @@ -5,38 +5,34 @@ /***************************************************************/ package com.stuypulse.robot.commands.auton.regular; +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeAutoDigest; import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.intake.IntakeDigest; import com.stuypulse.robot.commands.intake.IntakeOuttake; import com.stuypulse.robot.commands.intake.IntakeSetState; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveResetPose; import com.stuypulse.robot.subsystems.intake.Intake.PivotState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import java.util.Set; - -import com.pathplanner.lib.path.PathPlannerPath; - -public class LeftTwoCornerShallow extends SequentialCommandGroup { - - public LeftTwoCornerShallow(PathPlannerPath... paths) { +public class TwoCornerShallow extends SequentialCommandGroup { + //champs sequence but uses different timings with shallow + public TwoCornerShallow(PathPlannerPath... paths) { addCommands( From aaded0939c91357ec55301c36c78da8c81b7deda Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Sat, 20 Jun 2026 09:55:44 -0400 Subject: [PATCH 90/97] feat: remove intake stow button (jayden presses it by accident) --- src/main/java/com/stuypulse/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index dbc7bc89..2a5c2b4b 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -204,8 +204,8 @@ private void configureButtonBindings() { .onFalse(new IntakeDeploy()); // Intake Stow - driver.getLeftTriggerButton() - .onTrue(new IntakeStow()); + // driver.getLeftTriggerButton() + // .onTrue(new IntakeStow()); // Intake Deploy driver.getRightTriggerButton() From b0f09b4cd041ac52b147e771174760cf22ca0771 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Sat, 20 Jun 2026 09:59:36 -0400 Subject: [PATCH 91/97] feat: Qual 4 auton changes - we clipped trench on the first way back --- .../Champs Left Shallow To Score Wide.path | 59 +++++++++++++++++++ .../Champs Right Shallow To Score Wide.path | 59 +++++++++++++++++++ .../com/stuypulse/robot/RobotContainer.java | 4 +- 3 files changed, 120 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Champs Left Shallow To Score Wide.path create mode 100644 src/main/deploy/pathplanner/paths/Champs Right Shallow To Score Wide.path diff --git a/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score Wide.path b/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score Wide.path new file mode 100644 index 00000000..c774d057 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score Wide.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 5.265 + }, + "prevControl": null, + "nextControl": { + "x": 6.713949641685828, + "y": 7.614231551964771 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 7.441 + }, + "prevControl": { + "x": 6.721732371225743, + "y": 7.603974167740527 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Right Shallow To Score Wide.path b/src/main/deploy/pathplanner/paths/Champs Right Shallow To Score Wide.path new file mode 100644 index 00000000..b854b584 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Right Shallow To Score Wide.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 2.7346647646219684 + }, + "prevControl": null, + "nextControl": { + "x": 6.713949641685828, + "y": 0.3854332126571971 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 6.721732371225743, + "y": 0.396025832259473 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index dbc7bc89..d11907a1 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -456,11 +456,11 @@ public void configureAutons() { //might be a duplicate of Right Far Near Shallow Far Near - if no changes to that were made AutonConfig Right_Champs = new AutonConfig("Right Champs", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Champs Right To Shallow", "Champs Right Shallow To Score", "Champs Right Bite Score To Score", "Champs Right Score To Corner"); + "Champs Right To Shallow Wide", "Champs Right Shallow To Score", "Champs Right Bite Score To Score", "Champs Right Score To Corner"); Right_Champs.register(autonChooser); AutonConfig Left_Champs = new AutonConfig("Left Champs", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Champs Left To Shallow", "Champs Left Shallow To Score", "Champs Left Bite Score To Score", "Champs Left Score To Corner"); + "Champs Left To Shallow Wide", "Champs Left Shallow To Score", "Champs Left Bite Score To Score", "Champs Left Score To Corner"); Left_Champs.register(autonChooser); AutonConfig Right_NY = new AutonConfig("Right NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, From 93077ba43de9c68935545d658667a90aa628174d Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Sat, 20 Jun 2026 10:03:04 -0400 Subject: [PATCH 92/97] fix the prev commit - applied the "Wide" to the correct path --- src/main/java/com/stuypulse/robot/RobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 17a53604..8d222c10 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -456,11 +456,11 @@ public void configureAutons() { //might be a duplicate of Right Far Near Shallow Far Near - if no changes to that were made AutonConfig Right_Champs = new AutonConfig("Right Champs", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Champs Right To Shallow Wide", "Champs Right Shallow To Score", "Champs Right Bite Score To Score", "Champs Right Score To Corner"); + "Champs Right To Shallow", "Champs Right Shallow To Score Wide", "Champs Right Bite Score To Score", "Champs Right Score To Corner"); Right_Champs.register(autonChooser); AutonConfig Left_Champs = new AutonConfig("Left Champs", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Champs Left To Shallow Wide", "Champs Left Shallow To Score", "Champs Left Bite Score To Score", "Champs Left Score To Corner"); + "Champs Left To Shallow", "Champs Left Shallow To Score Wide", "Champs Left Bite Score To Score", "Champs Left Score To Corner"); Left_Champs.register(autonChooser); AutonConfig Right_NY = new AutonConfig("Right NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, @@ -472,11 +472,11 @@ public void configureAutons() { Left_NY.register(autonChooser); AutonConfig Right_Champs_NY = new AutonConfig("Right Champs NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Champs Right To Shallow", "Champs Right Shallow To Score", "NY Right Score To Score", "Right Score To Corner"); + "Champs Right To Shallow", "Champs Right Shallow To Score Wide", "NY Right Score To Score", "Right Score To Corner"); Right_Champs_NY.register(autonChooser); AutonConfig Left_Champs_NY = new AutonConfig("Left Champs NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Champs Left To Shallow", "Champs Left Shallow To Score", "NY Left Score To Score", "Left Score To Corner"); + "Champs Left To Shallow", "Champs Left Shallow To Score Wide", "NY Left Score To Score", "Left Score To Corner"); Left_Champs_NY.register(autonChooser); //BC Score To Score NY is a shorened version of NY and w the slow down From a2b246bd8fba15a86f0dededc183965873fd6963 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Sat, 20 Jun 2026 10:41:56 -0400 Subject: [PATCH 93/97] feat: reset heading to zero when shooting up against hub --- .../stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java index 818e36c1..ef83f3f3 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java @@ -4,6 +4,7 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; public class SwerveResetPoseKBShot extends SwerveResetPose { @@ -11,6 +12,6 @@ public SwerveResetPoseKBShot() { super(new Pose2d( Field.HUB_CENTER.getX() - Field.HUB_RADIUS - Units.inchesToMeters(23.5), Field.WIDTH / 2 - Units.inchesToMeters(6.5), - CommandSwerveDrivetrain.getInstance().getPose().getRotation())); + Rotation2d.kZero)); } } From cc5fd9458d1ca821dbeb188a4aa10d39c30c9fb2 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Sat, 20 Jun 2026 12:09:57 -0400 Subject: [PATCH 94/97] log is dead latency --- src/main/java/com/stuypulse/robot/constants/Cameras.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 02798453..d2970495 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -141,6 +141,7 @@ public void incrementLoopCounter() { public boolean isAlive() { //latest - old heartbeat boolean isDeadLatency = (LLLatency == LimelightHelpers.getLatency_Pipeline(this.name) && LLLatency != -1); + DogLog.log(keyName + "isDeadLatency", isDeadLatency); //TODO: double check that latency cannot be negative boolean isDeadHeartbeat = LimelightHelpers.getHeartbeat(this.getName()) - LLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && LLHeartbeat != -1; if (isDeadHeartbeat || isDeadLatency) { From 6b3f3a1981721bdae82d1cf17dd7fab573756083 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Sat, 20 Jun 2026 13:46:16 -0400 Subject: [PATCH 95/97] feat: 316 compatible champs ny auton --- .../deploy/pathplanner/autos/Champs + NY.auto | 2 +- ...316 Champs Left Shallow To Score Wide.path | 59 ++++++++ .../paths/316 Champs Left To Shallow.path | 81 +++++++++++ ...16 Champs Right Shallow To Score Wide.path | 59 ++++++++ .../paths/316 Champs Right To Shallow.path | 81 +++++++++++ .../paths/316 NY Left Score To Score.path | 111 +++++++++++++++ ....path => 316 NY Right Score To Score.path} | 26 ++-- .../paths/BC Right Score To Score NY.path | 12 +- .../Champs Left Shallow To Score Wide.path | 8 +- .../Copy of BC Right Score To Score NY.path | 127 ++++++++++++++++++ .../com/stuypulse/robot/RobotContainer.java | 8 +- 11 files changed, 546 insertions(+), 28 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/316 Champs Left Shallow To Score Wide.path create mode 100644 src/main/deploy/pathplanner/paths/316 Champs Left To Shallow.path create mode 100644 src/main/deploy/pathplanner/paths/316 Champs Right Shallow To Score Wide.path create mode 100644 src/main/deploy/pathplanner/paths/316 Champs Right To Shallow.path create mode 100644 src/main/deploy/pathplanner/paths/316 NY Left Score To Score.path rename src/main/deploy/pathplanner/paths/{Right Score To Score NY.path => 316 NY Right Score To Score.path} (79%) create mode 100644 src/main/deploy/pathplanner/paths/Copy of BC Right Score To Score NY.path diff --git a/src/main/deploy/pathplanner/autos/Champs + NY.auto b/src/main/deploy/pathplanner/autos/Champs + NY.auto index fe2fdcc4..2fe69e23 100644 --- a/src/main/deploy/pathplanner/autos/Champs + NY.auto +++ b/src/main/deploy/pathplanner/autos/Champs + NY.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Right Score To Score NY" + "pathName": "316 NY Right Score To Score" } }, { diff --git a/src/main/deploy/pathplanner/paths/316 Champs Left Shallow To Score Wide.path b/src/main/deploy/pathplanner/paths/316 Champs Left Shallow To Score Wide.path new file mode 100644 index 00000000..c8bad09f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/316 Champs Left Shallow To Score Wide.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 5.1 + }, + "prevControl": null, + "nextControl": { + "x": 7.100149920299288, + "y": 7.758980933132961 + }, + "isLocked": false, + "linkedName": "316 Left" + }, + { + "anchor": { + "x": 3.612, + "y": 7.441 + }, + "prevControl": { + "x": 7.092826633788956, + "y": 7.806849621436789 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/316 Champs Left To Shallow.path b/src/main/deploy/pathplanner/paths/316 Champs Left To Shallow.path new file mode 100644 index 00000000..e9bd8053 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/316 Champs Left To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 6.639728958630526, + "y": 7.677232524964337 + }, + "isLocked": false, + "linkedName": "Left Trench Start" + }, + { + "anchor": { + "x": 7.569, + "y": 5.1 + }, + "prevControl": { + "x": 7.361981455064195, + "y": 6.8596576319543505 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "316 Left" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.9253731343283487, + "rotationDegrees": -55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6867989646246767, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/316 Champs Right Shallow To Score Wide.path b/src/main/deploy/pathplanner/paths/316 Champs Right Shallow To Score Wide.path new file mode 100644 index 00000000..ae57bc5f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/316 Champs Right Shallow To Score Wide.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 2.9 + }, + "prevControl": null, + "nextControl": { + "x": 7.100149920299288, + "y": 0.2410190668670391 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 7.092826633788956, + "y": 0.19315037856321204 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/316 Champs Right To Shallow.path b/src/main/deploy/pathplanner/paths/316 Champs Right To Shallow.path new file mode 100644 index 00000000..a5991b1d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/316 Champs Right To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 0.325 + }, + "prevControl": null, + "nextControl": { + "x": 6.639728943436424, + "y": 0.32297044805622777 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.569, + "y": 2.9 + }, + "prevControl": { + "x": 7.36197644313434, + "y": 1.1403429576916921 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "316 Right" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.9253731343283487, + "rotationDegrees": 55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6867989646246767, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/316 NY Left Score To Score.path b/src/main/deploy/pathplanner/paths/316 NY Left Score To Score.path new file mode 100644 index 00000000..9377a06e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/316 NY Left Score To Score.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.2449526994735765, + "y": 7.376296404185324 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.850470756062768, + "y": 5.549 + }, + "prevControl": { + "x": 5.850470756062768, + "y": 8.072000000000001 + }, + "nextControl": { + "x": 5.850470756062768, + "y": 4.7490000000000006 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.353, + "y": 5.349 + }, + "prevControl": { + "x": 7.273386284835411, + "y": 4.452528217757139 + }, + "nextControl": { + "x": 7.575122265309203, + "y": 7.850156272457584 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 7.441 + }, + "prevControl": { + "x": 7.673006285012011, + "y": 7.479062563248229 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.65, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.289978678038381, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.6, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score NY.path b/src/main/deploy/pathplanner/paths/316 NY Right Score To Score.path similarity index 79% rename from src/main/deploy/pathplanner/paths/Right Score To Score NY.path rename to src/main/deploy/pathplanner/paths/316 NY Right Score To Score.path index 8ebe272a..1338a619 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score NY.path +++ b/src/main/deploy/pathplanner/paths/316 NY Right Score To Score.path @@ -17,31 +17,31 @@ { "anchor": { "x": 5.850470756062768, - "y": 2.851112696148359 + "y": 2.451 }, "prevControl": { "x": 5.850470756062768, - "y": 0.3280741797432247 + "y": -0.07200000000000006 }, "nextControl": { "x": 5.850470756062768, - "y": 4.150659142168011 + "y": 3.2510000000000003 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.302881844380403, - "y": 2.851112696148359 + "x": 7.353, + "y": 2.651 }, "prevControl": { - "x": 7.215816731986725, - "y": 3.83143356936277 + "x": 7.273381801565748, + "y": 3.547471384082104 }, "nextControl": { - "x": 7.525057636887608, - "y": 0.34949567723342856 + "x": 7.575134773631564, + "y": 0.14984483841092855 }, "isLocked": false, "linkedName": null @@ -52,7 +52,7 @@ "y": 0.559 }, "prevControl": { - "x": 7.673006390654898, + "x": 7.673006390654895, "y": 0.520948709769672 }, "nextControl": null, @@ -62,11 +62,11 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.17051509769094172, + "waypointRelativePos": 0.2, "rotationDegrees": 0.0 }, { - "waypointRelativePos": 0.644760213143872, + "waypointRelativePos": 0.65, "rotationDegrees": 90.0 }, { @@ -82,7 +82,7 @@ "rotationDegrees": -90.0 }, { - "waypointRelativePos": 2.6703967446591785, + "waypointRelativePos": 2.6, "rotationDegrees": 0.0 } ], diff --git a/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path b/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path index c9fbf182..758c9fd0 100644 --- a/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path +++ b/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path @@ -105,8 +105,8 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.4, - "maxWaypointRelativePos": 0.7, + "minWaypointRelativePos": 1.67, + "maxWaypointRelativePos": 1.92, "constraints": { "maxVelocity": 1.5, "maxAcceleration": 7.0, @@ -118,8 +118,8 @@ }, { "name": "Constraints Zone", - "minWaypointRelativePos": 1.0, - "maxWaypointRelativePos": 1.25, + "minWaypointRelativePos": 0.4, + "maxWaypointRelativePos": 0.7, "constraints": { "maxVelocity": 1.5, "maxAcceleration": 7.0, @@ -131,8 +131,8 @@ }, { "name": "Constraints Zone", - "minWaypointRelativePos": 1.67, - "maxWaypointRelativePos": 1.92, + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.25, "constraints": { "maxVelocity": 1.5, "maxAcceleration": 7.0, diff --git a/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score Wide.path b/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score Wide.path index c774d057..b34d5a08 100644 --- a/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score Wide.path +++ b/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score Wide.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.713949641685828, - "y": 7.614231551964771 + "x": 7.117514738065982, + "y": 7.8255001578317405 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 7.441 }, "prevControl": { - "x": 6.721732371225743, - "y": 7.603974167740527 + "x": 7.092826633788956, + "y": 7.806849621436789 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Copy of BC Right Score To Score NY.path b/src/main/deploy/pathplanner/paths/Copy of BC Right Score To Score NY.path new file mode 100644 index 00000000..0f6099e8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of BC Right Score To Score NY.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.46089863956213, + "y": 0.4753134455838692 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.163, + "y": 2.355 + }, + "prevControl": { + "x": 6.488590333125495, + "y": 0.5084854631021092 + }, + "nextControl": { + "x": 6.001159898414421, + "y": 3.272840825807378 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.637, + "y": 2.328 + }, + "prevControl": { + "x": 7.637, + "y": 3.228 + }, + "nextControl": { + "x": 7.637, + "y": 2.078 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.067, + "y": 0.559 + }, + "prevControl": { + "x": 8.016675326208684, + "y": 0.583834950985082 + }, + "nextControl": { + "x": 5.087385536493331, + "y": 0.5072311198219507 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 5.268148066661913, + "y": 0.6003141499164485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.34587995930824006, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.8, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.05, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 8d222c10..4d33cb8c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -204,8 +204,8 @@ private void configureButtonBindings() { .onFalse(new IntakeDeploy()); // Intake Stow - // driver.getLeftTriggerButton() - // .onTrue(new IntakeStow()); + driver.getLeftTriggerButton() + .onTrue(new IntakeStow()); // Intake Deploy driver.getRightTriggerButton() @@ -472,11 +472,11 @@ public void configureAutons() { Left_NY.register(autonChooser); AutonConfig Right_Champs_NY = new AutonConfig("Right Champs NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Champs Right To Shallow", "Champs Right Shallow To Score Wide", "NY Right Score To Score", "Right Score To Corner"); + "316 Champs Right To Shallow", "316 Champs Right Shallow To Score Wide", "316 NY Right Score To Score", "Right Score To Corner"); Right_Champs_NY.register(autonChooser); AutonConfig Left_Champs_NY = new AutonConfig("Left Champs NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Champs Left To Shallow", "Champs Left Shallow To Score Wide", "NY Left Score To Score", "Left Score To Corner"); + "316 Champs Left To Shallow", "316 Champs Left Shallow To Score Wide", "316 NY Left Score To Score", "Left Score To Corner"); Left_Champs_NY.register(autonChooser); //BC Score To Score NY is a shorened version of NY and w the slow down From b5596f92de7c694cbf48015dd2620f8d7dd9eb79 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Sat, 20 Jun 2026 14:15:27 -0400 Subject: [PATCH 96/97] feat: loosen first turn - champs + nyc/316 path --- .../paths/316 NY Left Score To Score.path | 16 +-- .../paths/316 NY Right Score To Score.path | 14 +-- .../paths/OLD 316 NY Left Score To Score.path | 111 ++++++++++++++++++ 3 files changed, 126 insertions(+), 15 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/OLD 316 NY Left Score To Score.path diff --git a/src/main/deploy/pathplanner/paths/316 NY Left Score To Score.path b/src/main/deploy/pathplanner/paths/316 NY Left Score To Score.path index 9377a06e..48860d93 100644 --- a/src/main/deploy/pathplanner/paths/316 NY Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/316 NY Left Score To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.2449526994735765, - "y": 7.376296404185324 + "x": 6.445245848532716, + "y": 7.510077505314976 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 5.549 }, "prevControl": { - "x": 5.850470756062768, - "y": 8.072000000000001 + "x": 6.024782241558084, + "y": 7.541389396183491 }, "nextControl": { - "x": 5.850470756062768, - "y": 4.7490000000000006 + "x": 5.7807461618646405, + "y": 4.752044241526603 }, "isLocked": false, "linkedName": null @@ -41,7 +41,7 @@ }, "nextControl": { "x": 7.575122265309203, - "y": 7.850156272457584 + "y": 7.850156272457583 }, "isLocked": false, "linkedName": null @@ -52,7 +52,7 @@ "y": 7.441 }, "prevControl": { - "x": 7.673006285012011, + "x": 7.67300628501201, "y": 7.479062563248229 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/316 NY Right Score To Score.path b/src/main/deploy/pathplanner/paths/316 NY Right Score To Score.path index 1338a619..0405eab1 100644 --- a/src/main/deploy/pathplanner/paths/316 NY Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/316 NY Right Score To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.244952924393722, - "y": 0.6236932952924387 + "x": 6.445245848532716, + "y": 0.48992249468502347 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 2.451 }, "prevControl": { - "x": 5.850470756062768, - "y": -0.07200000000000006 + "x": 6.024782241558084, + "y": 0.4586106038165092 }, "nextControl": { - "x": 5.850470756062768, - "y": 3.2510000000000003 + "x": 5.7807461618646405, + "y": 3.247955758473397 }, "isLocked": false, "linkedName": null @@ -52,7 +52,7 @@ "y": 0.559 }, "prevControl": { - "x": 7.673006390654895, + "x": 7.673006390654894, "y": 0.520948709769672 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/OLD 316 NY Left Score To Score.path b/src/main/deploy/pathplanner/paths/OLD 316 NY Left Score To Score.path new file mode 100644 index 00000000..9377a06e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/OLD 316 NY Left Score To Score.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.2449526994735765, + "y": 7.376296404185324 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.850470756062768, + "y": 5.549 + }, + "prevControl": { + "x": 5.850470756062768, + "y": 8.072000000000001 + }, + "nextControl": { + "x": 5.850470756062768, + "y": 4.7490000000000006 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.353, + "y": 5.349 + }, + "prevControl": { + "x": 7.273386284835411, + "y": 4.452528217757139 + }, + "nextControl": { + "x": 7.575122265309203, + "y": 7.850156272457584 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 7.441 + }, + "prevControl": { + "x": 7.673006285012011, + "y": 7.479062563248229 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.65, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.289978678038381, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.6, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file From d96425b793973edfabc07903e222d4397d0c6b3f Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Sat, 20 Jun 2026 15:24:31 -0400 Subject: [PATCH 97/97] FEAT: Limelight isdead debounce --- .../stuypulse/robot/constants/Cameras.java | 57 +++++++------------ 1 file changed, 19 insertions(+), 38 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index d2970495..a8177566 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -12,6 +12,8 @@ import com.stuypulse.robot.util.vision.LimelightHelpers.LimelightResults; import com.stuypulse.robot.util.vision.LimelightHelpers.RawFiducial; import com.stuypulse.stuylib.network.SmartBoolean; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose3d; @@ -54,6 +56,7 @@ public static class Camera { private int rejectedCounterAngularVelocity; private int rejectedCounterInvalidPosition; private int rejectedCounterTargetArea; + private final BStream isdead; private LimelightResults result; @@ -65,6 +68,7 @@ public Camera(String name, Pose3d location, SmartBoolean isEnabled) { this.isEnabled = isEnabled; this.keyName = "Vision/" + name + "/"; this.result = LimelightHelpers.getLatestResults(name); + this.isdead = BStream.create(() -> !isAlive()).filtered( new BDebounce.Rising(1), new BDebounce.Falling(0.2)); } public enum Pipeline { @@ -154,46 +158,23 @@ public boolean isAlive() { } public void updateLEDs() { - switch (this.getName()) { - case "limelight-right" -> { - if (this.loopCounter == 50) { - if (!isAlive()) { - LEDController.isRightLLDead = true; - } - else { - LEDController.isRightLLDead = false; - } - - this.loopCounter = 0; + if (this.loopCounter == 50) { + boolean iscurrentlyDead = isdead.get(); + switch (this.getName()) { + case "limelight-right" -> { + LEDController.isRightLLDead = iscurrentlyDead; + this.loopCounter = 0; } - } - - case "limelight-left" -> { - if (this.loopCounter == 50) { - if (!isAlive()) { - LEDController.isLeftLLDead = true; + case "limelight-left" -> { + LEDController.isLeftLLDead = iscurrentlyDead; + this.loopCounter = 0; + } + case "limelight-back" -> { + LEDController.isBackLLDead = iscurrentlyDead; } - else { - LEDController.isLeftLLDead = false; - } - - this.loopCounter = 0; - } - } - - - case "limelight-back" -> { - if (this.loopCounter == 50) { - if (!isAlive()) { - LEDController.isBackLLDead = true; - } - else { - LEDController.isBackLLDead = false; - } - - this.loopCounter = 0; - } - } + + } + this.loopCounter = 0; } }