From 83ff87da98019a93e822410824f404901dd9053d Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 15 Apr 2026 16:38:18 -0400 Subject: [PATCH 01/33] ADD: added bump auto, left and right sides --- .../deploy/pathplanner/autos/Left Middy.auto | 4 +- .../deploy/pathplanner/autos/Right Middy.auto | 4 +- .../pathplanner/paths/Depot To Score.path | 29 +--- .../paths/Left Bump Score To Depot.path | 16 +-- .../pathplanner/paths/Left Bump To NZ.path | 84 ----------- ...e.path => Left Bump To Score (Start).path} | 28 ++-- .../pathplanner/paths/Left Bump To Score.path | 133 ++++++++++++++++++ .../paths/Right Bump Score To Depot.path | 36 ++--- .../pathplanner/paths/Right Bump To NZ.path | 84 ----------- ....path => Right Bump To Score (Start).path} | 28 ++-- .../paths/Right Bump To Score.path | 133 ++++++++++++++++++ src/main/deploy/pathplanner/settings.json | 1 + .../com/stuypulse/robot/RobotContainer.java | 4 +- .../commands/auton/regular/LeftMiddy.java | 32 +++-- .../commands/auton/regular/RightMiddy.java | 32 +++-- 15 files changed, 364 insertions(+), 284 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/Left Bump To NZ.path rename src/main/deploy/pathplanner/paths/{Left Middy To Bump Score.path => Left Bump To Score (Start).path} (61%) create mode 100644 src/main/deploy/pathplanner/paths/Left Bump To Score.path delete mode 100644 src/main/deploy/pathplanner/paths/Right Bump To NZ.path rename src/main/deploy/pathplanner/paths/{Right Middy To Bump Score.path => Right Bump To Score (Start).path} (61%) create mode 100644 src/main/deploy/pathplanner/paths/Right Bump To Score.path diff --git a/src/main/deploy/pathplanner/autos/Left Middy.auto b/src/main/deploy/pathplanner/autos/Left Middy.auto index 427c8f10..067218bb 100644 --- a/src/main/deploy/pathplanner/autos/Left Middy.auto +++ b/src/main/deploy/pathplanner/autos/Left Middy.auto @@ -7,13 +7,13 @@ { "type": "path", "data": { - "pathName": "Left Bump To NZ" + "pathName": "Left Bump To Score (Start)" } }, { "type": "path", "data": { - "pathName": "Left Middy To Bump Score" + "pathName": "Left Bump To Score" } }, { diff --git a/src/main/deploy/pathplanner/autos/Right Middy.auto b/src/main/deploy/pathplanner/autos/Right Middy.auto index b1b5803c..baaf959b 100644 --- a/src/main/deploy/pathplanner/autos/Right Middy.auto +++ b/src/main/deploy/pathplanner/autos/Right Middy.auto @@ -7,13 +7,13 @@ { "type": "path", "data": { - "pathName": "Right Bump To NZ" + "pathName": "Right Bump To Score (Start)" } }, { "type": "path", "data": { - "pathName": "Right Middy To Bump Score" + "pathName": "Right Bump To Score" } }, { diff --git a/src/main/deploy/pathplanner/paths/Depot To Score.path b/src/main/deploy/pathplanner/paths/Depot To Score.path index d3c9f452..ac7273ec 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Score.path +++ b/src/main/deploy/pathplanner/paths/Depot To Score.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.7074970344009497, + "x": 1.4642653352353778, "y": 5.959713740458016 }, "prevControl": { - "x": 1.856554697887364, + "x": 0.6133229987217921, "y": 5.954351642472394 }, "nextControl": null, @@ -28,27 +28,8 @@ "linkedName": "Depot Out" } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3262260127931784, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.14848143982002254, - "maxWaypointRelativePos": 0.9448818897637792, - "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 0.75, - "maxAngularVelocity": 50.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.5, - "unlimited": false - } - } - ], + "rotationTargets": [], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -61,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "To Score", diff --git a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path index 01589c7e..77d7a066 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 0.7207369258195486, - "y": 6.064140721945863 + "x": 1.0114122681883022, + "y": 6.176348074179743 }, "isLocked": false, "linkedName": "Left Bump Score" @@ -40,11 +40,11 @@ "minWaypointRelativePos": 0, "maxWaypointRelativePos": 0.5016872890888694, "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 0.75, + "maxVelocity": 0.75, + "maxAcceleration": 1.0, "maxAngularVelocity": 50.0, "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false } } @@ -52,7 +52,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.19, + "maxVelocity": 1.5, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, @@ -67,7 +67,7 @@ "folder": "To Depot", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump To NZ.path b/src/main/deploy/pathplanner/paths/Left Bump To NZ.path deleted file mode 100644 index ea3c532e..00000000 --- a/src/main/deploy/pathplanner/paths/Left Bump To NZ.path +++ /dev/null @@ -1,84 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.500057251908398, - "y": 5.122881679389313 - }, - "prevControl": null, - "nextControl": { - "x": 6.378759541984733, - "y": 5.315353053435115 - }, - "isLocked": false, - "linkedName": "Left Bump Start" - }, - { - "anchor": { - "x": 6.110973282442748, - "y": 4.562204198473283 - }, - "prevControl": { - "x": 5.914394099487028, - "y": 5.110601060796104 - }, - "nextControl": { - "x": 6.354700620229008, - "y": 3.8822781488549625 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.144475190839694, - "y": 4.026631679389314 - }, - "prevControl": { - "x": 7.03148854961832, - "y": 3.951316793893131 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Middy" - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.6600517687661842, - "maxWaypointRelativePos": 2.0, - "constraints": { - "maxVelocity": 1.75, - "maxAcceleration": 7.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 NZ", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path b/src/main/deploy/pathplanner/paths/Left Bump To Score (Start).path similarity index 61% rename from src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path rename to src/main/deploy/pathplanner/paths/Left Bump To Score (Start).path index 7a194703..a2ac11b3 100644 --- a/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path +++ b/src/main/deploy/pathplanner/paths/Left Bump To Score (Start).path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.144475190839694, - "y": 4.026631679389314 + "x": 3.6250213980028523, + "y": 5.122881679389313 }, "prevControl": null, "nextControl": { - "x": 4.997986641221376, - "y": 5.876030534351146 + "x": 3.2144720637035995, + "y": 5.150459287975925 }, "isLocked": false, - "linkedName": "Middy" + "linkedName": "Left Bump Start" }, { "anchor": { - "x": 3.4749522900763368, - "y": 5.507824427480916 + "x": 3.0169044222539223, + "y": 5.122881679389313 }, "prevControl": { - "x": 5.516822519083972, - "y": 5.616612595419848 + "x": 3.3369416060580748, + "y": 5.124474131392073 }, "nextControl": null, "isLocked": false, - "linkedName": "Left Bump Score" + "linkedName": "Left Bump Pre" } ], "rotationTargets": [], @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.19, - "maxAcceleration": 10.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, @@ -45,10 +45,10 @@ "rotation": 0.0 }, "reversed": false, - "folder": "To Score", + "folder": "Bump Stuff", "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 Bump To Score.path b/src/main/deploy/pathplanner/paths/Left Bump To Score.path new file mode 100644 index 00000000..5ff13c1a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Bump To Score.path @@ -0,0 +1,133 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.0169044222539223, + "y": 5.122881679389313 + }, + "prevControl": null, + "nextControl": { + "x": 3.9614265335235372, + "y": 5.16713266761769 + }, + "isLocked": false, + "linkedName": "Left Bump Pre" + }, + { + "anchor": { + "x": 5.565820256776034, + "y": 5.122881679389313 + }, + "prevControl": { + "x": 4.8721189135072756, + "y": 5.185215403584219 + }, + "nextControl": { + "x": 7.377232524964336, + "y": 4.960114122681883 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.075920114122681, + "y": 3.9897146932952925 + }, + "prevControl": { + "x": 6.549158345221112, + "y": 3.562738944365193 + }, + "nextControl": { + "x": 8.677779314083576, + "y": 4.15803124921656 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.623067047075606, + "y": 5.9952068473609135 + }, + "prevControl": { + "x": 8.540539193302806, + "y": 6.260957400061206 + }, + "nextControl": { + "x": 5.746961483594864, + "y": 5.4517831669044226 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4749522900763368, + "y": 5.507824427480916 + }, + "prevControl": { + "x": 5.7340228245363765, + "y": 5.555292439372327 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Bump Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0319829424306861, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4, + "rotationDegrees": -45.0 + }, + { + "waypointRelativePos": 2.35, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.2964213369345192, + "maxWaypointRelativePos": 3.2194463200540184, + "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": "Bump Stuff", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path index 70b3af07..fd6d6263 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path @@ -14,30 +14,14 @@ "isLocked": false, "linkedName": "Right Bump Score" }, - { - "anchor": { - "x": 1.8933396946564884, - "y": 4.938778625954199 - }, - "prevControl": { - "x": 1.9041990007255656, - "y": 4.689014586365434 - }, - "nextControl": { - "x": 1.8369555219739746, - "y": 6.235614597651982 - }, - "isLocked": false, - "linkedName": null - }, { "anchor": { "x": 0.5795133587786261, "y": 5.959713740458016 }, "prevControl": { - "x": 1.684131679389313, - "y": 5.859293893129771 + "x": 2.072382310984308, + "y": 6.266918687589158 }, "nextControl": null, "isLocked": false, @@ -46,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.6, + "waypointRelativePos": 0.8, "rotationDegrees": 180.0 } ], @@ -54,13 +38,13 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.65, + "maxWaypointRelativePos": 0.8129642133693463, "constraints": { - "maxVelocity": 0.75, - "maxAcceleration": 1.0, + "maxVelocity": 1.25, + "maxAcceleration": 1.75, "maxAngularVelocity": 50.0, "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false } } @@ -68,7 +52,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.19, + "maxVelocity": 1.5, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, @@ -83,7 +67,7 @@ "folder": "To Depot", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump To NZ.path b/src/main/deploy/pathplanner/paths/Right Bump To NZ.path deleted file mode 100644 index e249ebb2..00000000 --- a/src/main/deploy/pathplanner/paths/Right Bump To NZ.path +++ /dev/null @@ -1,84 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.4833206106870227, - "y": 2.9220133587786257 - }, - "prevControl": null, - "nextControl": { - "x": 5.4749809160305345, - "y": 2.813225190839695 - }, - "isLocked": false, - "linkedName": "Right Bump Start" - }, - { - "anchor": { - "x": 6.336917938931298, - "y": 3.4492175572519077 - }, - "prevControl": { - "x": 6.36674218315174, - "y": 2.6439629632999404 - }, - "nextControl": { - "x": 6.3034446564885505, - "y": 4.352996183206107 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.144475190839694, - "y": 4.026631679389314 - }, - "prevControl": { - "x": 7.144475190839694, - "y": 4.026631679389314 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Middy" - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.6496980155306307, - "maxWaypointRelativePos": 2.0, - "constraints": { - "maxVelocity": 1.75, - "maxAcceleration": 7.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.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 NZ", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path b/src/main/deploy/pathplanner/paths/Right Bump To Score (Start).path similarity index 61% rename from src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path rename to src/main/deploy/pathplanner/paths/Right Bump To Score (Start).path index b9e05c6d..88b5c18a 100644 --- a/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path +++ b/src/main/deploy/pathplanner/paths/Right Bump To Score (Start).path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.144475190839694, - "y": 4.026631679389314 + "x": 3.689714693295292, + "y": 2.9220133587786257 }, "prevControl": null, "nextControl": { - "x": 5.533559160305344, - "y": 2.2023377862595424 + "x": 3.297321789223996, + "y": 2.918542121498757 }, "isLocked": false, - "linkedName": "Middy" + "linkedName": "Right Bump Start" }, { "anchor": { - "x": 3.4749522900763368, - "y": 2.486860687022901 + "x": 3.1592296718972896, + "y": 2.9220133587786257 }, "prevControl": { - "x": 5.223931297709925, - "y": 2.5119656488549618 + "x": 3.517516983361412, + "y": 2.8905025227689913 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Bump Score" + "linkedName": "Right Bump Pre" } ], "rotationTargets": [], @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.19, - "maxAcceleration": 10.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, @@ -45,10 +45,10 @@ "rotation": 0.0 }, "reversed": false, - "folder": "To Score", + "folder": "Bump Stuff", "idealStartingState": { "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump To Score.path b/src/main/deploy/pathplanner/paths/Right Bump To Score.path new file mode 100644 index 00000000..ad74de96 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Bump To Score.path @@ -0,0 +1,133 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.1592296718972896, + "y": 2.9220133587786257 + }, + "prevControl": null, + "nextControl": { + "x": 5.642827748799426, + "y": 2.838174037089873 + }, + "isLocked": false, + "linkedName": "Right Bump Pre" + }, + { + "anchor": { + "x": 6.80793152639087, + "y": 3.692125534950071 + }, + "prevControl": { + "x": 6.225691868758916, + "y": 2.8899286733238227 + }, + "nextControl": { + "x": 7.269102545829907, + "y": 4.327516717288298 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.386447931526389, + "y": 3.8603281027104144 + }, + "prevControl": { + "x": 8.27, + "y": 4.106162624821684 + }, + "nextControl": { + "x": 8.741324417931276, + "y": 3.1111444091889777 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.390171184022824, + "y": 2.035977175463623 + }, + "prevControl": { + "x": 8.68906165529474, + "y": 1.5971628270609481 + }, + "nextControl": { + "x": 5.475249643366619, + "y": 2.682910128388018 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4749522900763368, + "y": 2.486860687022901 + }, + "prevControl": { + "x": 4.634236804564908, + "y": 2.566462196861627 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Bump Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4690831556503233, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.9552238805970014, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 2.1918976545841944, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.8230277185500956, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8210668467251963, + "maxWaypointRelativePos": 2.9817690749493897, + "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": "Bump Stuff", + "idealStartingState": { + "velocity": 0, + "rotation": 0.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 a693d1fb..65d1dd1c 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,6 +3,7 @@ "robotLength": 0.762, "holonomicMode": true, "pathFolders": [ + "Bump Stuff", "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 ac0e8b58..37711e3e 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -361,10 +361,10 @@ public void configureAutons() { // DEPOT AutonConfig LEFT_MIDDY = new AutonConfig("Left Middy", LeftMiddy::new, - "Left Bump To NZ", "Left Middy To Bump Score", "Left Bump Score To Depot", "Depot To Score"); + "Left Bump To Score (Start)", "Left Bump To Score", "Left Bump Score To Depot"); LEFT_MIDDY.register(autonChooser); AutonConfig RIGHT_MIDDY = new AutonConfig("Right Middy", RightMiddy::new, - "Right Bump To NZ", "Right Middy To Bump Score", "Right Bump Score To Depot", "Depot To Score"); + "Right Bump To Score (Start)", "Right Bump To Score", "Right Bump Score To Depot"); RIGHT_MIDDY.register(autonChooser); // TWO CYCLES (TRENCH) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java index 44b4abea..78f73c6f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java @@ -10,6 +10,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.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -26,35 +27,42 @@ public LeftMiddy(PathPlannerPath... paths) { new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - // NZ Trip 1 - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.75).andThen(new IntakeDeploy()) + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new WaitCommand(2.0) ), - // Trip 1 To Score + // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() + new ParallelCommandGroup( + new IntakeDeploy(), + new HandoffStop(), + new SpindexerStop() + ) ), + new WaitCommand(0.5), + // SOTM To Depot - new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new SpindexerRun()), new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new WaitCommand(3.0).andThen( - new IntakeAutoDigest().repeatedly().withTimeout(2.0).andThen(new IntakeDeploy()) - ), new WaitCommand(5.0).andThen( new HandoffStop().alongWith(new SpindexerStop()) ) ), + + new WaitCommand(0.5), // Off Depot new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new WaitCommand(1.0).andThen(new HandoffRun().alongWith(new SpindexerRun())), - new WaitCommand(3.0).andThen(new IntakeAutoDigest().repeatedly()) + new HandoffRun().alongWith(new SpindexerRun()), + new WaitCommand(5.0).andThen(new IntakeAutoDigest().repeatedly()) ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java index 2cbffe4f..5ec42ed1 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java @@ -10,6 +10,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.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -26,35 +27,42 @@ public RightMiddy(PathPlannerPath... paths) { new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - // NZ Trip 1 - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.75).andThen(new IntakeDeploy()) + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new WaitCommand(2.0) ), - // Trip 1 To Score + // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() + new ParallelCommandGroup( + new IntakeDeploy(), + new HandoffStop(), + new SpindexerStop() + ) ), + new WaitCommand(0.5), + // SOTM To Depot - new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new SpindexerRun()), new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new WaitCommand(3.0).andThen( - new IntakeAutoDigest().repeatedly().withTimeout(3.0).andThen(new IntakeDeploy()) - ), new WaitCommand(6.0).andThen( new HandoffStop().alongWith(new SpindexerStop()) ) ), + + new WaitCommand(0.5), // Off Depot new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new WaitCommand(1.0).andThen(new HandoffRun().alongWith(new SpindexerRun())), - new WaitCommand(3.0).andThen(new IntakeAutoDigest().repeatedly()) + new HandoffRun().alongWith(new SpindexerRun()), + new WaitCommand(5.0).andThen(new IntakeAutoDigest().repeatedly()) ) ); From 5ebc72d662b95baabbdee20243b20dd989410b42 Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Wed, 15 Apr 2026 19:00:50 -0400 Subject: [PATCH 02/33] feat: memoization for booleans in drivetrain and interpolationcalc, clear all cached values in robotPeriodic() --- src/main/java/com/stuypulse/robot/Robot.java | 112 +- .../com/stuypulse/robot/RobotContainer.java | 1 + .../superstructure/Superstructure.java | 17 +- .../subsystems/superstructure/hood/Hood.java | 5 +- .../superstructure/shooter/Shooter.java | 4 +- .../superstructure/shooter/ShooterImpl.java | 1 + .../swerve/CommandSwerveDrivetrain.java | 1487 +++++++++-------- .../InterpolationCalculator.java | 38 + 8 files changed, 882 insertions(+), 783 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index d2b0eec1..48e360c0 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -1,13 +1,14 @@ -/************************ 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; import java.lang.management.GarbageCollectorMXBean; import java.lang.management.ManagementFactory; import java.util.List; +import java.util.function.BiConsumer; import com.pathplanner.lib.commands.FollowPathCommand; import com.pathplanner.lib.commands.PathfindingCommand; @@ -28,6 +29,7 @@ import com.stuypulse.robot.util.EnergyUtil; import com.stuypulse.robot.util.FMSUtil; import com.stuypulse.robot.util.PhoenixUtil; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.robot.util.superstructure.SOTMCalculator; import edu.wpi.first.wpilibj.DataLogManager; @@ -38,6 +40,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; + public class Robot extends TimedRobot { public enum RobotMode { @@ -56,7 +59,7 @@ public enum RobotMode { private GcStatsCollector gcStatsCollector; public static boolean fmsAttached; - private static int periodicCounter = 0; + private static int periodicCounter = 0; public static boolean isBlue() { return alliance == Alliance.Blue; @@ -74,10 +77,15 @@ public static int getPeriodicCounter() { return periodicCounter; } - /*************************/ - /*** ROBOT SCHEDULEING ***/ - /*************************/ - + /** + * ********************** + */ + /** + * * ROBOT SCHEDULEING ** + */ + /** + * ********************** + */ @Override public void robotInit() { robot = new RobotContainer(); @@ -93,8 +101,34 @@ public void robotInit() { CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand()); energyUtil = new EnergyUtil(); + + //TODO: UNCOMMENT WHEN TESTING ALL OF THESE CHANGES. + // try { + // Field watchdogField = IterativeRobotBase.class.getDeclaredField("m_watchdog"); + // watchdogField.setAccessible(true); + // Watchdog watchdog = (Watchdog) watchdogField.get(this); + // watchdog.setTimeout(Settings.DT); + // } catch (Exception e) { + // DriverStation.reportWarning("Failed to disable loop overrun warnings.", false); + // } + // CommandScheduler.getInstance().setPeriod(Settings.DT); + CommandScheduler.getInstance().schedule(new SwerveAutonInit()); RobotController.setBrownoutVoltage(6.3); + + BiConsumer logCommandFunction + = (Command command, Boolean active) -> { + String name = command.getName(); + SmartDashboard.putBoolean( + "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); + }; + + CommandScheduler.getInstance() + .onCommandInitialize((Command command) -> logCommandFunction.accept(command, true)); + CommandScheduler.getInstance() + .onCommandFinish((Command command) -> logCommandFunction.accept(command, false)); + CommandScheduler.getInstance() + .onCommandInterrupt((Command command) -> logCommandFunction.accept(command, false)); } @Override @@ -122,21 +156,29 @@ public void robotPeriodic() { if (!Robot.isReal()) { SmartDashboard.putData(CommandScheduler.getInstance()); } - SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime()); SmartDashboard.putData("Robot/Scheduled Commands", CommandScheduler.getInstance()); SmartDashboard.putNumber("Robot/Battery Voltage", batteryVoltage); SmartDashboard.putNumber("Robot/CPU Temperature (C)", RobotController.getCPUTemp()); - + robot.periodicAfterScheduler(); + + //clearing memoized values + InterpolationCalculator.clearMemoized(); + CommandSwerveDrivetrain.getInstance().clearMemoized(); energyUtil.periodic(); } - /*********************/ - /*** DISABLED MODE ***/ - /*********************/ - + /** + * ****************** + */ + /** + * * DISABLED MODE ** + */ + /** + * ****************** + */ @Override public void disabledInit() { mode = RobotMode.DISABLED; @@ -162,10 +204,15 @@ public void disabledPeriodic() { } } - /***********************/ - /*** AUTONOMOUS MODE ***/ - /***********************/ - + /** + * ******************** + */ + /** + * * AUTONOMOUS MODE ** + */ + /** + * ******************** + */ @Override public void autonomousInit() { mode = RobotMode.AUTON; @@ -189,10 +236,15 @@ public void autonomousExit() { CommandScheduler.getInstance().schedule(new SwerveTeleopInit()); } - /*******************/ - /*** TELEOP MODE ***/ - /*******************/ - + /** + * **************** + */ + /** + * * TELEOP MODE ** + */ + /** + * **************** + */ @Override public void teleopInit() { mode = RobotMode.TELEOP; @@ -225,10 +277,15 @@ public void teleopPeriodic() { public void teleopExit() { } - /*****************/ - /*** TEST MODE ***/ - /*****************/ - + /** + * ************** + */ + /** + * * TEST MODE ** + */ + /** + * ************** + */ @Override public void testInit() { mode = RobotMode.TEST; @@ -244,6 +301,7 @@ public void testExit() { } private static final class GcStatsCollector { + private List gcBeans = ManagementFactory.getGarbageCollectorMXBeans(); private final long[] lastTimes = new long[gcBeans.size()]; private final long[] lastCounts = new long[gcBeans.size()]; diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 37711e3e..86b6905c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -75,6 +75,7 @@ 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; 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 7cad5f63..38b06ab4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -157,7 +157,7 @@ public boolean isTurretWrapping() { public double getCurrentDraw() { return turret.getCurrentDraw() + shooter.getCurrentDraw() + hood.getCurrentDraw(); } - + public boolean shouldStop() { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); @@ -166,7 +166,7 @@ public boolean shouldStop() { boolean isTurretWrapping = isTurretWrapping(); boolean isBehindHubWhileFerrying = getState() == SuperstructureState.FOTM - && swerve.getIsBehindHub(); + && swerve.isBehindHub(); boolean isOutsideAllianceZone = CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && getState() != SuperstructureState.FOTM; @@ -181,13 +181,12 @@ public boolean shouldStop() { boolean turretLaggingSOTM = !isTurretAtTolerance() && getState() == SuperstructureState.SOTM; - SmartDashboard.putBoolean("Spindexer/Should Stop/turret lagging sotm", turretLaggingSOTM); - SmartDashboard.putBoolean("Spindexer/Should Stop/is Behind Hub While Ferrying?", isBehindHubWhileFerrying); - SmartDashboard.putBoolean("Spindexer/Should Stop/is Turret Wrapping?", isTurretWrapping); - SmartDashboard.putBoolean("Spindexer/Should Stop/is Outside Alliance zone?", isOutsideAllianceZone); - SmartDashboard.putBoolean("Spindexer/Should Stop/is Under Trenche?", isUnderTrench); - SmartDashboard.putBoolean("Spindexer/Should Stop/turret lagging sotm", turretLaggingSOTM); - SmartDashboard.putBoolean("Spindexer/Should Stop/inManualState", inManualState); + SmartDashboard.putBoolean("Spindexer/Should Stop/Is Behind Hub While Ferrying?", isBehindHubWhileFerrying); + SmartDashboard.putBoolean("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); + SmartDashboard.putBoolean("Spindexer/Should Stop/Is Outside Alliance Zone?", isOutsideAllianceZone); + SmartDashboard.putBoolean("Spindexer/Should Stop/Is Under Trench?", isUnderTrench); + SmartDashboard.putBoolean("Spindexer/Should Stop/Turret Lagging SOTM", turretLaggingSOTM); + SmartDashboard.putBoolean("Spindexer/Should Stop/In Manual State", inManualState); return isSpindexerStopState || isHandOffStopState || 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 4f15f808..6d004b24 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 @@ -10,6 +10,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.robot.util.superstructure.SOTMCalculator; @@ -74,12 +75,12 @@ public Rotation2d getTargetAngle() { return switch(state) { case STOW -> Settings.Superstructure.Hood.Angles.STOW; - case FERRY -> Settings.Superstructure.Hood.Angles.FERRY_ANGLE; + case FERRY -> InterpolationCalculator.getInterpolatedFerryAngle(); case MANUAL_OVERRIDE -> Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.MANUAL_OVERRIDE.get()); case KB -> Settings.Superstructure.Hood.Angles.KB; case LEFT_CORNER -> Settings.Superstructure.Hood.Angles.LEFT_CORNER; case RIGHT_CORNER -> Settings.Superstructure.Hood.Angles.RIGHT_CORNER; - case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetHoodAngle(); + case INTERPOLATION -> InterpolationCalculator.getInterpolatedShotAngle(); case HOMING_UPPER -> new Rotation2d(); //should just apply a voltage, not an angle! case HOMING_LOWER -> new Rotation2d(); case SOTM -> SOTMCalculator.calculateHoodAngleSOTM(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index fedb0915..51b7cc64 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -64,12 +64,12 @@ public double getTargetRPM() { return switch(state) { case STOP -> 0; case MANUAL_OVERRIDE -> getShootRPM(); - case FERRY -> InterpolationCalculator.interpolateFerryingInfo().targetRPM(); + case FERRY -> InterpolationCalculator.getInterpolatedFerryRPM(); case REVERSE -> Settings.Superstructure.Shooter.RPM.REVERSE; case KB -> Settings.Superstructure.Shooter.RPM.KB; case LEFT_CORNER -> Settings.Superstructure.Shooter.RPM.LEFT_CORNER; case RIGHT_CORNER -> Settings.Superstructure.Shooter.RPM.RIGHT_CORNER; - case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetRPM(); + case INTERPOLATION -> InterpolationCalculator.getInterpolatedShotRPM(); case SOTM -> SOTMCalculator.calculateShooterRPMSOTM(); case FOTM -> SOTMCalculator.calculateShooterRPMFOTM(); }; 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 e1f454f7..06cb401f 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 @@ -105,6 +105,7 @@ public ShooterImpl() { voltageOverride = Optional.empty(); } + //TODO: make all this RPM stuff one method. Should use a paramaeter to identify the RPM to get - like the motor @Override public double getRPM() { return getLeaderRPM(); 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 e0c31827..36e69d46 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.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.subsystems.swerve; import com.ctre.phoenix6.SignalLogger; @@ -54,6 +54,9 @@ import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Volts; + +import java.util.Optional; + import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -67,753 +70,751 @@ * Subsystem so it can easily be used in command-based projects. */ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { - private final static CommandSwerveDrivetrain instance; - - private FieldObject2d turret2d = Field.FIELD2D.getObject("Turret 2D"); - private Pose2d turretPose = new Pose2d(); - private StructPublisher leftBehindHubYPlublisher; - private StructPublisher rightBehindHubYPlublisher; - private StructPublisher vertexBehindHubPublisher; - private StatusSignal robotAccelerationX; - private StatusSignal robotAccelerationY; - private boolean isBehindHub = false; - - private StructPublisher robotPose = NetworkTableInstance.getDefault() - .getStructTopic("Robot Pose", Pose2d.struct).publish(); - - static { - instance = TunerConstants.createDrivetrain(); - // instance.registerTelemetry(instance::telemeterize); - } - - // public void telemeterize(SwerveDriveState state) { - // /* Write drive state to the log file */ - // SignalLogger.writeStruct("DriveState/Pose", Pose2d.struct, state.Pose); - // SignalLogger.writeStruct("DriveState/Speeds", ChassisSpeeds.struct, state.Speeds); - // SignalLogger.writeStructArray("DriveState/ModuleStates", SwerveModuleState.struct, state.ModuleStates); - // SignalLogger.writeStructArray("DriveState/ModuleTargets", SwerveModuleState.struct, state.ModuleTargets); - // SignalLogger.writeStructArray("DriveState/ModulePositions", SwerveModulePosition.struct, state.ModulePositions); - // SignalLogger.writeStruct("DriveState/RawHeading", Rotation2d.struct, state.RawHeading); - // SignalLogger.writeDouble("DriveState/Timestamp", state.Timestamp, "seconds"); - // SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - // SignalLogger.writeInteger("DriveState/FailedDaqs", state.FailedDaqs); - // } - - public static CommandSwerveDrivetrain getInstance() { - return instance; - } - - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - private Pose2d lastGoodPose = new Pose2d(1.5, 1.5, Rotation2d.kZero); - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_moduleTranslationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - - private final SwerveRequest.FieldCentric fieldCentricRequest = new SwerveRequest.FieldCentric() - .withDriveRequestType(DriveRequestType.OpenLoopVoltage) - .withDeadband(Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) - .withRotationalDeadband(Settings.Swerve.ROTATIONAL_DEADBAND_RAD_PER_S) - .withDesaturateWheelSpeeds(true); - - private final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric() - .withDriveRequestType(DriveRequestType.OpenLoopVoltage) - .withDeadband(Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) - .withRotationalDeadband(Settings.Swerve.ROTATIONAL_DEADBAND_RAD_PER_S) - .withDesaturateWheelSpeeds(true); - - public SwerveRequest.FieldCentric getFieldCentricSwerveRequest() { - return this.fieldCentricRequest; - } - - public SwerveRequest.RobotCentric getRobotCentricSwerveRequest() { - return this.robotCentricRequest; - } - - /* + + private final static CommandSwerveDrivetrain instance; + + private FieldObject2d turret2d = Field.FIELD2D.getObject("Turret 2D"); + private Pose2d turretPose = new Pose2d(); + private StructPublisher leftBehindHubYPlublisher; + private StructPublisher rightBehindHubYPlublisher; + private StructPublisher vertexBehindHubPublisher; + private StatusSignal robotAccelerationX; + private StatusSignal robotAccelerationY; + + //TODO: might wanna change some of these initialized values like isBehindTower indicates that we are in the alliance zone + private Optional isBehindHub = Optional.empty(); + private Optional isOutsideAllianceZone = Optional.empty(); + private Optional isInOpponentZone = Optional.empty(); + private Optional isUnderTrench = Optional.empty(); + private Optional isBehindTower = Optional.empty(); + + private StructPublisher robotPose = NetworkTableInstance.getDefault() + .getStructTopic("Robot Pose", Pose2d.struct).publish(); + + static { + instance = TunerConstants.createDrivetrain(); + // instance.registerTelemetry(instance::telemeterize); + } + + // public void telemeterize(SwerveDriveState state) { + // /* Write drive state to the log file */ + // SignalLogger.writeStruct("DriveState/Pose", Pose2d.struct, state.Pose); + // SignalLogger.writeStruct("DriveState/Speeds", ChassisSpeeds.struct, state.Speeds); + // SignalLogger.writeStructArray("DriveState/ModuleStates", SwerveModuleState.struct, state.ModuleStates); + // SignalLogger.writeStructArray("DriveState/ModuleTargets", SwerveModuleState.struct, state.ModuleTargets); + // SignalLogger.writeStructArray("DriveState/ModulePositions", SwerveModulePosition.struct, state.ModulePositions); + // SignalLogger.writeStruct("DriveState/RawHeading", Rotation2d.struct, state.RawHeading); + // SignalLogger.writeDouble("DriveState/Timestamp", state.Timestamp, "seconds"); + // SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); + // SignalLogger.writeInteger("DriveState/FailedDaqs", state.FailedDaqs); + // } + public static CommandSwerveDrivetrain getInstance() { + return instance; + } + + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; + + private Pose2d lastGoodPose = new Pose2d(1.5, 1.5, Rotation2d.kZero); + + /* Swerve requests to apply during SysId characterization */ + private final SwerveRequest.SysIdSwerveTranslation m_moduleTranslationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + + private final SwerveRequest.FieldCentric fieldCentricRequest = new SwerveRequest.FieldCentric() + .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withDeadband(Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) + .withRotationalDeadband(Settings.Swerve.ROTATIONAL_DEADBAND_RAD_PER_S) + .withDesaturateWheelSpeeds(true); + + private final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric() + .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withDeadband(Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) + .withRotationalDeadband(Settings.Swerve.ROTATIONAL_DEADBAND_RAD_PER_S) + .withDesaturateWheelSpeeds(true); + + public SwerveRequest.FieldCentric getFieldCentricSwerveRequest() { + return this.fieldCentricRequest; + } + + public SwerveRequest.RobotCentric getRobotCentricSwerveRequest() { + return this.robotCentricRequest; + } + + /* * SysId routine for characterizing module translation. This is used to find PID * gains for the drive motors. - */ - private final SysIdRoutine m_sysIdRoutineModuleTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdModuleTranslation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> setControl(m_moduleTranslationCharacterization.withVolts(output)), - null, - this)); - - /* + */ + private final SysIdRoutine m_sysIdRoutineModuleTranslation = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdModuleTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> setControl(m_moduleTranslationCharacterization.withVolts(output)), + null, + this)); + + /* * SysId routine for characterizing chassis translation. This is used to find * PID gains PID to pose. - */ - private final SysIdRoutine m_sysIdRoutineChassisTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in meters per second², but SysId only supports "volts per second" */ - Volts.of(0.5).per(Second), - /* This is in meters per second, but SysId only supports "volts" */ - Volts.of(Settings.Swerve.Constraints.MAX_VELOCITY_M_PER_S), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdChassisTranslation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually meters per second, but SysId only supports "volts" */ - setControl(getRobotCentricSwerveRequest().withVelocityX(output.in(Volts)).withVelocityY(0) - .withRotationalRate(0)); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Target X Velocity ('voltage')", output.in(Volts)); - SignalLogger.writeDouble("X Position", getPose().getX()); - SignalLogger.writeDouble("X Velocity", - getChassisSpeeds().vxMetersPerSecond * getPose().getRotation().getCos()); - }, - null, - this)); - - /* + */ + private final SysIdRoutine m_sysIdRoutineChassisTranslation = new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in meters per second², but SysId only supports "volts per second" */ + Volts.of(0.5).per(Second), + /* This is in meters per second, but SysId only supports "volts" */ + Volts.of(Settings.Swerve.Constraints.MAX_VELOCITY_M_PER_S), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdChassisTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> { + /* output is actually meters per second, but SysId only supports "volts" */ + setControl(getRobotCentricSwerveRequest().withVelocityX(output.in(Volts)).withVelocityY(0) + .withRotationalRate(0)); + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Target X Velocity ('voltage')", output.in(Volts)); + SignalLogger.writeDouble("X Position", getPose().getX()); + SignalLogger.writeDouble("X Velocity", + getChassisSpeeds().vxMetersPerSecond * getPose().getRotation().getCos()); + }, + null, + this)); + + /* * SysId routine for characterizing steer. This is used to find PID gains for * the steer motors. - */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this)); - - /* + */ + private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), + new SysIdRoutine.Mechanism( + volts -> setControl(m_steerCharacterization.withVolts(volts)), + null, + this)); + + /* * SysId routine for characterizing rotation. * This is used to find PID gains for the FieldCentricFacingAngle * HeadingController. * See the documentation of SwerveRequest.SysIdSwerveRotation for info on * importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Target_Rate ('voltage')", output.in(Volts)); - SignalLogger.writeDouble("Rotational Position", getPose().getRotation().getRadians()); - SignalLogger.writeDouble("Rotational_Velocity", getState().Speeds.omegaRadiansPerSecond); - }, - null, - this)); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineModuleTranslation; - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not - * construct - * the devices themselves. If they need the devices, they can access them - * through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - protected CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - - leftBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/LeftBehindHubY", Pose2d.struct).publish(); - rightBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/RightBehindHubY", Pose2d.struct).publish(); - vertexBehindHubPublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/VertexBehindHub", Pose2d.struct).publish(); - - robotAccelerationX = this.getPigeon2().getAccelerationX(); - robotAccelerationY = this.getPigeon2().getAccelerationY(); - - PhoenixUtil.registerToCanivore( - robotAccelerationX, - robotAccelerationY - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not - * construct - * the devices themselves. If they need the devices, they can access them - * through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - private CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules) { - super(drivetrainConstants, odometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not - * construct - * the devices themselves. If they need the devices, they can access them - * through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve - * drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz - * on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry - * calculation - * in the form [x, y, theta]ᵀ, with units in - * meters - * and radians - * @param visionStandardDeviation The standard deviation for vision - * calculation - * in the form [x, y, theta]ᵀ, with units in - * meters - * and radians - * @param modules Constants for each specific module - */ - private CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, - modules); - if (Utils.isSimulation()) { - startSimThread(); - } - } - - @Override - public void setControl(SwerveRequest request) { - if (EnabledSubsystems.SWERVE.get()) { - super.setControl(request); - } else { - super.setControl(new SwerveRequest.Idle()); - } - } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - - public Command sysidRotationDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineRotation.dynamic(direction); - } - - public Command sysidRotationQuasiStatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineRotation.quasistatic(direction); - } - - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } - - private boolean checkIfVisionMeasurementValid(Pose2d visionPose) { - return !Settings.ENABLE_DISTANCE_CHECK.get() || visionPose.getTranslation().getDistance(getState().Pose.getTranslation()) < Settings.Swerve.MAX_ACCEPTABLE_VISION_DEVIATION_METERS; - } - - /** - * Adds a vision measurement to the Kalman Filter. This will correct the - * odometry pose estimate - * while still accounting for measurement noise. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision - * camera. - * @param timestampSeconds The timestamp of the vision measurement in - * seconds. - */ - @Override - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - // SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); - // SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); - - if(checkIfVisionMeasurementValid(visionRobotPoseMeters)) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); - } - } - - /** - * Adds a vision measurement to the Kalman Filter. This will correct the - * odometry pose estimate - * while still accounting for measurement noise. - *

- * Note that the vision measurement standard deviations passed into this method - * will continue to apply to future measurements until a subsequent call to - * {@link #setVisionMeasurementStdDevs(Matrix)} or this method. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the - * vision camera. - * @param timestampSeconds The timestamp of the vision measurement in - * seconds. - * @param visionMeasurementStdDevs Standard deviations of the vision pose - * measurement - * in the form [x, y, theta]ᵀ, with units in - * meters and radians. - */ - @Override - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { - // SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); - // SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); - - if(checkIfVisionMeasurementValid(visionRobotPoseMeters)) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), - visionMeasurementStdDevs); - } - } - - public Pose2d getPose() { - Pose2d proposedPose = getState().Pose; - double proposedX = proposedPose.getX(); - double proposedY = proposedPose.getY(); - double poseDelta = lastGoodPose.getTranslation().getDistance(proposedPose.getTranslation()); - - // if (!(proposedX > Field.LENGTH || proposedX < 0 || proposedY > Field.WIDTH || proposedY < 0) && - // poseDelta <= Settings.Swerve.MAX_ACCEPTABLE_POSE_DELTA_METERS) { - lastGoodPose = proposedPose; - // } - - return lastGoodPose; - } - - public void configureAutoBuilder() { - try { - AutoBuilder.configure( - this::getPose, - this::resetPose, - this::getChassisSpeeds, - this::setChassisSpeeds, - new PPHolonomicDriveController(Gains.Swerve.Alignment.XY, Gains.Swerve.Alignment.THETA), - RobotConfig.fromGUISettings(), - () -> false, - instance); - PathPlannerLogging.setLogActivePathCallback((poses) -> { - if (Robot.isBlue()) { - Field.FIELD2D.getObject("path").setPoses(poses); - - } else { - Field.FIELD2D.getObject("path").setPoses(Field.transformToOppositeAlliance(poses)); - } - }); - } catch (Exception e) { - e.printStackTrace(); - } - } - - public Command followPathCommand(String pathName) { - try { - return followPathCommand(PathPlannerPath.fromPathFile(pathName)); - } catch (Exception e) { - throw new IllegalArgumentException(pathName + " does not exist"); - } - } - - public Command followPathCommand(PathPlannerPath path) { - return AutoBuilder.followPath(path); - } - - public SwerveModuleState[] getModuleStates() { - return getState().ModuleStates; - } - - public ChassisSpeeds getChassisSpeeds() { - return getState().Speeds; - } - - public Vector2D getFieldRelativeSpeeds() { - return new Vector2D(getChassisSpeeds().vxMetersPerSecond, getChassisSpeeds().vyMetersPerSecond) - .rotate(Angle.fromRotation2d(getPose().getRotation())); - } - - private void setChassisSpeeds(ChassisSpeeds robotSpeeds) { - setControl(new SwerveRequest.RobotCentric() - .withVelocityX(robotSpeeds.vxMetersPerSecond) - .withVelocityY(robotSpeeds.vyMetersPerSecond) - .withRotationalRate(robotSpeeds.omegaRadiansPerSecond)); - } - - public void drive(Vector2D velocity, double rotation) { - ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - Robot.isBlue() ? velocity.y : -velocity.y, - Robot.isBlue() ? -velocity.x : velocity.x, - -rotation, - getPose().getRotation()); - - Pose2d robotVel = new Pose2d( - Settings.DT * speeds.vxMetersPerSecond, - Settings.DT * speeds.vyMetersPerSecond, - Rotation2d.fromRadians(Settings.DT * speeds.omegaRadiansPerSecond)); - Twist2d twistVel = new Pose2d().log(robotVel); - - setChassisSpeeds(new ChassisSpeeds( - twistVel.dx / Settings.DT, - twistVel.dy / Settings.DT, - twistVel.dtheta / Settings.DT)); - } - - public Pose2d getTurretPose() { - return turretPose; - } - - public double[] getWheelDrivePositionsRadians() { - double[] positions = new double[4]; - double kDriveGearRatio = 7.67; - for (int i = 0; i < 4; i++) { - positions[i] = getModule(i).getDriveMotor().getPosition().getValueAsDouble() * 2 * Math.PI - / kDriveGearRatio; - } - return positions; - } - - public boolean isUnderTrench() { - Translation2d turretTranslation = getTurretPose().getTranslation(); - - boolean isBetweenRightTrenchesY = Field.AllianceRightTrench.rightEdge.getY() < turretTranslation.getY() - && Field.AllianceRightTrench.leftEdge.getY() > turretTranslation.getY(); - - boolean isBetweenLeftTrenchesY = Field.AllianceLeftTrench.rightEdge.getY() < turretTranslation.getY() - && Field.AllianceLeftTrench.leftEdge.getY() > turretTranslation.getY(); - - boolean isUnderAllianceTrenchX = Math.abs( - turretTranslation.getX() - Field.AllianceRightTrench.rightEdge.getX()) < Field.TRENCH_HOOD_TOLERANCE; - - boolean isUnderOpponentTrenchX = Math.abs( - turretTranslation.getX() - Field.OpponentRightTrench.rightEdge.getX()) < Field.TRENCH_HOOD_TOLERANCE; - - boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) - && (isUnderAllianceTrenchX || isUnderOpponentTrenchX); - - return isUnderTrench; - } - - public boolean isInOpponentZone() { - Translation2d turretTranslation = getTurretPose().getTranslation(); - return turretTranslation.getX() > Field.OPPONENT_ZONE_X; - } - - public boolean isBehindTower() { - boolean withinTowerX = getPose().getTranslation().getX() < Field.TOWER_FAR_CENTER.getX(); - boolean withinTowerY = Field.TOWER_FAR_RIGHT.getY() < getTurretPose().getTranslation().getY() - && getTurretPose().getTranslation().getY() < Field.TOWER_FAR_LEFT.getY(); - return withinTowerX && withinTowerY; - } - - /** - * Checks whether the robot turret pose is behind the hub or not in the neutral zone. - * For stopping ferrying to prevent fuel hitting the hub. - * - * @return true if robot turret pose is behind the hub. - */ - public void updateIsBehindHub() { - // === TRIANGLE === (CUSTOM VERTEX) - Translation2d turretTranslation = getTurretPose().getTranslation(); - - boolean behindHubX = Field.HUB_FAR_LEFT_CORNER.getX() < turretTranslation.getX(); - // && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubToleranceX; // With this line the triangle will be cut to more like a trapezoid. - - Pose2d hubFarLeftCornerWithTolerance = new Pose2d(Field.HUB_FAR_LEFT_CORNER.getX(), Field.HUB_FAR_LEFT_CORNER.getY() + Field.BEHIND_HUB_TOLERANCE_Y, new Rotation2d()); - Pose2d hubFarRightCornerWithTolerance = new Pose2d(Field.HUB_FAR_RIGHT_CORNER.getX(), Field.HUB_FAR_RIGHT_CORNER.getY() - Field.BEHIND_HUB_TOLERANCE_Y, new Rotation2d()); - - // Find point on triangle using the point-slope formula (of the line constructed by the hub corner pose and ferry pose) - // y = (slope)(robotX - hubCornerX) + (hubCornerY) - // where the slope = (hubCornerY - vertexY)/(hubCornerX - vertexX) - double leftY = ((hubFarLeftCornerWithTolerance.getY() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getY()) - /(hubFarLeftCornerWithTolerance.getX() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getX())) // (Slope) - * (turretTranslation.getX() - hubFarLeftCornerWithTolerance.getX()) + hubFarLeftCornerWithTolerance.getY(); // *(robotX - hubCornerX) + (hubCornerY) - double rightY = ((hubFarRightCornerWithTolerance.getY() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getY()) - /(hubFarRightCornerWithTolerance.getX() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getX())) // (Slope) - * (turretTranslation.getX() - hubFarRightCornerWithTolerance.getX()) + hubFarRightCornerWithTolerance.getY(); // *(robotX - hubCornerX) + (hubCornerY) - - // Debug: - leftBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), leftY, new Rotation2d())); - rightBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), rightY, new Rotation2d())); - vertexBehindHubPublisher.set(Field.BEHIND_HUB_TRIANGLE_VERTEX); - - boolean withinHubY = rightY < getTurretPose().getY() - && getTurretPose().getY() < leftY; - - isBehindHub = behindHubX && withinHubY; - - // === TRIANGLE === (FROM FERRY ZONES): - // Translation2d turretTranslation = getTurretPose().getTranslation(); - // boolean behindHubX = Field.hubFarLeftCorner.getX() < turretTranslation.getX(); - // // && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubToleranceX; // With this line the triangle will be cut to more like a trapezoid. - - // // Find point on triangle using the point-slope formula (of the line constructed by the hub corner pose and ferry pose) - // // y = (slope)(robotX - hubCornerX) + (hubCornerY) - // // where the slope = (hubCornerY - ferryY)/(hubCornerX - ferryX) - // double leftY = ((Field.hubFarLeftCorner.getY() - Field.leftFerryZone.getY())/(Field.hubFarLeftCorner.getX() - Field.leftFerryZone.getX())) // (Slope) - // * (turretTranslation.getX() - Field.hubFarLeftCorner.getX()) + Field.hubFarLeftCorner.getY(); // *(robotX - hubCornerX) + (hubCornerY) - // double rightY = ((Field.hubFarRightCorner.getY() - Field.rightFerryZone.getY())/(Field.hubFarRightCorner.getX() - Field.rightFerryZone.getX())) // (Slope) - // * (turretTranslation.getX() - Field.hubFarRightCorner.getX()) + Field.hubFarRightCorner.getY(); // *(robotX - hubCornerX) + (hubCornerY) - - // leftBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), leftY - Field.hubToleranceY, new Rotation2d())); - // rightBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), rightY + Field.hubToleranceY, new Rotation2d())); - - // boolean withinHubY = rightY + Field.hubToleranceY < getTurretPose().getY() - // && getTurretPose().getY() < leftY - Field.hubToleranceY; - - // return behindHubX && withinHubY; - - // === RECTANGLE ===: - // Translation2d turretTranslation = getTurretPose().getTranslation(); - // boolean behindHubX = Field.hubFarLeftCorner.getX() < turretTranslation.getX() - // && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubToleranceX; - // boolean withinHubY = Field.hubFarRightCorner.getY() + Field.hubToleranceY < getTurretPose().getY() - // && getTurretPose().getY() < Field.hubFarLeftCorner.getY() - Field.hubToleranceY; - - // return behindHubX && withinHubY; - } - - public boolean isOutsideAllianceZone() { - return getPose().getX() > Field.AllianceRightTrench.rightEdge.getX() + Field.TRENCH_HOOD_TOLERANCE; - } - - public boolean getIsBehindHub() { - return isBehindHub; - } - - public void teleopInit() { - for (SwerveModule module : getModules()) { - TalonFXConfiguration newConfigs = new TalonFXConfiguration(); - module.getDriveMotor().getConfigurator().refresh(newConfigs); - newConfigs = newConfigs - .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(80) - .withStatorCurrentLimitEnable(true)); - - module.getDriveMotor().getConfigurator().apply(newConfigs); - } - } - - public void autonInit() { - for (SwerveModule module : getModules()) { - TalonFXConfiguration newConfigs = new TalonFXConfiguration(); - module.getDriveMotor().getConfigurator().refresh(newConfigs); - newConfigs = newConfigs - .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(120) - .withStatorCurrentLimitEnable(true)); - - module.getDriveMotor().getConfigurator().apply(newConfigs); - } - } - - public double getTotalDriveSupplyCurrent() { - double total = 0.0; - - for (SwerveModule module : getModules()) { - total += Double.max(0, module.getDriveMotor().getSupplyCurrent().getValueAsDouble()); - } - - return total; - } - - public double getTotalSteerSupplyCurrent() { - double total = 0.0; - - for (SwerveModule module : getModules()) { - total += Double.max(0, module.getSteerMotor().getSupplyCurrent().getValueAsDouble()); - } - - return total; - } - - public boolean canShootIntoHub() { - Superstructure superstructure = Superstructure.getInstance(); - SuperstructureState state = superstructure.getState(); - return !isOutsideAllianceZone() || (state == SuperstructureState.KB || state == SuperstructureState.LEFT_CORNER || state == SuperstructureState.RIGHT_CORNER); - } - - public void periodicAfterScheduler() { - Pose2d pose = getPose(); - turretPose = new Pose2d( - pose.getTranslation().plus( - Settings.Superstructure.Turret.TURRET_OFFSET.getTranslation().rotateBy(pose.getRotation())), - pose.getRotation().plus(Turret.getInstance().getAngle())); - - robotPose.set(pose); - - turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); - - updateIsBehindHub(); - - ChassisSpeeds chassisSpeeds = getChassisSpeeds(); - Vector2D fieldRelativeSpeeds = getFieldRelativeSpeeds(); - SmartDashboard.putNumber("Swerve/Velocity Robot Relative X (m per s)", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber("Swerve/Velocity Robot Relative Y (m per s)", chassisSpeeds.vyMetersPerSecond); - - SmartDashboard.putNumber("Swerve/Velocity Field Relative X (m per s)", fieldRelativeSpeeds.x); - SmartDashboard.putNumber("Swerve/Field Relative Rotation", pose.getRotation().getDegrees()); - SmartDashboard.putNumber("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); - - SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", chassisSpeeds.omegaRadiansPerSecond); - SmartDashboard.putNumber("Swerve/Distance From Hub (meters)", - Field.HUB_CENTER.getTranslation().getDistance(pose.getTranslation())); - - Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); - - if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { - SmartDashboard.putNumber("Swerve/Robot Accel X", robotAccelerationX.getValueAsDouble() * 9.81); - SmartDashboard.putNumber("Swerve/Robot Accel Y", robotAccelerationY.getValueAsDouble() * 9.81); - - SmartDashboard.putNumber("Swerve/Failed DAQ Count", this.getState().FailedDaqs); - SmartDashboard.putNumber("Swerve/CANBus Utiliaztion", Ports.CANIVORE.getStatus().BusUtilization); - // will confirm whether we are even getting data - - SmartDashboard.putBoolean("FieldPositions/isBehindTower", isBehindTower()); - SmartDashboard.putBoolean("FieldPositions/isUnderTrench", isUnderTrench()); - SmartDashboard.putBoolean("FieldPositions/isInOpponentZone", isInOpponentZone()); - - SmartDashboard.putNumber("Superstructure/Turret/Dist From Hub", - turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); - SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Hub", - turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); - SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Ferry Zone", turretPose.getTranslation() - .getDistance(Field.getFerryZonePose(pose.getTranslation()).getTranslation())); - - for (int i = 0; i < 4; i++) { - String prefix = "Swerve/Modules/Module " + i; - SwerveModuleState current = getModule(i).getCurrentState(); - SwerveModuleState target = getModule(i).getTargetState(); - - SmartDashboard.putNumber(prefix + "/Speed (m per s)", current.speedMetersPerSecond); - SmartDashboard.putNumber(prefix + "/Target Speed (m per s)", target.speedMetersPerSecond); - SmartDashboard.putNumber(prefix + "/Angle (deg)", current.angle.getDegrees() % 360); - SmartDashboard.putNumber(prefix + "/Target Angle (deg)", target.angle.getDegrees() % 360); - - if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber(prefix + "/Stator Current", - getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber(prefix + "/Supply Current", - getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble()); - } - } - Robot.getEnergyUtil().logEnergyUsage(getName() + " Drive", getTotalDriveSupplyCurrent()); - Robot.getEnergyUtil().logEnergyUsage(getName() + " Turn", getTotalSteerSupplyCurrent()); - - // CAN SIGNAL LOGGING - if (Settings.DEBUG_MODE.get() && Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Front Left Drive Motor Connected? (ID " - + String.valueOf(TunerConstants.kFrontLeftDriveMotorId) + ")", - getModule(0).getDriveMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Front Left Steer Motor Connected? (ID " - + String.valueOf(TunerConstants.kFrontLeftSteerMotorId) + ")", - getModule(0).getSteerMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Front Left CANcoder Connected? (ID " - + String.valueOf(TunerConstants.kFrontLeftEncoderId) + ")", - getModule(0).getEncoder().isConnected()); - - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Front Right Drive Motor Connected? (ID " - + String.valueOf(TunerConstants.kFrontRightDriveMotorId) + ")", - getModule(1).getDriveMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Front Right Steer Motor Connected? (ID " - + String.valueOf(TunerConstants.kFrontRightSteerMotorId) + ")", - getModule(1).getSteerMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Front Right CANcoder Connected? (ID " - + String.valueOf(TunerConstants.kFrontRightEncoderId) + ")", - getModule(1).getEncoder().isConnected()); - - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Back Left Drive Motor Connected? (ID " - + String.valueOf(TunerConstants.kBackLeftDriveMotorId) + ")", - getModule(2).getDriveMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Back Left Steer Motor Connected? (ID " - + String.valueOf(TunerConstants.kBackLeftSteerMotorId) + ")", - getModule(2).getSteerMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Back Left CANcoder Connected? (ID " - + String.valueOf(TunerConstants.kBackLeftEncoderId) + ")", - getModule(2).getEncoder().isConnected()); - - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Back Right Drive Motor Connected? (ID " - + String.valueOf(TunerConstants.kBackRightDriveMotorId) + ")", - getModule(3).getDriveMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Back Right Steer Motor Connected? (ID " - + String.valueOf(TunerConstants.kBackRightSteerMotorId) + ")", - getModule(3).getSteerMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Back Right CANcoder Connected? (ID " - + String.valueOf(TunerConstants.kBackRightEncoderId) + ")", - getModule(3).getEncoder().isConnected()); - - } - } - } -} \ No newline at end of file + */ + private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in radians per second², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> { + /* output is actually radians per second, but SysId only supports "volts" */ + setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Rotational_Target_Rate ('voltage')", output.in(Volts)); + SignalLogger.writeDouble("Rotational Position", getPose().getRotation().getRadians()); + SignalLogger.writeDouble("Rotational_Velocity", getState().Speeds.omegaRadiansPerSecond); + }, + null, + this)); + + /* The SysId routine to test */ + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineModuleTranslation; + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not + * construct the devices themselves. If they need the devices, they can + * access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + protected CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { + super(drivetrainConstants, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + + leftBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/LeftBehindHubY", Pose2d.struct).publish(); + rightBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/RightBehindHubY", Pose2d.struct).publish(); + vertexBehindHubPublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/VertexBehindHub", Pose2d.struct).publish(); + + robotAccelerationX = this.getPigeon2().getAccelerationX(); + robotAccelerationY = this.getPigeon2().getAccelerationY(); + + PhoenixUtil.registerToCanivore( + robotAccelerationX, + robotAccelerationY + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not + * construct the devices themselves. If they need the devices, they can + * access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN + * 2.0. + * @param modules Constants for each specific module + */ + private CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(drivetrainConstants, odometryUpdateFrequency, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not + * construct the devices themselves. If they need the devices, they can + * access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN + * 2.0. + * @param odometryStandardDeviation The standard deviation for odometry + * calculation in the form [x, y, theta]ᵀ, with units in meters and radians + * @param visionStandardDeviation The standard deviation for vision + * calculation in the form [x, y, theta]ᵀ, with units in meters and radians + * @param modules Constants for each specific module + */ + private CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, + modules); + if (Utils.isSimulation()) { + startSimThread(); + } + } + + @Override + public void setControl(SwerveRequest request) { + if (EnabledSubsystems.SWERVE.get()) { + super.setControl(request); + } else { + super.setControl(new SwerveRequest.Idle()); + } + } + + /** + * Runs the SysId Quasistatic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Quasistatic test + * @return Command to run + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.quasistatic(direction); + } + + /** + * Runs the SysId Dynamic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Dynamic test + * @return Command to run + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.dynamic(direction); + } + + public Command sysidRotationDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineRotation.dynamic(direction); + } + + public Command sysidRotationQuasiStatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineRotation.quasistatic(direction); + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } + + private boolean checkIfVisionMeasurementValid(Pose2d visionPose) { + return !Settings.ENABLE_DISTANCE_CHECK.get() || visionPose.getTranslation().getDistance(getState().Pose.getTranslation()) < Settings.Swerve.MAX_ACCEPTABLE_VISION_DEVIATION_METERS; + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the + * odometry pose estimate while still accounting for measurement noise. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the + * vision camera. + * @param timestampSeconds The timestamp of the vision measurement in + * seconds. + */ + @Override + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + // SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); + // SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); + + if (checkIfVisionMeasurementValid(visionRobotPoseMeters)) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + } + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the + * odometry pose estimate while still accounting for measurement noise. + *

+ * Note that the vision measurement standard deviations passed into this + * method will continue to apply to future measurements until a subsequent + * call to {@link #setVisionMeasurementStdDevs(Matrix)} or this method. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the + * vision camera. + * @param timestampSeconds The timestamp of the vision measurement in + * seconds. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement in the form [x, y, theta]ᵀ, with units in meters and radians. + */ + @Override + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { + // SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); + // SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); + + if (checkIfVisionMeasurementValid(visionRobotPoseMeters)) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), + visionMeasurementStdDevs); + } + } + + public Pose2d getPose() { + Pose2d proposedPose = getState().Pose; + double proposedX = proposedPose.getX(); + double proposedY = proposedPose.getY(); + double poseDelta = lastGoodPose.getTranslation().getDistance(proposedPose.getTranslation()); + + // if (!(proposedX > Field.LENGTH || proposedX < 0 || proposedY > Field.WIDTH || proposedY < 0) && + // poseDelta <= Settings.Swerve.MAX_ACCEPTABLE_POSE_DELTA_METERS) { + lastGoodPose = proposedPose; + // } + + return lastGoodPose; + } + + public void configureAutoBuilder() { + try { + AutoBuilder.configure( + this::getPose, + this::resetPose, + this::getChassisSpeeds, + this::setChassisSpeeds, + new PPHolonomicDriveController(Gains.Swerve.Alignment.XY, Gains.Swerve.Alignment.THETA), + RobotConfig.fromGUISettings(), + () -> false, + instance); + PathPlannerLogging.setLogActivePathCallback((poses) -> { + if (Robot.isBlue()) { + Field.FIELD2D.getObject("path").setPoses(poses); + + } else { + Field.FIELD2D.getObject("path").setPoses(Field.transformToOppositeAlliance(poses)); + } + }); + } catch (Exception e) { + e.printStackTrace(); + } + } + + public Command followPathCommand(String pathName) { + try { + return followPathCommand(PathPlannerPath.fromPathFile(pathName)); + } catch (Exception e) { + throw new IllegalArgumentException(pathName + " does not exist"); + } + } + + public Command followPathCommand(PathPlannerPath path) { + return AutoBuilder.followPath(path); + } + + public SwerveModuleState[] getModuleStates() { + return getState().ModuleStates; + } + + public ChassisSpeeds getChassisSpeeds() { + return getState().Speeds; + } + + public Vector2D getFieldRelativeSpeeds() { + return new Vector2D(getChassisSpeeds().vxMetersPerSecond, getChassisSpeeds().vyMetersPerSecond) + .rotate(Angle.fromRotation2d(getPose().getRotation())); + } + + private void setChassisSpeeds(ChassisSpeeds robotSpeeds) { + setControl(new SwerveRequest.RobotCentric() + .withVelocityX(robotSpeeds.vxMetersPerSecond) + .withVelocityY(robotSpeeds.vyMetersPerSecond) + .withRotationalRate(robotSpeeds.omegaRadiansPerSecond)); + } + + public void drive(Vector2D velocity, double rotation) { + ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( + Robot.isBlue() ? velocity.y : -velocity.y, + Robot.isBlue() ? -velocity.x : velocity.x, + -rotation, + getPose().getRotation()); + + Pose2d robotVel = new Pose2d( + Settings.DT * speeds.vxMetersPerSecond, + Settings.DT * speeds.vyMetersPerSecond, + Rotation2d.fromRadians(Settings.DT * speeds.omegaRadiansPerSecond)); + Twist2d twistVel = new Pose2d().log(robotVel); + + setChassisSpeeds(new ChassisSpeeds( + twistVel.dx / Settings.DT, + twistVel.dy / Settings.DT, + twistVel.dtheta / Settings.DT)); + } + + public Pose2d getTurretPose() { + return turretPose; + } + + public double[] getWheelDrivePositionsRadians() { + double[] positions = new double[4]; + double kDriveGearRatio = 7.67; + for (int i = 0; i < 4; i++) { + positions[i] = getModule(i).getDriveMotor().getPosition().getValueAsDouble() * 2 * Math.PI + / kDriveGearRatio; + } + return positions; + } + + public boolean isUnderTrench() { + if (isUnderTrench.isEmpty()) { + Translation2d turretTranslation = getTurretPose().getTranslation(); + + boolean isBetweenRightTrenchesY = Field.AllianceRightTrench.rightEdge.getY() < turretTranslation.getY() + && Field.AllianceRightTrench.leftEdge.getY() > turretTranslation.getY(); + + boolean isBetweenLeftTrenchesY = Field.AllianceLeftTrench.rightEdge.getY() < turretTranslation.getY() + && Field.AllianceLeftTrench.leftEdge.getY() > turretTranslation.getY(); + + boolean isUnderAllianceTrenchX = Math.abs( + turretTranslation.getX() - Field.AllianceRightTrench.rightEdge.getX()) < Field.TRENCH_HOOD_TOLERANCE; + + boolean isUnderOpponentTrenchX = Math.abs( + turretTranslation.getX() - Field.OpponentRightTrench.rightEdge.getX()) < Field.TRENCH_HOOD_TOLERANCE; + + boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) + && (isUnderAllianceTrenchX || isUnderOpponentTrenchX); + + this.isUnderTrench = Optional.of(isUnderTrench); + } + + return isUnderTrench.get(); + } + + public boolean isInOpponentZone() { + if (isInOpponentZone.isEmpty()) { + Translation2d turretTranslation = getTurretPose().getTranslation(); + isInOpponentZone = Optional.of(turretTranslation.getX() > Field.OPPONENT_ZONE_X); + } + return isInOpponentZone.get(); + } + + public boolean isBehindTower() { + if (isBehindTower.isEmpty()) { + boolean withinTowerX = getPose().getTranslation().getX() < Field.TOWER_FAR_CENTER.getX(); + boolean withinTowerY = Field.TOWER_FAR_RIGHT.getY() < getTurretPose().getTranslation().getY() + && getTurretPose().getTranslation().getY() < Field.TOWER_FAR_LEFT.getY(); + isBehindTower = Optional.of(withinTowerX && withinTowerY); + } + + return isBehindTower.get(); + } + + /** + * Checks whether the robot turret pose is behind the hub or not in the + * neutral zone. For stopping ferrying to prevent fuel hitting the hub. + * + * @return true if robot turret pose is behind the hub. + */ + public boolean isBehindHub() { + if (isBehindHub.isEmpty()) { + // === TRIANGLE === (CUSTOM VERTEX) + Translation2d turretTranslation = getTurretPose().getTranslation(); + + boolean behindHubX = Field.HUB_FAR_LEFT_CORNER.getX() < turretTranslation.getX(); + // && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubToleranceX; // With this line the triangle will be cut to more like a trapezoid. + + Pose2d hubFarLeftCornerWithTolerance = new Pose2d(Field.HUB_FAR_LEFT_CORNER.getX(), Field.HUB_FAR_LEFT_CORNER.getY() + Field.BEHIND_HUB_TOLERANCE_Y, new Rotation2d()); + Pose2d hubFarRightCornerWithTolerance = new Pose2d(Field.HUB_FAR_RIGHT_CORNER.getX(), Field.HUB_FAR_RIGHT_CORNER.getY() - Field.BEHIND_HUB_TOLERANCE_Y, new Rotation2d()); + + // Find point on triangle using the point-slope formula (of the line constructed by the hub corner pose and ferry pose) + // y = (slope)(robotX - hubCornerX) + (hubCornerY) + // where the slope = (hubCornerY - vertexY)/(hubCornerX - vertexX) + double leftY = ((hubFarLeftCornerWithTolerance.getY() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getY()) + / (hubFarLeftCornerWithTolerance.getX() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getX())) // (Slope) + * (turretTranslation.getX() - hubFarLeftCornerWithTolerance.getX()) + hubFarLeftCornerWithTolerance.getY(); // *(robotX - hubCornerX) + (hubCornerY) + double rightY = ((hubFarRightCornerWithTolerance.getY() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getY()) + / (hubFarRightCornerWithTolerance.getX() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getX())) // (Slope) + * (turretTranslation.getX() - hubFarRightCornerWithTolerance.getX()) + hubFarRightCornerWithTolerance.getY(); // *(robotX - hubCornerX) + (hubCornerY) + + // Debug: + leftBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), leftY, new Rotation2d())); + rightBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), rightY, new Rotation2d())); + vertexBehindHubPublisher.set(Field.BEHIND_HUB_TRIANGLE_VERTEX); + + boolean withinHubY = rightY < getTurretPose().getY() + && getTurretPose().getY() < leftY; + + isBehindHub = Optional.of(behindHubX && withinHubY); + + // === TRIANGLE === (FROM FERRY ZONES): + // Translation2d turretTranslation = getTurretPose().getTranslation(); + // boolean behindHubX = Field.hubFarLeftCorner.getX() < turretTranslation.getX(); + // // && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubToleranceX; // With this line the triangle will be cut to more like a trapezoid. + // // Find point on triangle using the point-slope formula (of the line constructed by the hub corner pose and ferry pose) + // // y = (slope)(robotX - hubCornerX) + (hubCornerY) + // // where the slope = (hubCornerY - ferryY)/(hubCornerX - ferryX) + // double leftY = ((Field.hubFarLeftCorner.getY() - Field.leftFerryZone.getY())/(Field.hubFarLeftCorner.getX() - Field.leftFerryZone.getX())) // (Slope) + // * (turretTranslation.getX() - Field.hubFarLeftCorner.getX()) + Field.hubFarLeftCorner.getY(); // *(robotX - hubCornerX) + (hubCornerY) + // double rightY = ((Field.hubFarRightCorner.getY() - Field.rightFerryZone.getY())/(Field.hubFarRightCorner.getX() - Field.rightFerryZone.getX())) // (Slope) + // * (turretTranslation.getX() - Field.hubFarRightCorner.getX()) + Field.hubFarRightCorner.getY(); // *(robotX - hubCornerX) + (hubCornerY) + // leftBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), leftY - Field.hubToleranceY, new Rotation2d())); + // rightBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), rightY + Field.hubToleranceY, new Rotation2d())); + // boolean withinHubY = rightY + Field.hubToleranceY < getTurretPose().getY() + // && getTurretPose().getY() < leftY - Field.hubToleranceY; + // return behindHubX && withinHubY; + // === RECTANGLE ===: + // Translation2d turretTranslation = getTurretPose().getTranslation(); + // boolean behindHubX = Field.hubFarLeftCorner.getX() < turretTranslation.getX() + // && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubToleranceX; + // boolean withinHubY = Field.hubFarRightCorner.getY() + Field.hubToleranceY < getTurretPose().getY() + // && getTurretPose().getY() < Field.hubFarLeftCorner.getY() - Field.hubToleranceY; + // return behindHubX && withinHubY; + } + + return isBehindHub.get(); + } + + public boolean isOutsideAllianceZone() { + if (isOutsideAllianceZone.isEmpty()) { + isOutsideAllianceZone = Optional.of(getPose().getX() > Field.AllianceRightTrench.rightEdge.getX() + Field.TRENCH_HOOD_TOLERANCE); + } + + return isOutsideAllianceZone.get(); + } + + public void clearMemoized() { + isBehindHub = Optional.empty(); + isOutsideAllianceZone = Optional.empty(); + isInOpponentZone = Optional.empty(); + isUnderTrench = Optional.empty(); + isBehindTower = Optional.empty(); + } + + public void teleopInit() { + for (SwerveModule module : getModules()) { + TalonFXConfiguration newConfigs = new TalonFXConfiguration(); + module.getDriveMotor().getConfigurator().refresh(newConfigs); + newConfigs = newConfigs + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(80) + .withStatorCurrentLimitEnable(true)); + + module.getDriveMotor().getConfigurator().apply(newConfigs); + } + } + + public void autonInit() { + for (SwerveModule module : getModules()) { + TalonFXConfiguration newConfigs = new TalonFXConfiguration(); + module.getDriveMotor().getConfigurator().refresh(newConfigs); + newConfigs = newConfigs + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(120) + .withStatorCurrentLimitEnable(true)); + + module.getDriveMotor().getConfigurator().apply(newConfigs); + } + } + + public double getTotalDriveSupplyCurrent() { + double total = 0.0; + + for (SwerveModule module : getModules()) { + total += Double.max(0, module.getDriveMotor().getSupplyCurrent().getValueAsDouble()); + } + + return total; + } + + public double getTotalSteerSupplyCurrent() { + double total = 0.0; + + for (SwerveModule module : getModules()) { + total += Double.max(0, module.getSteerMotor().getSupplyCurrent().getValueAsDouble()); + } + + return total; + } + + public boolean canShootIntoHub() { + Superstructure superstructure = Superstructure.getInstance(); + SuperstructureState state = superstructure.getState(); + return !isOutsideAllianceZone() || (state == SuperstructureState.KB || state == SuperstructureState.LEFT_CORNER || state == SuperstructureState.RIGHT_CORNER); + } + + public void periodicAfterScheduler() { + Pose2d pose = getPose(); + turretPose = new Pose2d( + pose.getTranslation().plus( + Settings.Superstructure.Turret.TURRET_OFFSET.getTranslation().rotateBy(pose.getRotation())), + pose.getRotation().plus(Turret.getInstance().getAngle())); + + robotPose.set(pose); + + turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); + + ChassisSpeeds chassisSpeeds = getChassisSpeeds(); + Vector2D fieldRelativeSpeeds = getFieldRelativeSpeeds(); + SmartDashboard.putNumber("Swerve/Velocity Robot Relative X (m per s)", chassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber("Swerve/Velocity Robot Relative Y (m per s)", chassisSpeeds.vyMetersPerSecond); + + SmartDashboard.putNumber("Swerve/Velocity Field Relative X (m per s)", fieldRelativeSpeeds.x); + SmartDashboard.putNumber("Swerve/Field Relative Rotation", pose.getRotation().getDegrees()); + SmartDashboard.putNumber("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); + + SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", chassisSpeeds.omegaRadiansPerSecond); + SmartDashboard.putNumber("Swerve/Distance From Hub (meters)", Field.HUB_CENTER.getTranslation().getDistance(pose.getTranslation())); + + Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); + + if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { + SmartDashboard.putNumber("Swerve/Robot Accel X", robotAccelerationX.getValueAsDouble() * 9.81); + SmartDashboard.putNumber("Swerve/Robot Accel Y", robotAccelerationY.getValueAsDouble() * 9.81); + + SmartDashboard.putNumber("Swerve/Failed DAQ Count", this.getState().FailedDaqs); + SmartDashboard.putNumber("Swerve/CANBus Utiliaztion", Ports.CANIVORE.getStatus().BusUtilization); + // will confirm whether we are even getting data + + SmartDashboard.putNumber("Superstructure/Turret/Dist From Hub", + turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); + SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Hub", + turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); + SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Ferry Zone", turretPose.getTranslation() + .getDistance(Field.getFerryZonePose(pose.getTranslation()).getTranslation())); + + for (int i = 0; i < 4; i++) { + String prefix = "Swerve/Modules/Module " + i; + SwerveModuleState current = getModule(i).getCurrentState(); + SwerveModuleState target = getModule(i).getTargetState(); + + SmartDashboard.putNumber(prefix + "/Speed (m per s)", current.speedMetersPerSecond); + SmartDashboard.putNumber(prefix + "/Target Speed (m per s)", target.speedMetersPerSecond); + SmartDashboard.putNumber(prefix + "/Angle (deg)", current.angle.getDegrees() % 360); + SmartDashboard.putNumber(prefix + "/Target Angle (deg)", target.angle.getDegrees() % 360); + + if (Settings.DEBUG_MODE.get()) { + SmartDashboard.putNumber(prefix + "/Stator Current", + getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber(prefix + "/Supply Current", + getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble()); + } + } + Robot.getEnergyUtil().logEnergyUsage(getName() + " Drive", getTotalDriveSupplyCurrent()); + Robot.getEnergyUtil().logEnergyUsage(getName() + " Turn", getTotalSteerSupplyCurrent()); + + // CAN SIGNAL LOGGING + if (Settings.DEBUG_MODE.get() && Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { + SmartDashboard.putBoolean( + "Robot/CAN/Canivore/Front Left Drive Motor Connected? (ID " + + String.valueOf(TunerConstants.kFrontLeftDriveMotorId) + ")", + getModule(0).getDriveMotor().isConnected()); + SmartDashboard.putBoolean( + "Robot/CAN/Canivore/Front Left Steer Motor Connected? (ID " + + String.valueOf(TunerConstants.kFrontLeftSteerMotorId) + ")", + getModule(0).getSteerMotor().isConnected()); + SmartDashboard.putBoolean( + "Robot/CAN/Canivore/Front Left CANcoder Connected? (ID " + + String.valueOf(TunerConstants.kFrontLeftEncoderId) + ")", + getModule(0).getEncoder().isConnected()); + + SmartDashboard.putBoolean( + "Robot/CAN/Canivore/Front Right Drive Motor Connected? (ID " + + String.valueOf(TunerConstants.kFrontRightDriveMotorId) + ")", + getModule(1).getDriveMotor().isConnected()); + SmartDashboard.putBoolean( + "Robot/CAN/Canivore/Front Right Steer Motor Connected? (ID " + + String.valueOf(TunerConstants.kFrontRightSteerMotorId) + ")", + getModule(1).getSteerMotor().isConnected()); + SmartDashboard.putBoolean( + "Robot/CAN/Canivore/Front Right CANcoder Connected? (ID " + + String.valueOf(TunerConstants.kFrontRightEncoderId) + ")", + getModule(1).getEncoder().isConnected()); + + SmartDashboard.putBoolean( + "Robot/CAN/Canivore/Back Left Drive Motor Connected? (ID " + + String.valueOf(TunerConstants.kBackLeftDriveMotorId) + ")", + getModule(2).getDriveMotor().isConnected()); + SmartDashboard.putBoolean( + "Robot/CAN/Canivore/Back Left Steer Motor Connected? (ID " + + String.valueOf(TunerConstants.kBackLeftSteerMotorId) + ")", + getModule(2).getSteerMotor().isConnected()); + SmartDashboard.putBoolean( + "Robot/CAN/Canivore/Back Left CANcoder Connected? (ID " + + String.valueOf(TunerConstants.kBackLeftEncoderId) + ")", + getModule(2).getEncoder().isConnected()); + + SmartDashboard.putBoolean( + "Robot/CAN/Canivore/Back Right Drive Motor Connected? (ID " + + String.valueOf(TunerConstants.kBackRightDriveMotorId) + ")", + getModule(3).getDriveMotor().isConnected()); + SmartDashboard.putBoolean( + "Robot/CAN/Canivore/Back Right Steer Motor Connected? (ID " + + String.valueOf(TunerConstants.kBackRightSteerMotorId) + ")", + getModule(3).getSteerMotor().isConnected()); + SmartDashboard.putBoolean( + "Robot/CAN/Canivore/Back Right CANcoder Connected? (ID " + + String.valueOf(TunerConstants.kBackRightEncoderId) + ")", + getModule(3).getEncoder().isConnected()); + + } + } + } +} diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java index 70658778..e5b7c81d 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.util.superstructure; +import java.util.Optional; + import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Superstructure.AngleInterpolation; @@ -29,6 +31,42 @@ public class InterpolationCalculator { public static InterpolatingDoubleTreeMap ferryingDistanceRPMInterpolator; public static InterpolatingDoubleTreeMap ferryingDistanceTOFInterpolator; + private static Optional cachedInterpolatedShotInfo = Optional.empty(); + private static Optional cachedInterpolatedFerryInfo = Optional.empty(); + + public static void clearMemoized() { + cachedInterpolatedShotInfo = Optional.empty(); + cachedInterpolatedFerryInfo = Optional.empty(); + } + + public static double getInterpolatedShotRPM() { + if (cachedInterpolatedShotInfo.isEmpty()) { + cachedInterpolatedShotInfo = Optional.of(interpolateShotInfo()); + } + return cachedInterpolatedShotInfo.get().targetRPM(); + } + + public static Rotation2d getInterpolatedShotAngle() { + if (cachedInterpolatedShotInfo.isEmpty()) { + cachedInterpolatedShotInfo = Optional.of(interpolateShotInfo()); + } + return cachedInterpolatedShotInfo.get().targetHoodAngle(); + } + + public static double getInterpolatedFerryRPM() { + if (cachedInterpolatedFerryInfo.isEmpty()) { + cachedInterpolatedFerryInfo = Optional.of(interpolateFerryingInfo()); + } + return cachedInterpolatedFerryInfo.get().targetRPM(); + } + + public static Rotation2d getInterpolatedFerryAngle() { + if (cachedInterpolatedFerryInfo.isEmpty()) { + cachedInterpolatedFerryInfo = Optional.of(interpolateFerryingInfo()); + } + return cachedInterpolatedFerryInfo.get().targetHoodAngle(); + } + public record InterpolatedShotInfo( Rotation2d targetHoodAngle, double targetRPM, From 8c6546e5e6c281d5726b0b5a04915a7fd4d96526 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 15 Apr 2026 19:48:31 -0400 Subject: [PATCH 03/33] fix: auto changes, new spindexer gear ratio, tolerance switching, new drivetrain offsets --- .../pathplanner/paths/Depot To Score.path | 8 ++-- .../paths/Left Bump Score To Depot.path | 14 +++--- .../paths/Left Bump To Score (Start).path | 16 +++---- .../pathplanner/paths/Left Bump To Score.path | 47 ++++++++++++------- .../paths/Right Bump Score To Depot.path | 18 +++---- .../paths/Right Bump To Score (Start).path | 16 +++---- .../paths/Right Bump To Score.path | 39 ++++++++++----- src/main/java/com/stuypulse/robot/Robot.java | 32 ++++++------- .../commands/auton/regular/LeftMiddy.java | 13 +++-- .../commands/auton/regular/RightMiddy.java | 15 +++--- .../stuypulse/robot/constants/Settings.java | 6 ++- .../superstructure/turret/Turret.java | 7 ++- .../subsystems/swerve/TunerConstants.java | 6 +-- 13 files changed, 134 insertions(+), 103 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Depot To Score.path b/src/main/deploy/pathplanner/paths/Depot To Score.path index ac7273ec..20eab246 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Score.path +++ b/src/main/deploy/pathplanner/paths/Depot To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.5795133587786261, - "y": 5.959713740458016 + "x": 0.5585592011412266, + "y": 5.969329529243937 }, "prevControl": null, "nextControl": { - "x": 1.617398935069807, - "y": 5.933846094852308 + "x": 1.5964447774324075, + "y": 5.943461883638229 }, "isLocked": false, "linkedName": "Depot" diff --git a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path index 77d7a066..ba3c9466 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.0114122681883022, - "y": 6.176348074179743 + "x": 1.0114122681883018, + "y": 6.1375320970042795 }, "isLocked": false, "linkedName": "Left Bump Score" }, { "anchor": { - "x": 0.5795133587786261, - "y": 5.959713740458016 + "x": 0.5585592011412266, + "y": 5.969329529243937 }, "prevControl": { - "x": 1.4665553435114504, - "y": 5.959713740458016 + "x": 1.445601185874051, + "y": 5.969329529243937 }, "nextControl": null, "isLocked": false, @@ -52,7 +52,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.75, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, diff --git a/src/main/deploy/pathplanner/paths/Left Bump To Score (Start).path b/src/main/deploy/pathplanner/paths/Left Bump To Score (Start).path index a2ac11b3..b6a8bcfd 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump To Score (Start).path +++ b/src/main/deploy/pathplanner/paths/Left Bump To Score (Start).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.6250213980028523, - "y": 5.122881679389313 + "x": 3.650898716119829, + "y": 5.503537803138374 }, "prevControl": null, "nextControl": { - "x": 3.2144720637035995, - "y": 5.150459287975925 + "x": 3.240349381820576, + "y": 5.531115411724985 }, "isLocked": false, "linkedName": "Left Bump Start" }, { "anchor": { - "x": 3.0169044222539223, - "y": 5.122881679389313 + "x": 2.9263338088445074, + "y": 5.503537803138374 }, "prevControl": { - "x": 3.3369416060580748, - "y": 5.124474131392073 + "x": 3.24637099264866, + "y": 5.505130255141134 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Bump To Score.path b/src/main/deploy/pathplanner/paths/Left Bump To Score.path index 5ff13c1a..20dca98f 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Bump To Score.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 3.0169044222539223, - "y": 5.122881679389313 + "x": 2.9263338088445074, + "y": 5.503537803138374 }, "prevControl": null, "nextControl": { - "x": 3.9614265335235372, - "y": 5.16713266761769 + "x": 3.8708559201141224, + "y": 5.5477887913667505 }, "isLocked": false, "linkedName": "Left Bump Pre" }, { "anchor": { - "x": 5.565820256776034, - "y": 5.122881679389313 + "x": 5.59169757489301, + "y": 5.503537803138374 }, "prevControl": { - "x": 4.8721189135072756, - "y": 5.185215403584219 + "x": 5.365151789894193, + "y": 5.609259169471154 }, "nextControl": { - "x": 7.377232524964336, - "y": 4.960114122681883 + "x": 7.100692772580049, + "y": 4.799340044217763 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 3.9897146932952925 }, "prevControl": { - "x": 6.549158345221112, - "y": 3.562738944365193 + "x": 6.1480599144079875, + "y": 3.52392296718973 }, "nextControl": { - "x": 8.677779314083576, - "y": 4.15803124921656 + "x": 8.683392720506964, + "y": 4.136486598193509 }, "isLocked": false, "linkedName": null @@ -78,7 +78,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.0319829424306861, + "waypointRelativePos": 1.03, "rotationDegrees": 0.0 }, { @@ -97,8 +97,21 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 1.2964213369345192, - "maxWaypointRelativePos": 3.2194463200540184, + "minWaypointRelativePos": 1.588116137744762, + "maxWaypointRelativePos": 3.0357866306549584, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.35651586765697285, + "maxWaypointRelativePos": 0.7454422687373548, "constraints": { "maxVelocity": 1.5, "maxAcceleration": 10.0, diff --git a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path index fd6d6263..45b51f15 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.4749522900763368, - "y": 2.486860687022901 + "x": 2.175891583452211, + "y": 2.514707560627676 }, "prevControl": null, "nextControl": { - "x": 1.8598664122137403, - "y": 4.838358778625954 + "x": 2.2535235378031384, + "y": 6.49981455064194 }, "isLocked": false, "linkedName": "Right Bump Score" }, { "anchor": { - "x": 0.5795133587786261, - "y": 5.959713740458016 + "x": 0.5585592011412266, + "y": 5.969329529243937 }, "prevControl": { - "x": 2.072382310984308, - "y": 6.266918687589158 + "x": 2.473480741797432, + "y": 5.814065620542082 }, "nextControl": null, "isLocked": false, @@ -52,7 +52,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.75, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, diff --git a/src/main/deploy/pathplanner/paths/Right Bump To Score (Start).path b/src/main/deploy/pathplanner/paths/Right Bump To Score (Start).path index 88b5c18a..35964a50 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump To Score (Start).path +++ b/src/main/deploy/pathplanner/paths/Right Bump To Score (Start).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.689714693295292, - "y": 2.9220133587786257 + "x": 3.663837375178317, + "y": 2.514707560627676 }, "prevControl": null, "nextControl": { - "x": 3.297321789223996, - "y": 2.918542121498757 + "x": 3.271444471107021, + "y": 2.511236323347807 }, "isLocked": false, "linkedName": "Right Bump Start" }, { "anchor": { - "x": 3.1592296718972896, - "y": 2.9220133587786257 + "x": 2.9263338088445074, + "y": 2.514707560627676 }, "prevControl": { - "x": 3.517516983361412, - "y": 2.8905025227689913 + "x": 3.2846211203086297, + "y": 2.4831967246180415 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Bump To Score.path b/src/main/deploy/pathplanner/paths/Right Bump To Score.path index ad74de96..a0d18b65 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Bump To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.1592296718972896, - "y": 2.9220133587786257 + "x": 2.9263338088445074, + "y": 2.514707560627676 }, "prevControl": null, "nextControl": { - "x": 5.642827748799426, - "y": 2.838174037089873 + "x": 5.759900142653352, + "y": 2.4241369472182615 }, "isLocked": false, "linkedName": "Right Bump Pre" @@ -20,12 +20,12 @@ "y": 3.692125534950071 }, "prevControl": { - "x": 6.225691868758916, - "y": 2.8899286733238227 + "x": 6.290385164051354, + "y": 2.7864194008559195 }, "nextControl": { - "x": 7.269102545829907, - "y": 4.327516717288298 + "x": 7.1974561009137075, + "y": 4.373793540365035 }, "isLocked": false, "linkedName": null @@ -64,12 +64,12 @@ }, { "anchor": { - "x": 3.4749522900763368, - "y": 2.486860687022901 + "x": 2.175891583452211, + "y": 2.514707560627676 }, "prevControl": { - "x": 4.634236804564908, - "y": 2.566462196861627 + "x": 3.335176097940783, + "y": 2.594309070466402 }, "nextControl": null, "isLocked": false, @@ -97,7 +97,7 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.8210668467251963, + "minWaypointRelativePos": 1.1127616475354434, "maxWaypointRelativePos": 2.9817690749493897, "constraints": { "maxVelocity": 1.5, @@ -107,6 +107,19 @@ "nominalVoltage": 12.7, "unlimited": false } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.12964213369344296, + "maxWaypointRelativePos": 0.38, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } } ], "pointTowardsZones": [], diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 48e360c0..d475ec22 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -102,7 +102,7 @@ public void robotInit() { energyUtil = new EnergyUtil(); - //TODO: UNCOMMENT WHEN TESTING ALL OF THESE CHANGES. + // TODO: UNCOMMENT WHEN TESTING ALL OF THESE CHANGES. // try { // Field watchdogField = IterativeRobotBase.class.getDeclaredField("m_watchdog"); // watchdogField.setAccessible(true); @@ -114,21 +114,21 @@ public void robotInit() { // CommandScheduler.getInstance().setPeriod(Settings.DT); CommandScheduler.getInstance().schedule(new SwerveAutonInit()); - RobotController.setBrownoutVoltage(6.3); - - BiConsumer logCommandFunction - = (Command command, Boolean active) -> { - String name = command.getName(); - SmartDashboard.putBoolean( - "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); - }; - - CommandScheduler.getInstance() - .onCommandInitialize((Command command) -> logCommandFunction.accept(command, true)); - CommandScheduler.getInstance() - .onCommandFinish((Command command) -> logCommandFunction.accept(command, false)); - CommandScheduler.getInstance() - .onCommandInterrupt((Command command) -> logCommandFunction.accept(command, false)); + RobotController.setBrownoutVoltage(5.5); + + // BiConsumer logCommandFunction + // = (Command command, Boolean active) -> { + // String name = command.getName(); + // SmartDashboard.putBoolean( + // "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); + // }; + + // CommandScheduler.getInstance() + // .onCommandInitialize((Command command) -> logCommandFunction.accept(command, true)); + // CommandScheduler.getInstance() + // .onCommandFinish((Command command) -> logCommandFunction.accept(command, false)); + // CommandScheduler.getInstance() + // .onCommandInterrupt((Command command) -> logCommandFunction.accept(command, false)); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java index 78f73c6f..dc8240cb 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java @@ -33,16 +33,15 @@ public LeftMiddy(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new WaitCommand(2.0) + new WaitCommand(0.5).andThen(new IntakeDeploy()).andThen(new WaitCommand(1.0)) ), // NZ Trip 1 - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new ParallelCommandGroup( - new IntakeDeploy(), - new HandoffStop(), - new SpindexerStop() - ) + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new IntakeDeploy(), + new HandoffStop(), + new SpindexerStop() ), new WaitCommand(0.5), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java index 5ec42ed1..e0be98b3 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java @@ -33,16 +33,15 @@ public RightMiddy(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new WaitCommand(2.0) + new WaitCommand(0.5).andThen(new IntakeDeploy()).andThen(new WaitCommand(1.0)) ), // NZ Trip 1 - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new ParallelCommandGroup( - new IntakeDeploy(), - new HandoffStop(), - new SpindexerStop() - ) + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new IntakeDeploy(), + new HandoffStop(), + new SpindexerStop() ), new WaitCommand(0.5), @@ -62,7 +61,7 @@ public RightMiddy(PathPlannerPath... paths) { // Off Depot new ParallelCommandGroup( new HandoffRun().alongWith(new SpindexerRun()), - new WaitCommand(5.0).andThen(new IntakeAutoDigest().repeatedly()) + new WaitCommand(3.0).andThen(new IntakeAutoDigest().repeatedly()) ) ); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 8d72c96b..d2a26d3f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -97,7 +97,7 @@ public interface Spindexer { double STALL_CURRENT_LIMIT = 40.0; // random number as of 3/9 /* CONSTANTS */ - double GEAR_RATIO = 9.6 / 1.0; + double GEAR_RATIO = 11.04 / 1.0; } public interface Superstructure { @@ -241,7 +241,9 @@ public interface Turret { public final Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0)); public final Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); public final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); - public final SmartNumber SOTM_TOLERANCE = new SmartNumber("Superstructure/Turret/SOTM Tolerance", 6);//Rotation2d.fromDegrees(10.0); + public final SmartNumber SOTM_TOLERANCE_THRESHOLD_METERS = new SmartNumber("Superstructure/Turret/SOTM Tolerance Dist Threshold (Meters)", 1.75); + public final SmartNumber SOTM_TOLERANCE_CLOSE = new SmartNumber("Superstructure/Turret/SOTM Tolerance (Close)", 10.0); + public final SmartNumber SOTM_TOLERANCE_FAR = new SmartNumber("Superstructure/Turret/SOTM Tolerance (Far)", 6.0);//Rotation2d.fromDegrees(10.0); public final Rotation2d FOTM_TOLERANCE = Rotation2d.fromDegrees(10.0); public final Rotation2d KB = Rotation2d.fromDegrees(0.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index d1458669..b416ef24 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 @@ -81,9 +81,14 @@ public Rotation2d driverInputToAngle() { public boolean atTolerance() { double error = getAngle().minus(getTargetAngle()).getRotations(); + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); double tolerance = switch (state) { - case SOTM -> Settings.Superstructure.Turret.SOTM_TOLERANCE.get() / 360.0; + case SOTM -> + swerve.getTurretPose().getTranslation().getDistance(Field.HUB_CENTER.getTranslation()) > + Settings.Superstructure.Turret.SOTM_TOLERANCE_THRESHOLD_METERS.get() ? + Settings.Superstructure.Turret.SOTM_TOLERANCE_CLOSE.get() / 360.0 : + Settings.Superstructure.Turret.SOTM_TOLERANCE_FAR.get() / 360.0; case FOTM -> Settings.Superstructure.Turret.FOTM_TOLERANCE.getRotations(); default -> Settings.Superstructure.Turret.TOLERANCE.getRotations(); }; diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java index 5d6c648f..19bb9e0f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java @@ -145,7 +145,7 @@ public class TunerConstants { public static final int kFrontLeftDriveMotorId = 14; public static final int kFrontLeftSteerMotorId = 15; public static final int kFrontLeftEncoderId = 2; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.220703125); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.2275390625); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -167,7 +167,7 @@ public class TunerConstants { public static final int kBackLeftDriveMotorId = 16; public static final int kBackLeftSteerMotorId = 17; public static final int kBackLeftEncoderId = 1; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.488525390625); + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.488037109375); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -178,7 +178,7 @@ public class TunerConstants { public static final int kBackRightDriveMotorId = 11; public static final int kBackRightSteerMotorId = 10; public static final int kBackRightEncoderId = 4; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.450927734375); + private static final Angle kBackRightEncoderOffset = Rotations.of(0.4453125); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; From ae83f17070dcb7c6a2ea5da04af1f64150f8c28a Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Thu, 16 Apr 2026 12:16:48 -0400 Subject: [PATCH 04/33] feat: more ferrying poses --- .../com/stuypulse/robot/constants/Field.java | 36 +++++++++++++++---- 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 0ce7ce96..617b523c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -64,25 +64,49 @@ public static boolean closerToTop() { return CommandSwerveDrivetrain.getInstance().getPose().getY() >= Field.TOWER_FAR_CENTER.getY(); } - public final Pose2d LEFT_FERRY_ZONE = new Pose2d( + public final Pose2d INNER_LEFT_FERRY_ZONE = new Pose2d( Units.inchesToMeters(31.5), - WIDTH - Units.inchesToMeters(34.5) - Units.inchesToMeters(12), + WIDTH - Units.inchesToMeters(34.5) - Units.inchesToMeters(18), new Rotation2d() ); - public final Pose2d RIGHT_FERRY_ZONE = new Pose2d( + public final Pose2d INNER_RIGHT_FERRY_ZONE = new Pose2d( Units.inchesToMeters(20.75), - Units.inchesToMeters(76) + Units.inchesToMeters(12), + Units.inchesToMeters(76) + Units.inchesToMeters(18), new Rotation2d() ); + public final Pose2d OUTER_LEFT_FERRY_ZONE = new Pose2d( + Units.inchesToMeters(31.5), + WIDTH - Units.inchesToMeters(34.5), + new Rotation2d() + ); + + public final Pose2d OUTER_RIGHT_FERRY_ZONE = new Pose2d( + Units.inchesToMeters(20.75), + Units.inchesToMeters(76), + new Rotation2d() + ); + + public final double FERRY_SWITCH_TRIGGER_METERS_FROM_EDGE = Units.inchesToMeters(50); + public static Pose2d getFerryZonePose(Translation2d robot) { double fieldMidY = WIDTH / 2.0; if (robot.getY() > fieldMidY) { - return LEFT_FERRY_ZONE; + if(robot.getY() > WIDTH - FERRY_SWITCH_TRIGGER_METERS_FROM_EDGE) { + return INNER_LEFT_FERRY_ZONE; + } + else { + return OUTER_LEFT_FERRY_ZONE; + } } else { - return RIGHT_FERRY_ZONE; + if(robot.getY() < FERRY_SWITCH_TRIGGER_METERS_FROM_EDGE) { + return INNER_RIGHT_FERRY_ZONE; + } + else { + return OUTER_RIGHT_FERRY_ZONE; + } } } From 090e572e228944d63902a09ce0d972ae82807d8a Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 16 Apr 2026 18:26:54 -0400 Subject: [PATCH 05/33] FEAT: depot auto, dynamic ferry targets FIX: properly disable loop overrun prints --- .../deploy/pathplanner/autos/Depot Only.auto | 19 +++++ .../paths/Center Hub To Depot.path | 84 +++++++++++++++++++ .../pathplanner/paths/Depot To Score.path | 8 +- .../paths/Left Bump Score To Depot.path | 8 +- .../paths/Right Bump Score To Depot.path | 8 +- src/main/java/com/stuypulse/robot/Robot.java | 21 +++-- .../com/stuypulse/robot/RobotContainer.java | 6 +- .../robot/commands/auton/regular/Depot.java | 48 +++++++++++ .../com/stuypulse/robot/constants/Field.java | 6 +- .../stuypulse/robot/constants/Settings.java | 8 ++ 10 files changed, 191 insertions(+), 25 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Depot Only.auto create mode 100644 src/main/deploy/pathplanner/paths/Center Hub To Depot.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/Depot.java diff --git a/src/main/deploy/pathplanner/autos/Depot Only.auto b/src/main/deploy/pathplanner/autos/Depot Only.auto new file mode 100644 index 00000000..175aaa05 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Depot Only.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Hub To Depot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Hub To Depot.path b/src/main/deploy/pathplanner/paths/Center Hub To Depot.path new file mode 100644 index 00000000..2acfdcd0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Hub To Depot.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6767760342368048, + "y": 4.015592011412268 + }, + "prevControl": null, + "nextControl": { + "x": 1.8783024251069897, + "y": 4.106162624821684 + }, + "isLocked": false, + "linkedName": "Center Hub" + }, + { + "anchor": { + "x": 1.9947503566333804, + "y": 5.374151212553495 + }, + "prevControl": { + "x": 1.9947503566333804, + "y": 4.339058487874466 + }, + "nextControl": { + "x": 1.9947503566333804, + "y": 5.859787410660789 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.4421112696148356, + "y": 5.956390870185448 + }, + "prevControl": { + "x": 1.5548359486447927, + "y": 6.085777460770328 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.7771775827143685, + "constraints": { + "maxVelocity": 0.75, + "maxAcceleration": 1.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.75, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To Depot", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot To Score.path b/src/main/deploy/pathplanner/paths/Depot To Score.path index 20eab246..cdfac99d 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Score.path +++ b/src/main/deploy/pathplanner/paths/Depot To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.5585592011412266, - "y": 5.969329529243937 + "x": 0.4421112696148356, + "y": 5.956390870185448 }, "prevControl": null, "nextControl": { - "x": 1.5964447774324075, - "y": 5.943461883638229 + "x": 1.4799968459060167, + "y": 5.930523224579741 }, "isLocked": false, "linkedName": "Depot" diff --git a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path index ba3c9466..e4edaad7 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.5585592011412266, - "y": 5.969329529243937 + "x": 0.4421112696148356, + "y": 5.956390870185448 }, "prevControl": { - "x": 1.445601185874051, - "y": 5.969329529243937 + "x": 1.32915325434766, + "y": 5.956390870185448 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path index 45b51f15..3d29cedf 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.5585592011412266, - "y": 5.969329529243937 + "x": 0.4421112696148356, + "y": 5.956390870185448 }, "prevControl": { - "x": 2.473480741797432, - "y": 5.814065620542082 + "x": 2.3570328102710407, + "y": 5.801126961483593 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index d475ec22..e651431c 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -7,6 +7,7 @@ import java.lang.management.GarbageCollectorMXBean; import java.lang.management.ManagementFactory; +import java.lang.reflect.Field; import java.util.List; import java.util.function.BiConsumer; @@ -35,8 +36,10 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.IterativeRobotBase; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Watchdog; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -103,15 +106,15 @@ public void robotInit() { // TODO: UNCOMMENT WHEN TESTING ALL OF THESE CHANGES. - // try { - // Field watchdogField = IterativeRobotBase.class.getDeclaredField("m_watchdog"); - // watchdogField.setAccessible(true); - // Watchdog watchdog = (Watchdog) watchdogField.get(this); - // watchdog.setTimeout(Settings.DT); - // } catch (Exception e) { - // DriverStation.reportWarning("Failed to disable loop overrun warnings.", false); - // } - // CommandScheduler.getInstance().setPeriod(Settings.DT); + try { + Field watchdogField = IterativeRobotBase.class.getDeclaredField("m_watchdog"); + watchdogField.setAccessible(true); + Watchdog watchdog = (Watchdog) watchdogField.get(this); + watchdog.setTimeout(Settings.WATCHDOG_TIMEOUT); + } catch (Exception e) { + DriverStation.reportWarning("Failed to disable loop overrun warnings.", false); + } + CommandScheduler.getInstance().setPeriod(Settings.WATCHDOG_TIMEOUT); CommandScheduler.getInstance().schedule(new SwerveAutonInit()); RobotController.setBrownoutVoltage(5.5); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 86b6905c..2265b23e 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.Depot; import com.stuypulse.robot.commands.auton.regular.LeftMiddy; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; import com.stuypulse.robot.commands.auton.regular.RightMiddy; @@ -231,7 +232,7 @@ private void configureButtonBindings() { () -> superstructure.getState() == SuperstructureState.SOTM && swerve.canShootIntoHub()) ); - // FOTM + // FOTM (BL) driver.getLeftMenuButton() .onTrue(new LEDApplyPattern(Settings.LED.FOTM_ON)) .onTrue(new IntakeRunRollers()) @@ -361,6 +362,9 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); // DEPOT + AutonConfig DEPOT_ONLY = new AutonConfig("Depot Only", Depot::new, + "Center Hub To Depot"); + DEPOT_ONLY.register(autonChooser); AutonConfig LEFT_MIDDY = new AutonConfig("Left Middy", LeftMiddy::new, "Left Bump To Score (Start)", "Left Bump To Score", "Left Bump Score To Depot"); LEFT_MIDDY.register(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/Depot.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/Depot.java new file mode 100644 index 00000000..601d6e34 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/Depot.java @@ -0,0 +1,48 @@ +package com.stuypulse.robot.commands.auton.regular; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.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.handoff.Handoff; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class Depot extends SequentialCommandGroup { + + public Depot(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new HandoffRun().alongWith(new SpindexerRun()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new WaitCommand(4.5).andThen(new HandoffStop().alongWith(new SpindexerStop())), + new WaitCommand(0.5).andThen(new IntakeDeploy()) + ), + + new WaitCommand(0.5), + + new HandoffRun().alongWith(new SpindexerRun()) + + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 617b523c..d89a0b9d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -66,13 +66,13 @@ public static boolean closerToTop() { public final Pose2d INNER_LEFT_FERRY_ZONE = new Pose2d( Units.inchesToMeters(31.5), - WIDTH - Units.inchesToMeters(34.5) - Units.inchesToMeters(18), + WIDTH - Units.inchesToMeters(34.5) - Units.inchesToMeters(36), new Rotation2d() ); public final Pose2d INNER_RIGHT_FERRY_ZONE = new Pose2d( Units.inchesToMeters(20.75), - Units.inchesToMeters(76) + Units.inchesToMeters(18), + Units.inchesToMeters(76) + Units.inchesToMeters(36), new Rotation2d() ); @@ -88,7 +88,7 @@ public static boolean closerToTop() { new Rotation2d() ); - public final double FERRY_SWITCH_TRIGGER_METERS_FROM_EDGE = Units.inchesToMeters(50); + public final double FERRY_SWITCH_TRIGGER_METERS_FROM_EDGE = Units.inchesToMeters(75); public static Pose2d getFerryZonePose(Translation2d robot) { double fieldMidY = WIDTH / 2.0; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index d2a26d3f..339b1f86 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -32,6 +32,7 @@ public interface Settings { public final double DT = 0.020; + public final double WATCHDOG_TIMEOUT = 0.2; public final int LOGGING_FREQUENCY = 5; public final double SECONDS_IN_A_MINUTE = 60.0; public final SmartBoolean DEBUG_MODE = new SmartBoolean("Robot/DebugMode", true); @@ -51,6 +52,8 @@ public interface Handoff { double RPM_SOTM_TOLERANCE = 700.0; SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/Target RPM", HANDOFF_MAX); + double IS_EMPTY_AMPERAGE = 8; //TODO: update IS EMPTY VALUE + double FORWARD_DUTY_CYCLE = 1.0; double REVERSE_DUTY_CYCLE = -1.0; @@ -96,6 +99,8 @@ public interface Spindexer { double TOLERANCE_TO_START_INTAKE_ROLLERS_DURING_SCORING_ROUTINE = 1500.0; double STALL_CURRENT_LIMIT = 40.0; // random number as of 3/9 + double IS_EMPTY_AMPERAGE = 10; //TODO: update IS EMPTY VALUE + /* CONSTANTS */ double GEAR_RATIO = 11.04 / 1.0; } @@ -107,6 +112,9 @@ public interface Superstructure { public final double SHOOTER_SOTM_TOLERANCE_RPM_LOW = 100.0; public final double SHOOTER_FOTM_TOLERANCE_RPM_HIGH = 150.0; public final double SHOOTER_FOTM_TOLERANCE_RPM_LOW = 250.0; + + public final double IS_EMPTY_RPM_TOLERANCE = 150; //TODO: update IS EMPTY VALUE + public final double IS_EMPTY_DEBOUNCE_TIME = 0.4; //TODO: update IS EMPTY VALUE public final Rotation2d HOOD_TOLERANCE = Rotation2d.fromDegrees(0.5); public final Rotation2d HOOD_SOTM_TOLERANCE = Rotation2d.fromDegrees(2); From e6f673c14b39ced57be7576b6e43a74eb316201e Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 17 Apr 2026 11:20:53 -0400 Subject: [PATCH 06/33] feat: move inner ferry points in, widen triangle restriction zone base --- src/main/java/com/stuypulse/robot/constants/Field.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index d89a0b9d..5a1c76b9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -44,7 +44,7 @@ public interface Field { public static final double OPPONENT_ZONE_X = LENGTH - Units.inchesToMeters(158.6); 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); // To extend base of triangle (colinear with back hub) + public static final double BEHIND_HUB_TOLERANCE_Y = Units.inchesToMeters(12) + Units.inchesToMeters(2); // To extend base of triangle (colinear with back hub) public static final Pose2d BEHIND_HUB_TRIANGLE_VERTEX = new Pose2d(Units.inchesToMeters(182.11) + Field.BEHIND_HUB_TOLERANCE_X, WIDTH / 2.0, new Rotation2d()); @@ -66,13 +66,13 @@ public static boolean closerToTop() { public final Pose2d INNER_LEFT_FERRY_ZONE = new Pose2d( Units.inchesToMeters(31.5), - WIDTH - Units.inchesToMeters(34.5) - Units.inchesToMeters(36), + WIDTH - Units.inchesToMeters(34.5) - Units.inchesToMeters(48), new Rotation2d() ); public final Pose2d INNER_RIGHT_FERRY_ZONE = new Pose2d( Units.inchesToMeters(20.75), - Units.inchesToMeters(76) + Units.inchesToMeters(36), + Units.inchesToMeters(76) + Units.inchesToMeters(48), new Rotation2d() ); From 945fad39e14dd53ba6c5851ae1780a9fae85472a Mon Sep 17 00:00:00 2001 From: Brian Zheng Date: Sat, 18 Apr 2026 09:59:08 -0400 Subject: [PATCH 07/33] switched to doglog --- src/main/java/com/stuypulse/robot/Robot.java | 23 +++--- .../com/stuypulse/robot/RobotContainer.java | 3 +- .../commands/leds/LEDDefaultCommand.java | 4 +- .../swerve/SwerveDriveAlignTurretToHub.java | 18 ++--- .../commands/swerve/SwerveDriveFOTM.java | 21 +++-- .../commands/swerve/SwerveDriveSOTM.java | 21 +++-- .../SwerveWheelRadiusCharacterization.java | 10 +-- .../pidToPose/SwerveDrivePIDToPose.java | 41 +++++----- .../stuypulse/robot/constants/Cameras.java | 10 +-- .../robot/subsystems/handoff/Handoff.java | 10 +-- .../robot/subsystems/handoff/HandoffImpl.java | 24 +++--- .../robot/subsystems/handoff/HandoffSim.java | 18 ++--- .../robot/subsystems/intake/Intake.java | 18 ++--- .../robot/subsystems/intake/IntakeImpl.java | 40 +++++----- .../robot/subsystems/intake/IntakeSim.java | 14 ++-- .../robot/subsystems/spindexer/Spindexer.java | 10 +-- .../subsystems/spindexer/SpindexerImpl.java | 14 ++-- .../subsystems/spindexer/SpindexerSim.java | 14 ++-- .../superstructure/Superstructure.java | 28 +++---- .../subsystems/superstructure/hood/Hood.java | 12 ++- .../superstructure/hood/HoodImpl.java | 26 +++--- .../superstructure/hood/HoodSim.java | 8 +- .../superstructure/shooter/Shooter.java | 8 +- .../superstructure/shooter/ShooterImpl.java | 28 +++---- .../superstructure/shooter/ShooterSim.java | 12 +-- .../superstructure/turret/Turret.java | 15 ++-- .../superstructure/turret/TurretImpl.java | 24 +++--- .../superstructure/turret/TurretSim.java | 22 +++--- .../swerve/CommandSwerveDrivetrain.java | 79 +++++++++---------- .../subsystems/vision/LimelightVision.java | 34 ++++---- .../com/stuypulse/robot/util/EnergyUtil.java | 17 ++-- .../com/stuypulse/robot/util/FMSUtil.java | 4 +- .../InterpolationCalculator.java | 7 +- vendordeps/DogLog.json | 20 +++++ 34 files changed, 333 insertions(+), 324 deletions(-) create mode 100644 vendordeps/DogLog.json diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index e651431c..9334050f 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -33,6 +33,8 @@ import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.robot.util.superstructure.SOTMCalculator; +import dev.doglog.DogLog; +import dev.doglog.DogLogOptions; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -91,6 +93,7 @@ public static int getPeriodicCounter() { */ @Override public void robotInit() { + DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); robot = new RobotContainer(); mode = RobotMode.DISABLED; energyUtil = new EnergyUtil(); @@ -160,10 +163,10 @@ public void robotPeriodic() { SmartDashboard.putData(CommandScheduler.getInstance()); } - SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime()); + DogLog.log("Robot/Match Time", DriverStation.getMatchTime()); SmartDashboard.putData("Robot/Scheduled Commands", CommandScheduler.getInstance()); - SmartDashboard.putNumber("Robot/Battery Voltage", batteryVoltage); - SmartDashboard.putNumber("Robot/CPU Temperature (C)", RobotController.getCPUTemp()); + DogLog.log("Robot/Battery Voltage", batteryVoltage); + DogLog.log("Robot/CPU Temperature (C)", RobotController.getCPUTemp()); robot.periodicAfterScheduler(); @@ -263,9 +266,9 @@ public void teleopInit() { @Override public void teleopPeriodic() { - SmartDashboard.putNumber("FMSUtil/time left in shift", fmsUtil.getTimeLeftInShift()); - SmartDashboard.putBoolean("FMSUtil/is active shift", fmsUtil.isActiveShift()); - SmartDashboard.putBoolean("FMSUtil/won auto?", fmsUtil.didWinAuto()); + DogLog.log("FMSUtil/time left in shift", fmsUtil.getTimeLeftInShift()); + DogLog.log("FMSUtil/is active shift", fmsUtil.isActiveShift()); + DogLog.log("FMSUtil/won auto?", fmsUtil.didWinAuto()); if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && Superstructure.getInstance().getState() == SuperstructureState.SOTM) { CommandScheduler.getInstance().schedule( @@ -327,10 +330,10 @@ public void update() { totalTime += (int) accumTime; totalCount += (int) accumCounts; - SmartDashboard.putNumber("Robot/GC Time MS", (double) accumTime); - SmartDashboard.putNumber("Robot/GC Counts", (double) accumCounts); - SmartDashboard.putNumber("Robot/Sum of GC Time MS", totalTime); - SmartDashboard.putNumber("Robot/Sum of GC Counts", totalCount); + DogLog.log("Robot/GC Time MS", (double) accumTime); + DogLog.log("Robot/GC Counts", (double) accumCounts); + DogLog.log("Robot/Sum of GC Time MS", totalTime); + DogLog.log("Robot/Sum of GC Counts", totalCount); // Logger.recordOutput("LoggedRobot/GCTimeMS", (double) accumTime); // Logger.recordOutput("LoggedRobot/GCCounts", (double) accumCounts); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 2265b23e..c968d01a 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -81,6 +81,7 @@ import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import com.stuypulse.stuylib.network.SmartBoolean; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -457,7 +458,7 @@ public void periodicAfterScheduler() { superstructure.getCurrentDraw() + swerve.getTotalDriveSupplyCurrent() + swerve.getTotalSteerSupplyCurrent(); - SmartDashboard.putNumber("Robot/Total Current Draw", totalCurrentDraw); + DogLog.log("Robot/Total Current Draw", totalCurrentDraw); handoff.periodicAfterScheduler(); intake.periodicAfterScheduler(); 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 9453134f..08192002 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java @@ -26,8 +26,8 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.vision.LimelightVision; +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.Command; @@ -121,7 +121,7 @@ else if (intake.getPivotState() == PivotState.DEPLOY) { } } - SmartDashboard.putString("Leds/State", state); + DogLog.log("Leds/State", state); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java index 62213abb..cfdba21b 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java @@ -5,26 +5,24 @@ /***************************************************************/ package com.stuypulse.robot.commands.swerve; -import com.stuypulse.stuylib.control.angle.AngleController; -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; - +import com.ctre.phoenix6.swerve.SwerveRequest; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains.Swerve.Alignment; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import com.ctre.phoenix6.swerve.SwerveRequest; - public class SwerveDriveAlignTurretToHub extends Command { private final CommandSwerveDrivetrain swerve; @@ -64,8 +62,8 @@ public void execute() { Angle.fromRotation2d(getTargetAngle()), Angle.fromRotation2d(robot.getRotation())))); - SmartDashboard.putNumber("Swerve/Angle Error", angleController.getError().toDegrees()); - SmartDashboard.putNumber("Swerve/Target Angle Hub Deg", TurretAngleCalculator.getPointAtTargetAngle(Field.getHubPose().getTranslation(), swerve.getTurretPose().getTranslation(), robot.getRotation()).getDegrees()); + DogLog.log("Swerve/Angle Error", angleController.getError().toDegrees()); + DogLog.log("Swerve/Target Angle Hub Deg", TurretAngleCalculator.getPointAtTargetAngle(Field.getHubPose().getTranslation(), swerve.getTurretPose().getTranslation(), robot.getRotation()).getDegrees()); } @Override 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 792d9711..e8ea6c32 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,13 @@ /***************************************************************/ package com.stuypulse.robot.commands.swerve; +import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; +import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; +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.SLMath; import com.stuypulse.stuylib.math.Vector2D; @@ -15,15 +22,7 @@ import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; -import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; -import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Swerve; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveFOTM extends Command{ @@ -76,8 +75,8 @@ public void execute() { .withVelocityY(speed.get().y) .withRotationalRate(-turn.get())); - SmartDashboard.putNumber("Swerve/Speed x", speed.get().x); - SmartDashboard.putNumber("Swerve/Speed y", speed.get().y); + DogLog.log("Swerve/Speed x", speed.get().x); + DogLog.log("Swerve/Speed y", speed.get().y); } @Override 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 3d0e744a..c9b75562 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -5,6 +5,14 @@ /***************************************************************/ 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; +import com.stuypulse.robot.constants.Settings.Swerve; +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.SLMath; import com.stuypulse.stuylib.math.Vector2D; @@ -17,16 +25,7 @@ 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; -import com.stuypulse.robot.constants.Settings.Swerve; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveSOTM extends Command { @@ -89,7 +88,7 @@ public void execute() { .withRotationalRate(-turn.get())); } - SmartDashboard.putBoolean("Swerve/SOTM/Idle?", isIdle.get()); + DogLog.log("Swerve/SOTM/Idle?", isIdle.get()); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java index 4f480ae0..f39f6490 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java @@ -7,10 +7,10 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; 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.Command; public class SwerveWheelRadiusCharacterization extends Command { @@ -91,10 +91,10 @@ public void execute() { double wheelRadius = (gyroDeltaRad * DRIVE_RADIUS_METERS) / wheelDeltaRad; - SmartDashboard.putNumber("Radius Characterization/Radius (m)", wheelRadius); - SmartDashboard.putNumber("Radius Characterization/Radius (in.)", Units.metersToInches(wheelRadius)); - SmartDashboard.putNumber("Radius Characterization/Gyro Delta (rad)", gyroDeltaRad); - SmartDashboard.putNumber("Radius Characterization/Wheel Delta (rad)", wheelDeltaRad); + DogLog.log("Radius Characterization/Radius (m)", wheelRadius); + DogLog.log("Radius Characterization/Radius (in.)", Units.metersToInches(wheelRadius)); + DogLog.log("Radius Characterization/Gyro Delta (rad)", gyroDeltaRad); + DogLog.log("Radius Characterization/Wheel Delta (rad)", wheelDeltaRad); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java index f2936cd6..b8640092 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java @@ -5,6 +5,14 @@ /***************************************************************/ package com.stuypulse.robot.commands.swerve.pidToPose; +import java.util.function.Supplier; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Gains.Swerve.Alignment; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.HolonomicController; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.math.Vector2D; @@ -17,22 +25,13 @@ import com.stuypulse.stuylib.streams.vectors.VStream; import com.stuypulse.stuylib.streams.vectors.filters.VFilter; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Gains.Swerve.Alignment; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.HolonomicController; - +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import java.util.function.Supplier; - public class SwerveDrivePIDToPose extends Command { private final CommandSwerveDrivetrain swerve; @@ -160,20 +159,20 @@ public void execute() { .withVelocityY(controller.getOutput().vyMetersPerSecond) .withRotationalRate(controller.getOutput().omegaRadiansPerSecond)); - SmartDashboard.putNumber("Alignment/Target x", targetPose.get().getX()); - SmartDashboard.putNumber("Alignment/Target y", targetPose.get().getY()); - SmartDashboard.putNumber("Alignment/Target angle", targetPose.get().getRotation().getDegrees()); + DogLog.log("Alignment/Target x", targetPose.get().getX()); + DogLog.log("Alignment/Target y", targetPose.get().getY()); + DogLog.log("Alignment/Target angle", targetPose.get().getRotation().getDegrees()); - SmartDashboard.putNumber("Alignment/Error of Angle Controller)", controller.getError().omegaRadiansPerSecond); + DogLog.log("Alignment/Error of Angle Controller)", controller.getError().omegaRadiansPerSecond); - SmartDashboard.putNumber("Alignment/Target Velocity Robot Relative X (m per s)", controller.getOutput().vxMetersPerSecond); - SmartDashboard.putNumber("Alignment/Target Velocity Robot Relative Y (m per s)", controller.getOutput().vyMetersPerSecond); - SmartDashboard.putNumber("Alignment/Target Angular Velocity (rad per s)", controller.getOutput().omegaRadiansPerSecond); + DogLog.log("Alignment/Target Velocity Robot Relative X (m per s)", controller.getOutput().vxMetersPerSecond); + DogLog.log("Alignment/Target Velocity Robot Relative Y (m per s)", controller.getOutput().vyMetersPerSecond); + DogLog.log("Alignment/Target Angular Velocity (rad per s)", controller.getOutput().omegaRadiansPerSecond); - SmartDashboard.putBoolean("Alignment/Is Aligned", isAligned()); - SmartDashboard.putBoolean("Alignment/Is Aligned X", isAlignedX()); - SmartDashboard.putBoolean("Alignment/Is Aligned Y", isAlignedY()); - SmartDashboard.putBoolean("Alignment/Is Aligned Theta", isAlignedTheta()); + DogLog.log("Alignment/Is Aligned", isAligned()); + DogLog.log("Alignment/Is Aligned X", isAlignedX()); + DogLog.log("Alignment/Is Aligned Y", isAlignedY()); + DogLog.log("Alignment/Is Aligned Theta", isAlignedTheta()); } @Override diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 5176c811..0bcc63ce 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -9,10 +9,10 @@ import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.stuylib.network.SmartBoolean; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** This interface stores information about each camera. */ public interface Cameras { @@ -104,10 +104,10 @@ public void incrementRejection(RejectionValue rejectionValue) { } public void logRejections() { - SmartDashboard.putNumber("Vision/" + name + "/# Rejected Not Null", rejectedCounterNotNull); - SmartDashboard.putNumber("Vision/" + name + "/# Rejected Target Area", rejectedCounterTargetArea); - SmartDashboard.putNumber("Vision/" + name + "/# Rejected Angular Velocity", rejectedCounterAngularVelocity); - SmartDashboard.putNumber("Vision/" + name + "/# Rejected Invalid Position", rejectedCounterInvalidPosition); + DogLog.log("Vision/" + name + "/# Rejected Not Null", rejectedCounterNotNull); + DogLog.log("Vision/" + name + "/# Rejected Target Area", rejectedCounterTargetArea); + DogLog.log("Vision/" + name + "/# Rejected Angular Velocity", rejectedCounterAngularVelocity); + DogLog.log("Vision/" + name + "/# Rejected Invalid Position", rejectedCounterInvalidPosition); } public Camera(String name, Pose3d location, SmartBoolean isEnabled) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java index e9f8a71e..accaf1f2 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java @@ -5,15 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.handoff; +import java.util.Optional; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public abstract class Handoff extends SubsystemBase { private static final Handoff instance; private HandoffState state; @@ -76,10 +76,10 @@ public void setState(HandoffState state) { public abstract double getCurrentDraw(); public void periodicAfterScheduler() { - SmartDashboard.putString("Handoff/State", getState().toString()); + DogLog.log("Handoff/State", getState().toString()); // SmartDashboard.putNumber("Handoff/Target RPM", getTargetRPM()); - SmartDashboard.putNumber("Handoff/Current RPM", getLeaderRPM()); + DogLog.log("Handoff/Current RPM", getLeaderRPM()); // SmartDashboard.putBoolean("Handoff/At Tolerance?", atTolerance()); } } 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 399913ad..0a5da6c1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -23,17 +23,15 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import dev.doglog.DogLog; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class HandoffImpl extends Handoff { @@ -134,22 +132,22 @@ public void periodicAfterScheduler() { motorFollow.stopMotor(); } - SmartDashboard.putNumber("Handoff/Lead Velocity", getLeaderRPM()); - SmartDashboard.putNumber("Handoff/Follow Velocity", getLeaderRPM()); + DogLog.log("Handoff/Lead Velocity", getLeaderRPM()); + DogLog.log("Handoff/Follow Velocity", getLeaderRPM()); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber("Handoff/Lead Voltage", motorLeadVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Handoff/Lead Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Handoff/Lead Stator Current", motorLeadStatorCurrent.getValueAsDouble()); + DogLog.log("Handoff/Lead Voltage", motorLeadVoltage.getValueAsDouble()); + DogLog.log("Handoff/Lead Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); + DogLog.log("Handoff/Lead Stator Current", motorLeadStatorCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Handoff/Follow Voltage", motorLeadVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Handoff/Follow Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Handoff/Follow Stator Current", motorLeadStatorCurrent.getValueAsDouble()); + DogLog.log("Handoff/Follow Voltage", motorLeadVoltage.getValueAsDouble()); + DogLog.log("Handoff/Follow Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); + DogLog.log("Handoff/Follow Stator Current", motorLeadStatorCurrent.getValueAsDouble()); if(Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean("Robot/CAN/Main/Handoff Lead Motor Connected? (ID " + String.valueOf(Ports.Handoff.MOTOR_LEAD) + ")", motorLead.isConnected()); - SmartDashboard.putBoolean("Robot/CAN/Main/Handoff Follow Motor Connected? (ID " + String.valueOf(Ports.Handoff.MOTOR_FOLLOW) + ")", motorFollow.isConnected()); + DogLog.log("Robot/CAN/Main/Handoff Lead Motor Connected? (ID " + String.valueOf(Ports.Handoff.MOTOR_LEAD) + ")", motorLead.isConnected()); + DogLog.log("Robot/CAN/Main/Handoff Follow Motor Connected? (ID " + String.valueOf(Ports.Handoff.MOTOR_FOLLOW) + ")", motorFollow.isConnected()); } Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java index 8150d7ed..b85888cb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java @@ -5,13 +5,14 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.handoff; +import java.util.Optional; + import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.SysId; +import dev.doglog.DogLog; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.LinearQuadraticRegulator; @@ -22,11 +23,8 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public class HandoffSim extends Handoff { private LinearSystemSim sim; @@ -76,22 +74,22 @@ public void periodic() { if (EnabledSubsystems.HANDOFF.get()) { if (voltageOverride.isPresent()) { sim.setInput(voltageOverride.get()); - SmartDashboard.putNumber("Handoff/Input Voltage", voltageOverride.get()); + DogLog.log("Handoff/Input Voltage", voltageOverride.get()); } else if (Superstructure.getInstance().shouldStop()) { sim.setInput(0.0); - SmartDashboard.putNumber("Handoff/Input Voltage", 0.0); + DogLog.log("Handoff/Input Voltage", 0.0); } else { - SmartDashboard.putNumber("Handoff/Input Voltage", controller.getU(0)); + DogLog.log("Handoff/Input Voltage", controller.getU(0)); sim.setInput(getTargetDutyCycle() * 12); } } else { sim.setInput(0); - SmartDashboard.putNumber("Handoff/Input Voltage", 0.0); + DogLog.log("Handoff/Input Voltage", 0.0); } sim.update(Settings.DT); - SmartDashboard.putNumber("Handoff/Target Duty Cycle", getTargetDutyCycle()); + DogLog.log("Handoff/Target Duty Cycle", getTargetDutyCycle()); } @Override 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 65ed6419..cb0c1496 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -5,16 +5,16 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.intake; +import java.util.Optional; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public abstract class Intake extends SubsystemBase { private static final Intake instance; @@ -100,15 +100,15 @@ public void setRollerState(RollerState state) { public abstract double getCurrentDraw(); public void periodicAfterScheduler() { - SmartDashboard.putString("Intake/Pivot State", getPivotState().toString()); - SmartDashboard.putString("Intake/Roller State", getRollerState().toString()); + DogLog.log("Intake/Pivot State", getPivotState().toString()); + DogLog.log("Intake/Roller State", getRollerState().toString()); - SmartDashboard.putNumber("Intake/Current Angle (deg)", getPivotAngle().getDegrees()); - SmartDashboard.putNumber("Intake/Target Angle (deg)", getPivotState().getTargetAngle().getDegrees()); + DogLog.log("Intake/Current Angle (deg)", getPivotAngle().getDegrees()); + DogLog.log("Intake/Target Angle (deg)", getPivotState().getTargetAngle().getDegrees()); - SmartDashboard.putNumber("Intake/Target Duty Cycle", getRollerState().getTargetDutyCycle()); + DogLog.log("Intake/Target Duty Cycle", getRollerState().getTargetDutyCycle()); - SmartDashboard.putBoolean("Intake/Pivot At Tolerance?", pivotAtTolerance()); + DogLog.log("Intake/Pivot At Tolerance?", pivotAtTolerance()); } } 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 15e217da..54c86b92 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -31,12 +31,12 @@ 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.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class IntakeImpl extends Intake { @@ -224,52 +224,52 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge rollerFollower.stopMotor(); } - SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", + DogLog.log("Intake/Pivot Angle Error (deg)", Math.abs(pivotState.getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { // PIVOT - SmartDashboard.putBoolean("Intake/Pivot Pushdown Voltage Applied?", applyingPushdownCurrent); + DogLog.log("Intake/Pivot Pushdown Voltage Applied?", applyingPushdownCurrent); - SmartDashboard.putNumber("Intake/Pivot Closed Loop Error (deg)", + DogLog.log("Intake/Pivot Closed Loop Error (deg)", pivot.getClosedLoopError().getValueAsDouble() * 360.0); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putBoolean("Intake/Voltage Override", pivotVoltageOverride.isPresent()); - SmartDashboard.putNumber("Intake/Pivot Temperature (C)", pivotTemperature.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Leader Temperature (C)", + DogLog.log("Intake/Voltage Override", pivotVoltageOverride.isPresent()); + DogLog.log("Intake/Pivot Temperature (C)", pivotTemperature.getValueAsDouble()); + DogLog.log("Intake/Leader Temperature (C)", rollerLeaderTemperature.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Follower Temperature (C)", + DogLog.log("Intake/Follower Temperature (C)", rollerFollowerTemperature.getValueAsDouble()); // Rolers - SmartDashboard.putNumber("Intake/Roller Leader Voltage (volts)", + DogLog.log("Intake/Roller Leader Voltage (volts)", rollerLeaderVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Leader Current (amps)", + DogLog.log("Intake/Roller Leader Current (amps)", rollerLeaderSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Leader Stator Current (amps)", + DogLog.log("Intake/Roller Leader Stator Current (amps)", rollerLeaderStatorCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Follower Voltage (volts)", + DogLog.log("Intake/Roller Follower Voltage (volts)", rollerFollowerVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Follower Current (amps)", + DogLog.log("Intake/Roller Follower Current (amps)", rollerFollowerSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Follower Stator Current (amps)", + DogLog.log("Intake/Roller Follower Stator Current (amps)", rollerFollowerStatorCurrent.getValueAsDouble()); // Pivot - SmartDashboard.putNumber("Intake/Pivot Voltage (volts)", pivotMotorVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Pivot Supply Current (amps)", + DogLog.log("Intake/Pivot Voltage (volts)", pivotMotorVoltage.getValueAsDouble()); + DogLog.log("Intake/Pivot Supply Current (amps)", pivotSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Pivot Stator Current (amps)", + DogLog.log("Intake/Pivot Stator Current (amps)", pivotStatorCurrent.getValueAsDouble()); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean("Robot/CAN/Main/Intake Pivot Motor Connected? (ID " + DogLog.log("Robot/CAN/Main/Intake Pivot Motor Connected? (ID " + String.valueOf(Ports.Intake.PIVOT) + ")", pivot.isConnected()); - SmartDashboard.putBoolean("Robot/CAN/Main/Intake Roller Leader Motor Connected? (ID " + DogLog.log("Robot/CAN/Main/Intake Roller Leader Motor Connected? (ID " + String.valueOf(Ports.Intake.ROLLER_LEADER) + ")", rollerLeader.isConnected()); - SmartDashboard.putBoolean("Robot/CAN/Main/Intake Roller Follower Motor Connected? (ID " + 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 b5e46563..ca3dbffb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java @@ -5,10 +5,13 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.intake; +import java.util.Optional; + import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; +import dev.doglog.DogLog; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.LinearQuadraticRegulator; @@ -23,11 +26,8 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public class IntakeSim extends Intake { private static final double ARM_LENGTH_METERS = 0.4; @@ -186,10 +186,10 @@ public void periodic() { rollerFollowerSim.update(Settings.DT); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber("Intake/Sim Pivot Angle (deg)", getPivotAngle().getDegrees()); - SmartDashboard.putNumber("Intake/Sim Pivot Velocity (deg per s)", Units.radiansToDegrees(pivotSim.getVelocityRadPerSec())); - SmartDashboard.putNumber("Intake/Sim Roller Leader Velocity (RPM)", rollerLeaderSim.getOutput(0) * 60.0 / (2.0 * Math.PI)); - SmartDashboard.putNumber("Intake/Sim Roller Follower Velocity (RPM)", rollerFollowerSim.getOutput(0) * 60.0 / (2.0 * Math.PI)); + DogLog.log("Intake/Sim Pivot Angle (deg)", getPivotAngle().getDegrees()); + DogLog.log("Intake/Sim Pivot Velocity (deg per s)", Units.radiansToDegrees(pivotSim.getVelocityRadPerSec())); + DogLog.log("Intake/Sim Roller Leader Velocity (RPM)", rollerLeaderSim.getOutput(0) * 60.0 / (2.0 * Math.PI)); + DogLog.log("Intake/Sim Roller Follower Velocity (RPM)", rollerFollowerSim.getOutput(0) * 60.0 / (2.0 * Math.PI)); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java index de13f04a..a91710e0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java @@ -5,15 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.spindexer; +import java.util.Optional; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public abstract class Spindexer extends SubsystemBase { private static final Spindexer instance; private SpindexerState spindexerState; @@ -65,7 +65,7 @@ public void setState(SpindexerState state) { public abstract double getCurrentDraw(); public void periodicAfterScheduler() { - SmartDashboard.putString("Spindexer/State", getState().name()); - SmartDashboard.putNumber("Spindexer/Target Duty Cycle", getTargetDutyCycle()); + DogLog.log("Spindexer/State", getState().name()); + DogLog.log("Spindexer/Target Duty Cycle", getTargetDutyCycle()); } } \ 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 7002c909..88f93592 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -21,18 +21,16 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import dev.doglog.DogLog; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class SpindexerImpl extends Spindexer { @@ -129,18 +127,18 @@ public void periodicAfterScheduler() { leaderMotor.stopMotor(); } - SmartDashboard.putNumber("Spindexer/Leader Motor RPM", getMotorRPM()); + DogLog.log("Spindexer/Leader Motor RPM", getMotorRPM()); // SmartDashboard.putBoolean("Spindexer/Unjamming", unJamming); - SmartDashboard.putNumber("Spindexer/Leader Voltage (volts)", leaderMotorVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Spindexer/Leader Supply Current (amps)", leaderSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Spindexer/Leader Stator Current (amps)", leaderStatorCurrent.getValueAsDouble()); + DogLog.log("Spindexer/Leader Voltage (volts)", leaderMotorVoltage.getValueAsDouble()); + DogLog.log("Spindexer/Leader Supply Current (amps)", leaderSupplyCurrent.getValueAsDouble()); + DogLog.log("Spindexer/Leader Stator Current (amps)", leaderStatorCurrent.getValueAsDouble()); // SmartDashboard.putBoolean("Spindexer/Should Stop?", shouldStop()); if (Settings.DEBUG_MODE.get()) { if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Spindexer Leader Motor Connected? (ID " + String.valueOf(Ports.Spindexer.MOTOR) + ")", leaderMotor.isConnected()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerSim.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerSim.java index 09bf056b..4fa13b41 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerSim.java @@ -11,10 +11,9 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.SysId; +import dev.doglog.DogLog; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.LinearQuadraticRegulator; @@ -26,7 +25,6 @@ import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class SpindexerSim extends Spindexer { @@ -98,20 +96,20 @@ public void periodic() { if (EnabledSubsystems.SHOOTER.get()) { if (voltageOverride.isPresent()) { sim.setInput(voltageOverride.get()); - SmartDashboard.putNumber("Spindexer/Input Voltage", voltageOverride.get()); + DogLog.log("Spindexer/Input Voltage", voltageOverride.get()); } else if (Superstructure.getInstance().shouldStop() && !isUnjamming) { sim.setInput(0); } else { - SmartDashboard.putNumber("Spindexer/Input Voltage", controller.getU(0)); + DogLog.log("Spindexer/Input Voltage", controller.getU(0)); sim.setInput(controller.getU(0)); } } else { sim.setInput(0); - SmartDashboard.putNumber("Spindexer/Input Voltage", 0.0); + DogLog.log("Spindexer/Input Voltage", 0.0); } - SmartDashboard.putNumber("Spindexer/Current RPM", getCurrentRPM()); - SmartDashboard.putBoolean("Spindexer/Unjamming", isUnjamming); + DogLog.log("Spindexer/Current RPM", getCurrentRPM()); + DogLog.log("Spindexer/Unjamming", isUnjamming); sim.update(Settings.DT); } 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 38b06ab4..48e28056 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -21,10 +21,10 @@ 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.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Superstructure extends SubsystemBase { @@ -181,12 +181,12 @@ public boolean shouldStop() { boolean turretLaggingSOTM = !isTurretAtTolerance() && getState() == SuperstructureState.SOTM; - SmartDashboard.putBoolean("Spindexer/Should Stop/Is Behind Hub While Ferrying?", isBehindHubWhileFerrying); - SmartDashboard.putBoolean("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); - SmartDashboard.putBoolean("Spindexer/Should Stop/Is Outside Alliance Zone?", isOutsideAllianceZone); - SmartDashboard.putBoolean("Spindexer/Should Stop/Is Under Trench?", isUnderTrench); - SmartDashboard.putBoolean("Spindexer/Should Stop/Turret Lagging SOTM", turretLaggingSOTM); - SmartDashboard.putBoolean("Spindexer/Should Stop/In Manual State", inManualState); + DogLog.log("Spindexer/Should Stop/Is Behind Hub While Ferrying?", isBehindHubWhileFerrying); + DogLog.log("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); + DogLog.log("Spindexer/Should Stop/Is Outside Alliance Zone?", isOutsideAllianceZone); + DogLog.log("Spindexer/Should Stop/Is Under Trench?", isUnderTrench); + DogLog.log("Spindexer/Should Stop/Turret Lagging SOTM", turretLaggingSOTM); + DogLog.log("Spindexer/Should Stop/In Manual State", inManualState); return isSpindexerStopState || isHandOffStopState || @@ -223,15 +223,15 @@ else if (getState() == SuperstructureState.FOTM && shouldStop() && DriverStation Handoff.getInstance().setState(HandoffState.STOP); } - SmartDashboard.putString("Superstructure/State", state.name()); + DogLog.log("Superstructure/State", state.name()); - SmartDashboard.putNumber("Superstructure/SOTM Stopped Timer", sotmStoppedTimer.get()); - SmartDashboard.putNumber("Superstructure/FOTM Stopped Timer", fotmStoppedTimer.get()); + DogLog.log("Superstructure/SOTM Stopped Timer", sotmStoppedTimer.get()); + DogLog.log("Superstructure/FOTM Stopped Timer", fotmStoppedTimer.get()); - SmartDashboard.putBoolean("Superstructure/Shooter At Tolerance?", isShooterAtTolerance()); - SmartDashboard.putBoolean("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); - SmartDashboard.putBoolean("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); + DogLog.log("Superstructure/Shooter At Tolerance?", isShooterAtTolerance()); + DogLog.log("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); + DogLog.log("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); - SmartDashboard.putBoolean("Superstructure/Should Stop?", shouldStop()); + DogLog.log("Superstructure/Should Stop?", shouldStop()); } } \ No newline at end of file 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 6d004b24..0555fb55 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 @@ -5,19 +5,17 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.hood; -import com.stuypulse.stuylib.input.Gamepad; - import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.robot.util.superstructure.VisualizerHood; +import com.stuypulse.stuylib.input.Gamepad; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -131,10 +129,10 @@ public Rotation2d hoodAnalogToOutput() { public void periodicAfterScheduler() { - SmartDashboard.putString("Superstructure/Hood/State", state.name()); + DogLog.log("Superstructure/Hood/State", state.name()); - SmartDashboard.putNumber("Superstructure/Hood/Target Angle (deg)", getTargetAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/Hood/Current Angle (deg)", getAngle().getDegrees()); + DogLog.log("Superstructure/Hood/Target Angle (deg)", getTargetAngle().getDegrees()); + DogLog.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/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 0950ddef..21fc5de5 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 @@ -26,11 +26,11 @@ 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.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class HoodImpl extends Hood { @@ -156,13 +156,13 @@ public void periodicAfterScheduler() { if (isStalling() && state == HoodState.HOMING_UPPER) { seedHoodAtUpperHardStop(); setState(HoodState.STOW); - SmartDashboard.putBoolean("Superstructure/Hood/SUCCESFULLY HOMED UPPER", true); + DogLog.log("Superstructure/Hood/SUCCESFULLY HOMED UPPER", true); } if (isStalling() && state == HoodState.HOMING_LOWER) { seedHoodAtLowerHardStop(); setState(HoodState.STOW); - SmartDashboard.putBoolean("Superstructure/Hood/SUCCESFULLY HOMED LOWER", true); + DogLog.log("Superstructure/Hood/SUCCESFULLY HOMED LOWER", true); } if (EnabledSubsystems.HOOD.get()) { @@ -181,21 +181,21 @@ public void periodicAfterScheduler() { // SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); - SmartDashboard.putBoolean("Prematch Checks/Hood at Bottom?", getAngle().getDegrees() < Settings.Superstructure.Hood.REVERSE_SOFT_LIMIT.getDegrees()); - SmartDashboard.putNumber("Superstructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); - SmartDashboard.putNumber("Superstructure/Hood/Closed Loop Error (deg)", hoodMotorClosedLoopError.getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Superstructure/Hood/Implemented Error (Degrees)", getTargetAngle().getDegrees() - getAngle().getDegrees()); + 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()); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber("Superstructure/Hood/Applied Voltage (amps)", hoodMotorVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Hood/Supply Current (amps)", hoodMotorSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Hood/Stator Current (amps)", hoodMotorStatorCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Hood/Raw Motor Encoder Value",hoodMotorStatorCurrent.getValueAsDouble()); - SmartDashboard.putBoolean("Superstructure/Hood/is stalling", isStalling()); + 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/is stalling", isStalling()); Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean("Robot/CAN/Rio/Hood Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Hood.MOTOR) + ")", hoodMotor.isConnected()); + DogLog.log("Robot/CAN/Rio/Hood Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Hood.MOTOR) + ")", hoodMotor.isConnected()); // SmartDashboard.putBoolean("Robot/CAN/Rio/Hood Encoder Connected? (ID " + String.valueOf(hoodEncoder.getDeviceID()) + ")", hoodEncoder.isConnected()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java index dd459edc..f982e6a0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java @@ -5,19 +5,19 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.hood; +import java.util.Optional; + import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; import com.stuypulse.robot.util.superstructure.VisualizerHood; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.ElevatorSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public class HoodSim extends Hood { // Simulates hood motion as a linear elevator, then converts to angle @@ -112,7 +112,7 @@ public void periodic() { VisualizerHood.getInstance().update(getAngle(), atTolerance()); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber("Superstructure/Hood/Sim Height (m)", sim.getPositionMeters()); + DogLog.log("Superstructure/Hood/Sim Height (m)", sim.getPositionMeters()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index 51b7cc64..afd0b423 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -10,7 +10,7 @@ import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.robot.util.superstructure.SOTMCalculator; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -104,9 +104,9 @@ public boolean atTolerance() { public abstract double getCurrentDraw(); public void periodicAfterScheduler() { - SmartDashboard.putString("Superstructure/Shooter/State", state.name()); + DogLog.log("Superstructure/Shooter/State", state.name()); - SmartDashboard.putNumber("Superstructure/Shooter/Current RPM (Leader)", getRPM()); - SmartDashboard.putNumber("Superstructure/Shooter/Target RPM", getTargetRPM()); + DogLog.log("Superstructure/Shooter/Current RPM (Leader)", getRPM()); + DogLog.log("Superstructure/Shooter/Target RPM", getTargetRPM()); } } 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 06cb401f..f52865cc 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 @@ -24,10 +24,10 @@ import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.SysId; +import dev.doglog.DogLog; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class ShooterImpl extends Shooter { @@ -138,33 +138,33 @@ public void periodicAfterScheduler() { shooterLeader.stopMotor(); } - SmartDashboard.putNumber("Superstructure/Shooter/Leader RPM", getLeaderRPM()); - SmartDashboard.putNumber("Superstructure/Shooter/Follower RPM", getFollowerRPM()); + DogLog.log("Superstructure/Shooter/Leader RPM", getLeaderRPM()); + DogLog.log("Superstructure/Shooter/Follower RPM", getFollowerRPM()); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber("InterpolationTesting/Shooter Applied Voltage", + DogLog.log("InterpolationTesting/Shooter Applied Voltage", shooterLeaderVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Leader Voltage (volts)", + DogLog.log("Superstructure/Shooter/Leader Voltage (volts)", shooterLeaderVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Leader Supply Current (amps)", + DogLog.log("Superstructure/Shooter/Leader Supply Current (amps)", shooterLeadSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Leader Stator Current (amps)", + DogLog.log("Superstructure/Shooter/Leader Stator Current (amps)", shooterLeadStatorCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Follower Voltage (volts)", + DogLog.log("Superstructure/Shooter/Follower Voltage (volts)", shooterFollowerVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Follower Supply Current (amps)", + DogLog.log("Superstructure/Shooter/Follower Supply Current (amps)", shooterFollowSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Follower Stator Current (amps)", + DogLog.log("Superstructure/Shooter/Follower Stator Current (amps)", shooterFollowStatorCurrent.getValueAsDouble()); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Main/Shooter Leader Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Shooter.MOTOR_LEAD) + ")", shooterLeader.isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Main/Shooter Follower Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Shooter.MOTOR_FOLLOW) + ")", shooterFollower.isConnected()); @@ -172,10 +172,10 @@ public void periodicAfterScheduler() { Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); } - SmartDashboard.putNumber("InterpolationTesting/Shooter Closed Loop Error (RPM)", + DogLog.log("InterpolationTesting/Shooter Closed Loop Error (RPM)", shooterLeaderClosedLoopError.getValueAsDouble() * 60.0); - SmartDashboard.putNumber("Superstructure/Shooter/Implemented Error (RPM)", getTargetRPM() - getLeaderRPM()); + DogLog.log("Superstructure/Shooter/Implemented Error (RPM)", getTargetRPM() - getLeaderRPM()); } private void setVoltageOverride(Optional voltageOverride) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java index 57742fff..8b9be7c2 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java @@ -5,10 +5,13 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.shooter; +import java.util.Optional; + import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; +import dev.doglog.DogLog; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.LinearQuadraticRegulator; @@ -19,11 +22,8 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public class ShooterSim extends Shooter { private LinearSystemSim sim; @@ -75,14 +75,14 @@ public void periodic() { if (EnabledSubsystems.SHOOTER.get()) { if (voltageOverride.isPresent()) { sim.setInput(voltageOverride.get()); - SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", voltageOverride.get()); + DogLog.log("Superstructure/Shooter/Input Voltage", voltageOverride.get()); } else { - SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", controller.getU(0)); + DogLog.log("Superstructure/Shooter/Input Voltage", controller.getU(0)); sim.setInput(controller.getU(0)); } } else { sim.setInput(0); - SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", 0.0); + DogLog.log("Superstructure/Shooter/Input Voltage", 0.0); } sim.update(Settings.DT); 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 b416ef24..1b549bc6 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 @@ -5,9 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.turret; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.math.Vector2D; - import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Field; @@ -16,11 +13,13 @@ import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; import com.stuypulse.robot.util.superstructure.VisualizerTurret; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.Vector2D; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -75,7 +74,7 @@ public Rotation2d getTargetAngle() { } public Rotation2d driverInputToAngle() { - SmartDashboard.putNumber("Superstructure/Turret/Driver Input", driverInput.x); + DogLog.log("Superstructure/Turret/Driver Input", driverInput.x); return Rotation2d.fromDegrees(driverInput.x * 180); } @@ -134,10 +133,10 @@ public TurretState getState() { } public void periodicAfterScheduler() { - SmartDashboard.putString("Superstructure/Turret/State", state.name()); + DogLog.log("Superstructure/Turret/State", state.name()); - SmartDashboard.putNumber("Superstructure/Turret/Target Angle", getTargetAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/Turret/Current Angle", getAngle().getDegrees()); + DogLog.log("Superstructure/Turret/Target Angle", getTargetAngle().getDegrees()); + DogLog.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/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index fbbb3d88..0c7b0fe4 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 @@ -29,11 +29,11 @@ import com.stuypulse.robot.util.SysId; import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class TurretImpl extends Turret { @@ -275,34 +275,34 @@ public void periodicAfterScheduler() { turretMotor.stopMotor(); } - SmartDashboard.putNumber("Superstructure/Turret/Relative Encoder Position (deg)", + DogLog.log("Superstructure/Turret/Relative Encoder Position (deg)", turretMotorPos.getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Superstructure/Turret/Closed Loop Error (deg)", + DogLog.log("Superstructure/Turret/Closed Loop Error (deg)", turretMotorClosedLoopError.getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Superstructure/Turret/Encoder18t Abs Position (Rot)", + DogLog.log("Superstructure/Turret/Encoder18t Abs Position (Rot)", encoder18tPos.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Encoder17t Abs Position (Rot)", + DogLog.log("Superstructure/Turret/Encoder17t Abs Position (Rot)", encoder17tPos.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Voltage (volts)", + DogLog.log("Superstructure/Turret/Voltage (volts)", turretMotorVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Wrapped Target Angle (deg)", prevActualTargetAngle); + DogLog.log("Superstructure/Turret/Wrapped Target Angle (deg)", prevActualTargetAngle); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber("Superstructure/Turret/Stator Current (amps)", + DogLog.log("Superstructure/Turret/Stator Current (amps)", turretMotorStatorCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Supply Current (amps)", + DogLog.log("Superstructure/Turret/Supply Current (amps)", turretMotorSupplyCurrent.getValueAsDouble()); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Main/Turret Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Turret.MOTOR) + ")", turretMotor.isConnected()); - SmartDashboard.putBoolean("Robot/CAN/Main/Turret 17t Encoder Connected? (ID " + DogLog.log("Robot/CAN/Main/Turret 17t Encoder Connected? (ID " + String.valueOf(Ports.Superstructure.Turret.ENCODER17T) + ")", encoder17t.isConnected()); - SmartDashboard.putBoolean("Robot/CAN/Main/Turret 18t Encoder Connected? (ID " + DogLog.log("Robot/CAN/Main/Turret 18t Encoder Connected? (ID " + String.valueOf(Ports.Superstructure.Turret.ENCODER18T) + ")", encoder18t.isConnected()); } Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java index e902fac9..de15ecef 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java @@ -5,12 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.turret; +import java.util.Optional; + import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.DriverConstants; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; +import dev.doglog.DogLog; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.LinearQuadraticRegulator; @@ -25,11 +28,8 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public class TurretSim extends Turret { private LinearSystemSim sim; @@ -161,15 +161,15 @@ public void periodic() { goal = new TrapezoidProfile.State(Units.degreesToRadians(actualTargetAngle), 0.0); setpoint = profile.calculate(Settings.DT, setpoint, goal); - SmartDashboard.putNumber("Superstructure/Turret/Constraints/Max Vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); - SmartDashboard.putNumber("Superstructure/Turret/Constraints/Max Accel (deg per s per s)", Units.radiansToDegrees(maxAngularAccelRadiansPerSecondSquared)); + DogLog.log("Superstructure/Turret/Constraints/Max Vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); + DogLog.log("Superstructure/Turret/Constraints/Max Accel (deg per s per s)", Units.radiansToDegrees(maxAngularAccelRadiansPerSecondSquared)); - SmartDashboard.putNumber("Superstructure/Turret/Motion Profile Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); - SmartDashboard.putNumber("Superstructure/Turret/Error: abs(turret - target) (deg)", Math.abs(getAngle().minus(getTargetAngle()).getDegrees())); - SmartDashboard.putNumber("Superstructure/Turret/Current Angle (deg)", Units.radiansToDegrees(sim.getOutput(0))); - SmartDashboard.putNumber("Superstructure/Turret/Wrapped Target Angle (deg)", actualTargetAngle); - SmartDashboard.putNumber("Superstructure/Turret/Setpoint Filtered Angle (deg)", prevActualTargetAngle); - SmartDashboard.putBoolean("Superstructure/Turret/Is Wrapping", isWrapping); + DogLog.log("Superstructure/Turret/Motion Profile Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); + DogLog.log("Superstructure/Turret/Error: abs(turret - target) (deg)", Math.abs(getAngle().minus(getTargetAngle()).getDegrees())); + DogLog.log("Superstructure/Turret/Current Angle (deg)", Units.radiansToDegrees(sim.getOutput(0))); + DogLog.log("Superstructure/Turret/Wrapped Target Angle (deg)", actualTargetAngle); + DogLog.log("Superstructure/Turret/Setpoint Filtered Angle (deg)", prevActualTargetAngle); + DogLog.log("Superstructure/Turret/Is Wrapping", isWrapping); controller.setNextR(VecBuilder.fill(setpoint.position, 0.0)); controller.correct(VecBuilder.fill(sim.getOutput(0), sim.getOutput(1))); 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 36e69d46..a5051b09 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -5,6 +5,11 @@ /** ************************************************************ */ package com.stuypulse.robot.subsystems.swerve; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; + +import java.util.Optional; + import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; @@ -37,30 +42,22 @@ import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.math.Vector2D; +import dev.doglog.DogLog; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.LinearAcceleration; - -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; - -import java.util.Optional; - import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -713,31 +710,31 @@ public void periodicAfterScheduler() { ChassisSpeeds chassisSpeeds = getChassisSpeeds(); Vector2D fieldRelativeSpeeds = getFieldRelativeSpeeds(); - SmartDashboard.putNumber("Swerve/Velocity Robot Relative X (m per s)", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber("Swerve/Velocity Robot Relative Y (m per s)", chassisSpeeds.vyMetersPerSecond); + DogLog.log("Swerve/Velocity Robot Relative X (m per s)", chassisSpeeds.vxMetersPerSecond); + DogLog.log("Swerve/Velocity Robot Relative Y (m per s)", chassisSpeeds.vyMetersPerSecond); - SmartDashboard.putNumber("Swerve/Velocity Field Relative X (m per s)", fieldRelativeSpeeds.x); - SmartDashboard.putNumber("Swerve/Field Relative Rotation", pose.getRotation().getDegrees()); - SmartDashboard.putNumber("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); + 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); - SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", chassisSpeeds.omegaRadiansPerSecond); - SmartDashboard.putNumber("Swerve/Distance From Hub (meters)", Field.HUB_CENTER.getTranslation().getDistance(pose.getTranslation())); + DogLog.log("Swerve/Angular Velocity (rad per s)", chassisSpeeds.omegaRadiansPerSecond); + DogLog.log("Swerve/Distance From Hub (meters)", Field.HUB_CENTER.getTranslation().getDistance(pose.getTranslation())); Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { - SmartDashboard.putNumber("Swerve/Robot Accel X", robotAccelerationX.getValueAsDouble() * 9.81); - SmartDashboard.putNumber("Swerve/Robot Accel Y", robotAccelerationY.getValueAsDouble() * 9.81); + DogLog.log("Swerve/Robot Accel X", robotAccelerationX.getValueAsDouble() * 9.81); + DogLog.log("Swerve/Robot Accel Y", robotAccelerationY.getValueAsDouble() * 9.81); - SmartDashboard.putNumber("Swerve/Failed DAQ Count", this.getState().FailedDaqs); - SmartDashboard.putNumber("Swerve/CANBus Utiliaztion", Ports.CANIVORE.getStatus().BusUtilization); + DogLog.log("Swerve/Failed DAQ Count", this.getState().FailedDaqs); + DogLog.log("Swerve/CANBus Utiliaztion", Ports.CANIVORE.getStatus().BusUtilization); // will confirm whether we are even getting data - SmartDashboard.putNumber("Superstructure/Turret/Dist From Hub", + DogLog.log("Superstructure/Turret/Dist From Hub", turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); - SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Hub", + DogLog.log("InterpolationTesting/Turret Dist From Hub", turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); - SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Ferry Zone", turretPose.getTranslation() + DogLog.log("InterpolationTesting/Turret Dist From Ferry Zone", turretPose.getTranslation() .getDistance(Field.getFerryZonePose(pose.getTranslation()).getTranslation())); for (int i = 0; i < 4; i++) { @@ -745,15 +742,15 @@ public void periodicAfterScheduler() { SwerveModuleState current = getModule(i).getCurrentState(); SwerveModuleState target = getModule(i).getTargetState(); - SmartDashboard.putNumber(prefix + "/Speed (m per s)", current.speedMetersPerSecond); - SmartDashboard.putNumber(prefix + "/Target Speed (m per s)", target.speedMetersPerSecond); - SmartDashboard.putNumber(prefix + "/Angle (deg)", current.angle.getDegrees() % 360); - SmartDashboard.putNumber(prefix + "/Target Angle (deg)", target.angle.getDegrees() % 360); + 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); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber(prefix + "/Stator Current", + DogLog.log(prefix + "/Stator Current", getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber(prefix + "/Supply Current", + DogLog.log(prefix + "/Supply Current", getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble()); } } @@ -762,54 +759,54 @@ public void periodicAfterScheduler() { // CAN SIGNAL LOGGING if (Settings.DEBUG_MODE.get() && Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Front Left Drive Motor Connected? (ID " + String.valueOf(TunerConstants.kFrontLeftDriveMotorId) + ")", getModule(0).getDriveMotor().isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Front Left Steer Motor Connected? (ID " + String.valueOf(TunerConstants.kFrontLeftSteerMotorId) + ")", getModule(0).getSteerMotor().isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Front Left CANcoder Connected? (ID " + String.valueOf(TunerConstants.kFrontLeftEncoderId) + ")", getModule(0).getEncoder().isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Front Right Drive Motor Connected? (ID " + String.valueOf(TunerConstants.kFrontRightDriveMotorId) + ")", getModule(1).getDriveMotor().isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Front Right Steer Motor Connected? (ID " + String.valueOf(TunerConstants.kFrontRightSteerMotorId) + ")", getModule(1).getSteerMotor().isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Front Right CANcoder Connected? (ID " + String.valueOf(TunerConstants.kFrontRightEncoderId) + ")", getModule(1).getEncoder().isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Back Left Drive Motor Connected? (ID " + String.valueOf(TunerConstants.kBackLeftDriveMotorId) + ")", getModule(2).getDriveMotor().isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Back Left Steer Motor Connected? (ID " + String.valueOf(TunerConstants.kBackLeftSteerMotorId) + ")", getModule(2).getSteerMotor().isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Back Left CANcoder Connected? (ID " + String.valueOf(TunerConstants.kBackLeftEncoderId) + ")", getModule(2).getEncoder().isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Back Right Drive Motor Connected? (ID " + String.valueOf(TunerConstants.kBackRightDriveMotorId) + ")", getModule(3).getDriveMotor().isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Back Right Steer Motor Connected? (ID " + String.valueOf(TunerConstants.kBackRightSteerMotorId) + ")", getModule(3).getSteerMotor().isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Back Right CANcoder Connected? (ID " + String.valueOf(TunerConstants.kBackRightEncoderId) + ")", getModule(3).getEncoder().isConnected()); 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 4454b8ee..d64aa84e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -22,13 +22,13 @@ 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.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 { @@ -180,7 +180,7 @@ public void setTagBlacklist(int[] tagsToBlacklist, String limelight) { } System.out.println(Arrays.toString(validTags)); - SmartDashboard.putString("Allowlisted Tags for " + limelight, Arrays.toString(validTags)); + DogLog.log("Allowlisted Tags for " + limelight, Arrays.toString(validTags)); LimelightHelpers.SetFiducialIDFiltersOverride(limelight, validTags); } @@ -284,16 +284,16 @@ public void periodicAfterScheduler() { hasData = true; } - SmartDashboard.putBoolean("Vision/Within Invalid Position Tolerance", withinInvalidPositionTolerance); - SmartDashboard.putBoolean("Vision/Within Angular Velocity Tolerance", withinAngularVelocityTolerance); - SmartDashboard.putBoolean("Vision/Not Null", notNull); + DogLog.log("Vision/Within Invalid Position Tolerance", withinInvalidPositionTolerance); + DogLog.log("Vision/Within Angular Velocity Tolerance", withinAngularVelocityTolerance); + DogLog.log("Vision/Not Null", notNull); - SmartDashboard.putNumber("Vision/Pose X Component", robotPose.getX()); - SmartDashboard.putNumber("Vision/Pose Y Component", robotPose.getY()); - SmartDashboard.putNumber("Vision/Pose Theta (Degrees)", robotPose.getRotation().getDegrees()); - SmartDashboard.putNumber("Vision/Pose Estimate X " + limelightName, poseEstimate.pose.getX()); - SmartDashboard.putNumber("Vision/Pose Estimate Y " + limelightName, poseEstimate.pose.getY()); - SmartDashboard.putNumber("Vision/Pose Estimate Theta " + limelightName, poseEstimate.pose.getRotation().getDegrees()); + 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()); switch (limelightName) { case "limelight-right" -> @@ -304,18 +304,18 @@ public void periodicAfterScheduler() { backLimelightPosePublisher.set(robotPose); } - SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", true); + DogLog.log("Vision/" + names[i] + " Has Data", true); } else { - SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); + DogLog.log("Vision/" + names[i] + " Has Data", false); Cameras.LimelightCameras[i].incrementRejection(RejectionValue.NOT_NULL); } - SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); + DogLog.log("Vision/MegaTag Mode", megaTagMode.toString()); // this yaw is seems to be the robot yaw passed into the LL - SmartDashboard.putNumber("Vision/Limelight Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); + DogLog.log("Vision/Limelight Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); // this is just the yaw of the internal imu - SmartDashboard.putNumber("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); + DogLog.log("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); //Rejection counters Cameras.LimelightCameras[i].logRejections(); @@ -341,7 +341,7 @@ public void periodicAfterScheduler() { } } - SmartDashboard.putBoolean("Vision/Has Data", hasData); + DogLog.log("Vision/Has Data", hasData); } } } diff --git a/src/main/java/com/stuypulse/robot/util/EnergyUtil.java b/src/main/java/com/stuypulse/robot/util/EnergyUtil.java index 4344f167..350cfd1a 100644 --- a/src/main/java/com/stuypulse/robot/util/EnergyUtil.java +++ b/src/main/java/com/stuypulse/robot/util/EnergyUtil.java @@ -5,6 +5,7 @@ import com.stuypulse.robot.constants.Settings; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** @@ -58,27 +59,27 @@ public void logEnergyUsage(String subsystem, double amps) { public void periodic() { // energy used over the total time the robot has been on - SmartDashboard.putNumber("EnergyUtil/Total used Supply Current draw Amps", totalCurrent); - SmartDashboard.putNumber("EnergyUtil/total used Power Watts", totalPowerWatts); - SmartDashboard.putNumber("EnergyUtil/Total Used Energy Watt Hours", joulesToWattHours(totalEnergyWattHours)); + DogLog.log("EnergyUtil/Total used Supply Current draw Amps", totalCurrent); + DogLog.log("EnergyUtil/total used Power Watts", totalPowerWatts); + DogLog.log("EnergyUtil/Total Used Energy Watt Hours", joulesToWattHours(totalEnergyWattHours)); // energy used over the current command schedule loop totalCurrentCurrent = totalCurrent - totalCurrentCurrent; totalCurrentPowerWatts = totalPowerWatts - totalCurrentPowerWatts; - SmartDashboard.putNumber("EnergyUtil/Total current Current", totalCurrentCurrent); - SmartDashboard.putNumber("EnergyUtil/Total current Power Watts", totalCurrentCurrent); + DogLog.log("EnergyUtil/Total current Current", totalCurrentCurrent); + DogLog.log("EnergyUtil/Total current Power Watts", totalCurrentCurrent); for (var entry : subsytemCurrents.entrySet()) { - SmartDashboard.putNumber("EnergyUtil/Supply Current Amps/" + entry.getKey(), entry.getValue()); + DogLog.log("EnergyUtil/Supply Current Amps/" + entry.getKey(), entry.getValue()); subsytemCurrents.put(entry.getKey(), 0.0); } for (var entry : subsytemPowers.entrySet()) { - SmartDashboard.putNumber("EnergyUtil/Power Watts/" + entry.getKey(), entry.getValue()); + DogLog.log("EnergyUtil/Power Watts/" + entry.getKey(), entry.getValue()); subsytemPowers.put(entry.getKey(), 0.0); } for (var entry : subsytemEnergies.entrySet()) { - SmartDashboard.putNumber("EnergyUtil/Energy Watt Hours/" + entry.getKey(), joulesToWattHours(entry.getValue())); + DogLog.log("EnergyUtil/Energy Watt Hours/" + entry.getKey(), joulesToWattHours(entry.getValue())); } } diff --git a/src/main/java/com/stuypulse/robot/util/FMSUtil.java b/src/main/java/com/stuypulse/robot/util/FMSUtil.java index 8158e407..09ccbb59 100644 --- a/src/main/java/com/stuypulse/robot/util/FMSUtil.java +++ b/src/main/java/com/stuypulse/robot/util/FMSUtil.java @@ -12,6 +12,8 @@ import java.util.Optional; +import dev.doglog.DogLog; + public class FMSUtil { private final Timer timer = new Timer(); @@ -111,7 +113,7 @@ public boolean didWinAuto() { if (winner == null || winner.isEmpty() || allianceOpt.isEmpty()) { DriverStation.reportWarning("No FMS auto winner data available", false); - SmartDashboard.putBoolean("FMSUtil/No Auto Winner Data", true); + DogLog.log("FMSUtil/No Auto Winner Data", true); return autoOverride; } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java index e5b7c81d..95fe7366 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java @@ -16,6 +16,7 @@ import com.stuypulse.robot.constants.Settings.Superstructure.TOFInterpolation; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -152,9 +153,9 @@ public static InterpolatedFerryInfo interpolateFerryingInfo(Pose2d turretPose, P double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters); double flightTime = ferryingDistanceTOFInterpolator.get(distanceMeters); - SmartDashboard.putNumber("Superstructure/Interpolated Ferry Target Angle", targetAngle.getDegrees()); - SmartDashboard.putNumber("Superstructure/Interpolated Ferry RPM", targetRPM); - SmartDashboard.putNumber("Superstructure/Interpolated Ferry TOF", flightTime); + DogLog.log("Superstructure/Interpolated Ferry Target Angle", targetAngle.getDegrees()); + DogLog.log("Superstructure/Interpolated Ferry RPM", targetRPM); + DogLog.log("Superstructure/Interpolated Ferry TOF", flightTime); return new InterpolatedFerryInfo( targetAngle, diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json new file mode 100644 index 00000000..7201ed05 --- /dev/null +++ b/vendordeps/DogLog.json @@ -0,0 +1,20 @@ +{ + "javaDependencies": [ + { + "groupId": "com.github.jonahsnider", + "artifactId": "doglog", + "version": "2026.5.0" + } + ], + "fileName": "DogLog.json", + "frcYear": "2026", + "jsonUrl": "https://doglog.dev/vendordep.json", + "name": "DogLog", + "jniDependencies": [], + "mavenUrls": [ + "https://jitpack.io" + ], + "cppDependencies": [], + "version": "2026.5.0", + "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" +} \ No newline at end of file From e8134422e7b0792526bf406181d2893d5168a90b Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 18 Apr 2026 10:26:05 -0400 Subject: [PATCH 08/33] feat: follower auton + Empty test auton --- .../pathplanner/autos/Left Follower.auto | 31 +++++ ...{Left 2 Cycle.auto => Left Two Cycle.auto} | 0 .../pathplanner/autos/Right Follower.auto | 31 +++++ ...ight 2 Cycle.auto => Right Two Cycle.auto} | 0 .../paths/Left Follower Bump To Corner.path | 54 ++++++++ .../paths/Left Follower Over Bump.path | 54 ++++++++ .../paths/Left Follower To Bump.path | 127 ++++++++++++++++++ .../paths/Right Follower Bump To Corner.path | 54 ++++++++ .../paths/Right Follower Over Bump.path | 54 ++++++++ .../paths/Right Follower To Bump.path | 127 ++++++++++++++++++ src/main/deploy/pathplanner/settings.json | 1 + .../com/stuypulse/robot/RobotContainer.java | 11 ++ .../commands/auton/regular/LeftFollower.java | 67 +++++++++ .../commands/auton/regular/RightFollower.java | 67 +++++++++ .../robot/commands/auton/test/EmptyTest.java | 26 ++++ 15 files changed, 704 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/Left Follower.auto rename src/main/deploy/pathplanner/autos/{Left 2 Cycle.auto => Left Two Cycle.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/Right Follower.auto rename src/main/deploy/pathplanner/autos/{Right 2 Cycle.auto => Right Two Cycle.auto} (100%) create mode 100644 src/main/deploy/pathplanner/paths/Left Follower Bump To Corner.path create mode 100644 src/main/deploy/pathplanner/paths/Left Follower Over Bump.path create mode 100644 src/main/deploy/pathplanner/paths/Left Follower To Bump.path create mode 100644 src/main/deploy/pathplanner/paths/Right Follower Bump To Corner.path create mode 100644 src/main/deploy/pathplanner/paths/Right Follower Over Bump.path create mode 100644 src/main/deploy/pathplanner/paths/Right Follower To Bump.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollower.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollower.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java diff --git a/src/main/deploy/pathplanner/autos/Left Follower.auto b/src/main/deploy/pathplanner/autos/Left Follower.auto new file mode 100644 index 00000000..936b5e45 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Follower.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Follower To Bump" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Follower Over Bump" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Follower Bump To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Left 2 Cycle.auto rename to src/main/deploy/pathplanner/autos/Left Two Cycle.auto diff --git a/src/main/deploy/pathplanner/autos/Right Follower.auto b/src/main/deploy/pathplanner/autos/Right Follower.auto new file mode 100644 index 00000000..6071e1ed --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Follower.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Follower To Bump" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Follower Over Bump" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Follower Bump To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Right 2 Cycle.auto rename to src/main/deploy/pathplanner/autos/Right Two Cycle.auto diff --git a/src/main/deploy/pathplanner/paths/Left Follower Bump To Corner.path b/src/main/deploy/pathplanner/paths/Left Follower Bump To Corner.path new file mode 100644 index 00000000..ac1f8961 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Follower Bump To Corner.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.9651497860199716, + "y": 5.477660485021398 + }, + "prevControl": null, + "nextControl": { + "x": 1.9849260565692943, + "y": 3.8087102769799372 + }, + "isLocked": false, + "linkedName": "Left Follower Bump Post" + }, + { + "anchor": { + "x": 1.9171184022824534, + "y": 3.7180028530670475 + }, + "prevControl": { + "x": 2.1240710000936103, + "y": 4.068157127189754 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Corner" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Follower", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Follower Over Bump.path b/src/main/deploy/pathplanner/paths/Left Follower Over Bump.path new file mode 100644 index 00000000..0505c62d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Follower Over Bump.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.850470756062768, + "y": 5.503537803138374 + }, + "prevControl": null, + "nextControl": { + "x": 5.268338214575557, + "y": 5.5146770668110285 + }, + "isLocked": false, + "linkedName": "Left Follower Bump" + }, + { + "anchor": { + "x": 2.9651497860199716, + "y": 5.477660485021398 + }, + "prevControl": { + "x": 4.136040206997414, + "y": 5.467941169189516 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Follower Bump Post" + } + ], + "rotationTargets": [], + "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": 180.0 + }, + "reversed": false, + "folder": "Follower", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Follower To Bump.path b/src/main/deploy/pathplanner/paths/Left Follower To Bump.path new file mode 100644 index 00000000..b677cf1b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Follower To Bump.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.534450784593438, + "y": 7.677232524964337 + }, + "prevControl": null, + "nextControl": { + "x": 6.264507845934379, + "y": 7.716048502139801 + }, + "isLocked": false, + "linkedName": "Left Follower Start" + }, + { + "anchor": { + "x": 10.404878744650498, + "y": 5.9952068473609135 + }, + "prevControl": { + "x": 10.404878744650498, + "y": 7.573723252496435 + }, + "nextControl": { + "x": 10.404878744650498, + "y": 4.583409998537509 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 10.404878744650498, + "y": 2.191241084165479 + }, + "prevControl": { + "x": 10.430756062767475, + "y": 4.287303851640513 + }, + "nextControl": { + "x": 10.395817994061506, + "y": 1.4573202864572623 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.489957203994294, + "y": 2.7605420827389446 + }, + "prevControl": { + "x": 8.38644793152639, + "y": 1.130271041369473 + }, + "nextControl": { + "x": 8.676070675703448, + "y": 5.691829262158123 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.850470756062768, + "y": 5.503537803138374 + }, + "prevControl": { + "x": 8.502895863052782, + "y": 5.6070470756062765 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Follower Bump" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5884861407249452, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.927505330490407, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.4051172707889257, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.908315565031985, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.232409381663122, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.667377398720684, + "rotationDegrees": 180.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": 180.0 + }, + "reversed": false, + "folder": "Follower", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Follower Bump To Corner.path b/src/main/deploy/pathplanner/paths/Right Follower Bump To Corner.path new file mode 100644 index 00000000..ffa5d749 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Follower Bump To Corner.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.9263338088445074, + "y": 2.514707560627676 + }, + "prevControl": null, + "nextControl": { + "x": 0.9596576319543506, + "y": 2.320627674750357 + }, + "isLocked": false, + "linkedName": "Right Follower Bump Post" + }, + { + "anchor": { + "x": 1.3219400855920111, + "y": 0.8844365192582018 + }, + "prevControl": { + "x": 1.4383880171184018, + "y": 1.3114122681883031 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Corner" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Follower", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Follower Over Bump.path b/src/main/deploy/pathplanner/paths/Right Follower Over Bump.path new file mode 100644 index 00000000..33c9e281 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Follower Over Bump.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.018673323823109, + "y": 2.501768901569187 + }, + "prevControl": null, + "nextControl": { + "x": 4.70357217256972, + "y": 2.5005480781874923 + }, + "isLocked": false, + "linkedName": "Right Follower Bump" + }, + { + "anchor": { + "x": 2.9263338088445074, + "y": 2.514707560627676 + }, + "prevControl": { + "x": 4.156424440701705, + "y": 2.4750602578513616 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Follower Bump Post" + } + ], + "rotationTargets": [], + "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": 180.0 + }, + "reversed": false, + "folder": "Follower", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Follower To Bump.path b/src/main/deploy/pathplanner/paths/Right Follower To Bump.path new file mode 100644 index 00000000..5215aa48 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Follower To Bump.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4826961483594863, + "y": 0.4315834522111266 + }, + "prevControl": null, + "nextControl": { + "x": 5.876348074179743, + "y": 0.36689015691868687 + }, + "isLocked": false, + "linkedName": "Right Follower Start" + }, + { + "anchor": { + "x": 10.469572039942939, + "y": 1.6090014265335242 + }, + "prevControl": { + "x": 10.452937586581934, + "y": 0.6386583138082187 + }, + "nextControl": { + "x": 10.547203994293866, + "y": 6.13753209700428 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.395663338088445, + "y": 6.551569186875891 + }, + "prevControl": { + "x": 10.598737663708622, + "y": 6.525127992906218 + }, + "nextControl": { + "x": 8.218245363766048, + "y": 6.577446504992866 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.619343794579173, + "y": 2.838174037089873 + }, + "prevControl": { + "x": 8.55912562999586, + "y": 5.126464291255471 + }, + "nextControl": { + "x": 8.632282453637663, + "y": 2.3465049928673345 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.018673323823109, + "y": 2.501768901569187 + }, + "prevControl": { + "x": 6.976134094151213, + "y": 2.4888302425106996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Follower Bump" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0234541577825158, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.3731343283582114, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.9, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.4051172707889066, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.874200426439231, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.3859275053305025, + "rotationDegrees": 180.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": 180.0 + }, + "reversed": false, + "folder": "Follower", + "idealStartingState": { + "velocity": 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 65d1dd1c..8feeed13 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,6 +4,7 @@ "holonomicMode": true, "pathFolders": [ "Bump Stuff", + "Follower", "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 2265b23e..05dfc540 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -8,8 +8,10 @@ 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.LeftFollower; import com.stuypulse.robot.commands.auton.regular.LeftMiddy; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; +import com.stuypulse.robot.commands.auton.regular.RightFollower; import com.stuypulse.robot.commands.auton.regular.RightMiddy; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.handoff.HandoffReverse; @@ -381,6 +383,15 @@ public void configureAutons() { "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Score To NZ (F)", "Right NZ To Score"); RIGHT_TWO_CYCLE.register(autonChooser); + // FOLLOWERS + AutonConfig LEFT_FOLLOWER = new AutonConfig("Left Follower", LeftFollower::new, + "Left Follower To Bump", "Left Follower Over Bump", "Left Follower Bump To Corner"); + LEFT_FOLLOWER.register(autonChooser); + + AutonConfig RIGHT_FOLLOWER = new AutonConfig("Right Follower", RightFollower::new, + "Right Follower To Bump", "Right Follower Over Bump", "Right Follower Bump To Corner"); + RIGHT_FOLLOWER.register(autonChooser); + SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollower.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollower.java new file mode 100644 index 00000000..29a40f5d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollower.java @@ -0,0 +1,67 @@ +package com.stuypulse.robot.commands.auton.regular; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.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.constants.Gains.Spindexer; +import com.stuypulse.robot.subsystems.handoff.Handoff; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class LeftFollower extends SequentialCommandGroup { + + public LeftFollower(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + // Preloads + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(3.0) + ), + + // To NZ + new ParallelCommandGroup( + new HandoffStop(), + new SpindexerStop(), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new WaitCommand(0.75).andThen(new IntakeDeploy()) + ), + + new WaitCommand(1.0), + + // Back + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + + new WaitCommand(0.5), + + // SOTM To Corner + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]) + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollower.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollower.java new file mode 100644 index 00000000..8b17b1aa --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollower.java @@ -0,0 +1,67 @@ +package com.stuypulse.robot.commands.auton.regular; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.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.constants.Gains.Spindexer; +import com.stuypulse.robot.subsystems.handoff.Handoff; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightFollower extends SequentialCommandGroup { + + public RightFollower(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + // Preloads + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(3.0) + ), + + // To NZ + new ParallelCommandGroup( + new HandoffStop(), + new SpindexerStop(), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new WaitCommand(0.75).andThen(new IntakeDeploy()) + ), + + new WaitCommand(1.0), + + // Back + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + + new WaitCommand(0.5), + + // SOTM To Corner + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]) + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java new file mode 100644 index 00000000..f692f5f7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java @@ -0,0 +1,26 @@ +/************************ 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.test; + +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class EmptyTest extends SequentialCommandGroup { + + public EmptyTest(PathPlannerPath... paths) { + + addCommands( + + + + ); + + } + +} From d1b918fb0132d91937dbd690cd82ea822ca24a94 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 18 Apr 2026 11:51:38 -0400 Subject: [PATCH 09/33] fix: gear ratio change --- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 +- .../robot/subsystems/superstructure/Superstructure.java | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 339b1f86..b9894a04 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -80,7 +80,7 @@ public interface Intake { double PUSHDOWN_CURRENT_TELEOP = -75.0;//new SmartNumber("Intake/Pushdown Current", -65.0); //TODO: GET ACTUAL TYTY double PUSHDOWN_CURRENT_AUTON = -80.0; - double GEAR_RATIO = 37.93; + double GEAR_RATIO = 32.0/20.0 * 64.0/18.0 * 60.0/8.0; double STALL_CURRENT_LIMIT = 0; //TODO: set value double STALL_DEBOUNCE = 1.0; //TODO: VERIFY 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 48e28056..6a702a4e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -180,7 +180,6 @@ public boolean shouldStop() { boolean turretLaggingSOTM = !isTurretAtTolerance() && getState() == SuperstructureState.SOTM; - DogLog.log("Spindexer/Should Stop/Is Behind Hub While Ferrying?", isBehindHubWhileFerrying); DogLog.log("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); DogLog.log("Spindexer/Should Stop/Is Outside Alliance Zone?", isOutsideAllianceZone); From 87cb879f46452a9a0fa1ca31d5d7d737309146ff Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 18 Apr 2026 11:52:44 -0400 Subject: [PATCH 10/33] feat: auto wait time --- simgui.json | 6 +++++ src/main/java/com/stuypulse/robot/Robot.java | 4 ++++ .../com/stuypulse/robot/RobotContainer.java | 23 ++++++++++++++++--- .../robot/commands/auton/test/BoxTest.java | 10 ++++---- .../com/stuypulse/robot/util/PathUtil.java | 21 +++++++++++++++-- 5 files changed, 55 insertions(+), 9 deletions(-) diff --git a/simgui.json b/simgui.json index 407c8652..0d40fac0 100644 --- a/simgui.json +++ b/simgui.json @@ -102,6 +102,12 @@ "Intake": { "open": true }, + "Robot": { + "Auton": { + "open": true + }, + "open": true + }, "Spindexer": { "open": true }, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 9334050f..98f08724 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -208,6 +208,10 @@ public void disabledPeriodic() { fmsAttached = true; } } + + if (periodicCounter % 50 == 0 && robot.hasWaitTimeChanged()) { + robot.configureAutons(); + } } /** diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c968d01a..f89192cb 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.LeftTwoCycle; import com.stuypulse.robot.commands.auton.regular.RightMiddy; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; +import com.stuypulse.robot.commands.auton.test.BoxTest; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; @@ -80,6 +81,7 @@ 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 edu.wpi.first.math.geometry.Pose2d; @@ -129,6 +131,9 @@ public interface EnabledSubsystems { // Autons private static SendableChooser autonChooser = new SendableChooser<>(); + private static SmartNumber waitTime = new SmartNumber("Robot/Auton/Wait Time", 0.0); + private static double prevWaitTime = 0.0; + private static boolean hasWaitTimeChanged = false; // Robot container public RobotContainer() { @@ -359,7 +364,6 @@ private void configureElasticButtons() { /**************/ public void configureAutons() { - autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); // DEPOT @@ -374,16 +378,25 @@ public void configureAutons() { RIGHT_MIDDY.register(autonChooser); // TWO CYCLES (TRENCH) - AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, + AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, "Left Trench To NZ", "Left NZ To Score", "Left Score To Score", "Left Score To NZ (F)", "Left NZ To Score"); LEFT_TWO_CYCLE.register(autonChooser); - AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, + AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Score To NZ (F)", "Right NZ To Score"); RIGHT_TWO_CYCLE.register(autonChooser); + AutonConfig BOX = new AutonConfig("Skibidi", BoxTest::new, + "Right Trench To NZ"); + BOX.register(autonChooser); + SmartDashboard.putData("Autonomous", autonChooser); + } + public boolean hasWaitTimeChanged() { + hasWaitTimeChanged = prevWaitTime != getWaitTime(); + prevWaitTime = getWaitTime(); + return hasWaitTimeChanged; } public void configureSysids() { @@ -450,6 +463,10 @@ public Command getAutonomousCommand() { } } + public double getWaitTime() { + return waitTime.get(); + } + public void periodicAfterScheduler() { //TODO: get from energy util after testing double totalCurrentDraw = handoff.getCurrentDraw() + diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java index 1ad35b24..7f158341 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java @@ -5,21 +5,23 @@ /***************************************************************/ package com.stuypulse.robot.commands.auton.test; +import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import com.pathplanner.lib.path.PathPlannerPath; public class BoxTest extends SequentialCommandGroup { + + // private RobotContainer robot = new RobotContainer(); + // private double waitTime = robot.getWaitTime(); public BoxTest(PathPlannerPath... paths) { addCommands( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) ); } diff --git a/src/main/java/com/stuypulse/robot/util/PathUtil.java b/src/main/java/com/stuypulse/robot/util/PathUtil.java index 01f7ba36..b536cc17 100644 --- a/src/main/java/com/stuypulse/robot/util/PathUtil.java +++ b/src/main/java/com/stuypulse/robot/util/PathUtil.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; import com.pathplanner.lib.path.PathPlannerPath; import java.io.IOException; @@ -19,6 +21,7 @@ import java.util.Collections; import java.util.HashMap; import java.util.List; +import java.util.Optional; import java.util.function.Function; public class PathUtil { @@ -27,11 +30,13 @@ public static class AutonConfig { private final String name; private final Function auton; private final String[] paths; + private final Optional waitTime; - public AutonConfig(String name, Function auton, String... paths) { + public AutonConfig(String name, Function auton, double waitTime, String... paths) { this.name = name; this.auton = auton; this.paths = paths; + this.waitTime = Optional.of(waitTime); for (String path : paths) { try { @@ -41,9 +46,21 @@ public AutonConfig(String name, Function auton, Stri } } } + + public AutonConfig(String name, Function auton, String... paths) { + this(name, auton, 0.0, paths); + } + + private Command buildCommand() { + Command autonCommand = auton.apply(loadPaths(paths)); + if (waitTime.isPresent() && waitTime.get() > 0.0) { + return Commands.sequence(new WaitCommand(waitTime.get()), autonCommand); + } + return autonCommand; + } public AutonConfig register(SendableChooser chooser) { - chooser.addOption(name, auton.apply(loadPaths(paths))); + chooser.addOption(name, buildCommand()); return this; } From 859e7e8faf800dd26743f4aa543e37321cee7d14 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 18 Apr 2026 14:25:01 -0400 Subject: [PATCH 11/33] feat: add limelight stuff to doglog and get rid of turret ramp rate --- .../stuypulse/robot/constants/Cameras.java | 39 +++++++++++++------ .../com/stuypulse/robot/constants/Gains.java | 4 +- .../stuypulse/robot/constants/Settings.java | 5 ++- .../superstructure/Superstructure.java | 3 ++ .../superstructure/turret/TurretImpl.java | 28 +++++++------ .../superstructure/turret/TurretSim.java | 2 +- .../swerve/CommandSwerveDrivetrain.java | 26 +++++++------ .../subsystems/vision/LimelightVision.java | 30 +++++++------- 8 files changed, 83 insertions(+), 54 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 0bcc63ce..ab5650a5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -5,7 +5,12 @@ /***************************************************************/ package com.stuypulse.robot.constants; +import org.jspecify.annotations.Nullable; + +import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.subsystems.vision.LimelightVision; +import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.stuylib.network.SmartBoolean; @@ -13,6 +18,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.struct.StructSerializable; /** This interface stores information about each camera. */ public interface Cameras { @@ -40,6 +46,7 @@ public static class Camera { private String name; private Pose3d location; private SmartBoolean isEnabled; + private String keyName; private int rejectedCounterNotNull; private int rejectedCounterAngularVelocity; @@ -48,6 +55,13 @@ public static class Camera { private Pipeline currentPipeline; + public Camera(String name, Pose3d location, SmartBoolean isEnabled) { + this.name = name; + this.location = location; + this.isEnabled = isEnabled; + this.keyName = "Vision/" + name + "/"; + } + public enum Pipeline { NO_SUN, LOW_SUN, @@ -103,17 +117,20 @@ public void incrementRejection(RejectionValue rejectionValue) { } } - public void logRejections() { - DogLog.log("Vision/" + name + "/# Rejected Not Null", rejectedCounterNotNull); - DogLog.log("Vision/" + name + "/# Rejected Target Area", rejectedCounterTargetArea); - DogLog.log("Vision/" + name + "/# Rejected Angular Velocity", rejectedCounterAngularVelocity); - DogLog.log("Vision/" + name + "/# Rejected Invalid Position", rejectedCounterInvalidPosition); - } - - public Camera(String name, Pose3d location, SmartBoolean isEnabled) { - this.name = name; - this.location = location; - this.isEnabled = isEnabled; + public void log() { + DogLog.log(keyName + "# Rejected Not Null", rejectedCounterNotNull); + DogLog.log(keyName + "# Rejected Target Area", rejectedCounterTargetArea); + DogLog.log(keyName + "# Rejected Angular Velocity", rejectedCounterAngularVelocity); + DogLog.log(keyName + "# Rejected Invalid Position", rejectedCounterInvalidPosition); + + DogLog.log(keyName + "Heartbeat", LimelightHelpers.getHeartbeat(name)); + DogLog.log(keyName + "Temp (C)", LimelightHelpers.getLimelightNTString(name, "hw")); + DogLog.log(keyName + "Pose MT1", (Robot.isBlue() + ? LimelightHelpers.getBotPoseEstimate_wpiBlue(name).pose + : LimelightHelpers.getBotPoseEstimate_wpiRed(name).pose)); + DogLog.log(keyName + "Pose MT2", (Robot.isBlue() + ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).pose + : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(name).pose)); } public String getName() { diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 0c9bc260..6d6e6132 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -45,9 +45,9 @@ public interface slot0 { } public interface slot1 { - SmartNumber kP = new SmartNumber("Superstructure/Turret/Gains/kP", 100.0); + SmartNumber kP = new SmartNumber("Superstructure/Turret/Gains/kP", 150.0); // 80 SmartNumber kI = new SmartNumber("Superstructure/Turret/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("Superstructure/Turret/Gains/kD", 10.0); + SmartNumber kD = new SmartNumber("Superstructure/Turret/Gains/kD", 3.0); // 10 SmartNumber kS = new SmartNumber("Superstructure/Turret/Gains/kS", 0.4775); SmartNumber kV = new SmartNumber("Superstructure/Turret/Gains/kV", 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 b9894a04..7e37caf7 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -269,8 +269,9 @@ public interface Turret { public final double RANGE_CW = 90.0;//-360.0; public final double RANGE_CCW = -360.0;//85.0; // -397.0 is further - public final Rotation2d GAIN_SWITCHING_THRESHOLD = Rotation2d.fromDegrees(30); - + public final Rotation2d GAIN_SWITCHING_THRESHOLD_START = Rotation2d.fromDegrees(30); + public final Rotation2d GAIN_SWITCHING_THRESHOLD_END = Rotation2d.fromDegrees(3); + public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(-4.0), Units.inchesToMeters(8.0), Rotation2d.kZero); public final double TURRET_HEIGHT = Units.inchesToMeters(0.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 6a702a4e..f3cea6e0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -61,9 +61,12 @@ public Superstructure() { sotmStoppedTimer = new Timer(); sotmStoppedTimer.restart(); + sotmStoppedTimer.stop(); + fotmStoppedTimer = new Timer(); fotmStoppedTimer.restart(); + fotmStoppedTimer.stop(); } public enum SuperstructureState { 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 0c7b0fe4..05119635 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 @@ -69,7 +69,7 @@ public TurretImpl() { .withSupplyCurrentLimitAmps(80) .withStatorCurrentLimitEnabled(false) - .withRampRate(0.25) + .withRampRate(0.0) .withPIDConstants(Gains.Superstructure.Turret.slot0.kP, Gains.Superstructure.Turret.slot0.kI, Gains.Superstructure.Turret.slot0.kD, 0) @@ -207,14 +207,14 @@ public double getWrappedTargetAngle() { public void periodicAfterScheduler() { super.periodicAfterScheduler(); - // turretConfig.updateGainsConfig( - // turretMotor, 1, - // Gains.Superstructure.Turret.slot1.kP, - // Gains.Superstructure.Turret.slot1.kI, - // Gains.Superstructure.Turret.slot1.kD, - // Gains.Superstructure.Turret.slot1.kS, - // Gains.Superstructure.Turret.slot1.kV, - // Gains.Superstructure.Turret.slot1.kA); + turretConfig.updateGainsConfig( + turretMotor, 1, + Gains.Superstructure.Turret.slot1.kP, + Gains.Superstructure.Turret.slot1.kI, + Gains.Superstructure.Turret.slot1.kD, + Gains.Superstructure.Turret.slot1.kS, + Gains.Superstructure.Turret.slot1.kV, + Gains.Superstructure.Turret.slot1.kA); if (!hasUsedAbsoluteEncoder) { seedTurret(); @@ -241,8 +241,14 @@ public void periodicAfterScheduler() { prevActualTargetAngle = actualTargetAngle; } - isWrapping = Math.abs(getWrappedTargetAngle() - - currentAngle) > Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD.getDegrees(); + if(isWrapping) { + isWrapping = Math.abs(getWrappedTargetAngle() + - currentAngle) > Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD_END.getDegrees(); + } else { + isWrapping = Math.abs(getWrappedTargetAngle() + - currentAngle) > Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD_START.getDegrees(); + } + int slot = 0; if (isWrapping) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java index de15ecef..f9e882c3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java @@ -156,7 +156,7 @@ public void periodic() { prevActualTargetAngle = actualTargetAngle; } isWrapping = Math.abs(actualTargetAngle - currentAngle) > - Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD.getDegrees(); + Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD_START.getDegrees(); goal = new TrapezoidProfile.State(Units.degreesToRadians(actualTargetAngle), 0.0); setpoint = profile.calculate(Settings.DT, setpoint, goal); 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 a5051b09..0e03cc16 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -72,9 +72,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private FieldObject2d turret2d = Field.FIELD2D.getObject("Turret 2D"); private Pose2d turretPose = new Pose2d(); - private StructPublisher leftBehindHubYPlublisher; - private StructPublisher rightBehindHubYPlublisher; - private StructPublisher vertexBehindHubPublisher; + // private StructPublisher leftBehindHubYPlublisher; + // private StructPublisher rightBehindHubYPlublisher; + // private StructPublisher vertexBehindHubPublisher; private StatusSignal robotAccelerationX; private StatusSignal robotAccelerationY; @@ -85,8 +85,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private Optional isUnderTrench = Optional.empty(); private Optional isBehindTower = Optional.empty(); - private StructPublisher robotPose = NetworkTableInstance.getDefault() - .getStructTopic("Robot Pose", Pose2d.struct).publish(); + // private StructPublisher robotPose = NetworkTableInstance.getDefault() + // .getStructTopic("Robot Pose", Pose2d.struct).publish(); static { instance = TunerConstants.createDrivetrain(); @@ -248,9 +248,9 @@ protected CommandSwerveDrivetrain( startSimThread(); } - leftBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/LeftBehindHubY", Pose2d.struct).publish(); - rightBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/RightBehindHubY", Pose2d.struct).publish(); - vertexBehindHubPublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/VertexBehindHub", Pose2d.struct).publish(); + // leftBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/LeftBehindHubY", Pose2d.struct).publish(); + // rightBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/RightBehindHubY", Pose2d.struct).publish(); + // vertexBehindHubPublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/VertexBehindHub", Pose2d.struct).publish(); robotAccelerationX = this.getPigeon2().getAccelerationX(); robotAccelerationY = this.getPigeon2().getAccelerationY(); @@ -592,9 +592,9 @@ public boolean isBehindHub() { * (turretTranslation.getX() - hubFarRightCornerWithTolerance.getX()) + hubFarRightCornerWithTolerance.getY(); // *(robotX - hubCornerX) + (hubCornerY) // Debug: - leftBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), leftY, new Rotation2d())); - rightBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), rightY, new Rotation2d())); - vertexBehindHubPublisher.set(Field.BEHIND_HUB_TRIANGLE_VERTEX); + // leftBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), leftY, new Rotation2d())); + // rightBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), rightY, new Rotation2d())); + // vertexBehindHubPublisher.set(Field.BEHIND_HUB_TRIANGLE_VERTEX); boolean withinHubY = rightY < getTurretPose().getY() && getTurretPose().getY() < leftY; @@ -704,7 +704,7 @@ public void periodicAfterScheduler() { Settings.Superstructure.Turret.TURRET_OFFSET.getTranslation().rotateBy(pose.getRotation())), pose.getRotation().plus(Turret.getInstance().getAngle())); - robotPose.set(pose); + // robotPose.set(pose); turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); @@ -720,6 +720,8 @@ public void periodicAfterScheduler() { 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/Pose", pose); + Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 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 d64aa84e..28f853e8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -50,9 +50,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; @@ -73,9 +73,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]; @@ -295,14 +295,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); @@ -318,7 +318,7 @@ public void periodicAfterScheduler() { DogLog.log("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); //Rejection counters - Cameras.LimelightCameras[i].logRejections(); + Cameras.LimelightCameras[i].log(); } } From 31e45bf9233e5523ad43fb5c8809de77612bfc02 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 18 Apr 2026 15:53:06 -0400 Subject: [PATCH 12/33] fix: digest angle, debounce individual tolerances for readyToShoot --- .../robot/commands/intake/IntakeAutoDigest.java | 3 +-- .../com/stuypulse/robot/constants/Settings.java | 9 ++++++--- .../robot/subsystems/intake/IntakeImpl.java | 4 ++-- .../subsystems/superstructure/Superstructure.java | 10 ++++++---- .../robot/subsystems/superstructure/hood/Hood.java | 9 +++++++++ .../subsystems/superstructure/shooter/Shooter.java | 13 +++++++++++++ .../subsystems/superstructure/turret/Turret.java | 10 ++++++++++ 7 files changed, 47 insertions(+), 11 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java index e8ea4209..1006ce74 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java @@ -9,8 +9,7 @@ public IntakeAutoDigest() { addCommands( - new IntakeDigest().andThen(new WaitCommand(0.25)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.25)) - .andThen(new IntakeDigest()).andThen(new WaitCommand(0.25)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.25)) + new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.5)) ); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 7e37caf7..1d6bff50 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -64,7 +64,7 @@ public interface Handoff { public interface Intake { Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(71.0); Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(-10.0); - Rotation2d PIVOT_DIGEST_ANGLE = Rotation2d.fromDegrees(30); + Rotation2d PIVOT_DIGEST_ANGLE = Rotation2d.fromDegrees(50); Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(5.0); @@ -82,8 +82,11 @@ public interface Intake { double GEAR_RATIO = 32.0/20.0 * 64.0/18.0 * 60.0/8.0; - double STALL_CURRENT_LIMIT = 0; //TODO: set value - double STALL_DEBOUNCE = 1.0; //TODO: VERIFY + double PIVOT_STALL_CURRENT = 0; //TODO: set value + double PIVOT_STALL_DEBOUNCE = 1.0; //TODO: VERIFY + + double ROLLER_STALL_DEBOUNCE = 0.05; //TODO: VERIFY + double ROLLER_STALL_CURRENT = 50.0; } public interface Spindexer { 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 54c86b92..422b6b40 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -137,8 +137,8 @@ public IntakeImpl() { rollerLeaderVoltage, rollerFollowerVoltage); pivotStalling = BStream.create( - () -> Math.abs(pivotSupplyCurrent.getValueAsDouble()) > Settings.Intake.STALL_CURRENT_LIMIT) - .filtered(new BDebounce.Both(Settings.Intake.STALL_DEBOUNCE)); + () -> Math.abs(pivotSupplyCurrent.getValueAsDouble()) > Settings.Intake.PIVOT_STALL_CURRENT) + .filtered(new BDebounce.Both(Settings.Intake.PIVOT_STALL_DEBOUNCE)); } @Override 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 f3cea6e0..26460c75 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -48,7 +48,7 @@ public static Superstructure getInstance(){ private final Shooter shooter; private final Turret turret; - private final BStream readyToShoot; + // private final BStream readyToShoot; public Superstructure() { state = SuperstructureState.INTERPOLATION; @@ -56,8 +56,8 @@ public Superstructure() { shooter = Shooter.getInstance(); turret = Turret.getInstance(); - readyToShoot = BStream.create(this::atTolerance) - .filtered(new BDebounce.Both(0.05)); + // readyToShoot = BStream.create(this::atTolerance) + // .filtered(new BDebounce.Both(0.05)); sotmStoppedTimer = new Timer(); sotmStoppedTimer.restart(); @@ -118,7 +118,7 @@ public SuperstructureState getState(){ } public boolean isReadyToShoot() { - return readyToShoot.get(); + return turret.turretReadyToShoot() && shooter.shooterReadyToShoot() && hood.hoodReadyToShoot(); } public boolean atTolerance() { @@ -234,6 +234,8 @@ else if (getState() == SuperstructureState.FOTM && shouldStop() && DriverStation DogLog.log("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); DogLog.log("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); + DogLog.log("Superstructure/Is Ready To Shoot?", isReadyToShoot()); + DogLog.log("Superstructure/Should Stop?", shouldStop()); } } \ No newline at end of file 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 0555fb55..30dc518f 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 @@ -13,6 +13,8 @@ import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.robot.util.superstructure.VisualizerHood; import com.stuypulse.stuylib.input.Gamepad; +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.Rotation2d; @@ -23,6 +25,7 @@ public abstract class Hood extends SubsystemBase{ private static final Hood instance; private HoodState state; + private BStream readyToShoot; private Rotation2d driverInput; @@ -56,6 +59,8 @@ public enum HoodState { public Hood() { state = HoodState.STOW; + readyToShoot = BStream.create(this::atTolerance) + .filtered(new BDebounce.Both(0.05)); } public HoodState getState(){ @@ -101,6 +106,10 @@ public boolean atTolerance() { } } + public boolean hoodReadyToShoot() { + return readyToShoot.get(); + } + public abstract Rotation2d getAngle(); public void hoodAnalogToInput(Gamepad gamepad) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index afd0b423..3441eeb3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -9,6 +9,8 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.robot.util.superstructure.SOTMCalculator; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -17,6 +19,8 @@ public abstract class Shooter extends SubsystemBase { private static final Shooter instance; + private BStream readyToShoot; + private ShooterState state; static { @@ -46,6 +50,9 @@ public enum ShooterState { public Shooter() { state = ShooterState.MANUAL_OVERRIDE; + + readyToShoot = BStream.create(this::atTolerance) + .filtered(new BDebounce.Both(0.05)); } public void setState(ShooterState state) { @@ -97,6 +104,12 @@ public boolean atTolerance() { return error > -toleranceLow && error < toleranceHigh; } + public boolean shooterReadyToShoot() { + return readyToShoot.get(); + } + + + public abstract double getRPM(); public abstract SysIdRoutine getShooterSysIdRoutine(); 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 1b549bc6..b9866e4d 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 @@ -15,6 +15,8 @@ import com.stuypulse.robot.util.superstructure.VisualizerTurret; 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 dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; @@ -28,6 +30,8 @@ public abstract class Turret extends SubsystemBase { private TurretState state; private Vector2D driverInput; + private final BStream readyToShoot; + static { instance = Robot.isReal() ? new TurretImpl() : new TurretSim(); } @@ -39,6 +43,8 @@ public static Turret getInstance() { public Turret() { driverInput = Vector2D.kOrigin; state = TurretState.SCORE; + readyToShoot = BStream.create(this::atTolerance) + .filtered(new BDebounce.Both(0.05)); } public void setDriverInput(Gamepad gamepad) { @@ -95,6 +101,10 @@ public boolean atTolerance() { return Math.abs(error) < tolerance; } + public boolean turretReadyToShoot() { + return readyToShoot.get(); + } + public Rotation2d getScoringAngle() { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); From 5028da66085261bc6fdea4fb57e3f72662e022f6 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 18 Apr 2026 16:03:30 -0400 Subject: [PATCH 13/33] FEAT: motor temp for shooter --- .../superstructure/shooter/ShooterImpl.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) 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 f52865cc..c2fb64ca 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -27,6 +27,7 @@ import dev.doglog.DogLog; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -49,6 +50,8 @@ public class ShooterImpl extends Shooter { private StatusSignal shooterLeaderVoltage; private StatusSignal shooterFollowerVoltage; private StatusSignal shooterLeaderClosedLoopError; + private StatusSignal shooterLeaderTemperature; + private StatusSignal shooterFollowerTemperature; public ShooterImpl() { shooterConfig = new Motors.TalonFXConfig() @@ -99,9 +102,14 @@ public ShooterImpl() { shooterLeaderVoltage = shooterLeader.getMotorVoltage(); shooterFollowerVoltage = shooterLeader.getMotorVoltage(); shooterLeaderClosedLoopError = shooterLeader.getClosedLoopError(); + + shooterLeaderTemperature = shooterLeader.getDeviceTemp(); + shooterFollowerTemperature = shooterFollower.getDeviceTemp(); + PhoenixUtil.registerToRio(shooterLeaderSpeed, shooterFollowerSpeed, shooterFollowSupplyCurrent, shooterFollowStatorCurrent, shooterLeadSupplyCurrent, shooterLeadStatorCurrent, - shooterLeaderVoltage, shooterFollowerVoltage, shooterLeaderClosedLoopError); + shooterLeaderVoltage, shooterFollowerVoltage, shooterLeaderClosedLoopError, + shooterLeaderTemperature, shooterFollowerTemperature); voltageOverride = Optional.empty(); } @@ -152,12 +160,20 @@ public void periodicAfterScheduler() { 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()); + + DogLog.log("Superstructure/Shooter/Follower Motor Temp (C)", + shooterFollowerTemperature.getValueAsDouble()); + + if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log( From fda4e8f9bd3a4860ab1706f49b24b77b7cf5edeb Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 20 Apr 2026 15:07:38 -0400 Subject: [PATCH 14/33] feat: wait time for follower autons --- elastic-layout.json | 35 ++++++++++--------- src/main/java/com/stuypulse/robot/Robot.java | 12 ++----- .../com/stuypulse/robot/RobotContainer.java | 10 ++---- 3 files changed, 23 insertions(+), 34 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 7b5f2dbb..8bda01ed 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -96,14 +96,14 @@ } }, { - "title": "Back Limelight", + "title": "Swerve", "x": 0.0, "y": 0.0, "width": 166.0, "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Back Limelight Is Enabled", + "topic": "SmartDashboard/Enabled Subsystems/Swerve Is Enabled", "period": 0.06, "data_type": "boolean" } @@ -122,27 +122,14 @@ } }, { - "title": "LEDs Is Enabled", - "x": 0.0, - "y": 0.0, - "width": 140.0, - "height": 140.0, - "type": "Toggle Switch", - "properties": { - "topic": "SmartDashboard/Enabled Subsystems/LEDs Is Enabled", - "period": 0.5, - "data_type": "boolean" - } - }, - { - "title": "Swerve", + "title": "Back Limelight", "x": 0.0, "y": 0.0, "width": 166.0, "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Swerve Is Enabled", + "topic": "SmartDashboard/Enabled Subsystems/Back Limelight Is Enabled", "period": 0.06, "data_type": "boolean" } @@ -519,6 +506,20 @@ "period": 0.5, "sort_options": false } + }, + { + "title": "Wait Time", + "x": 840.0, + "y": 560.0, + "width": 140.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "SmartDashboard/Robot/Auton/Wait Time", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } } ] } diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 98f08724..70bd7de9 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -82,15 +82,9 @@ public static int getPeriodicCounter() { return periodicCounter; } - /** - * ********************** - */ - /** - * * ROBOT SCHEDULEING ** - */ - /** - * ********************** - */ + /**************************/ + /**** ROBOT SCHEDULEING ***/ + /**************************/ @Override public void robotInit() { DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index dfe853e0..e0e3be82 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -389,21 +389,15 @@ public void configureAutons() { RIGHT_TWO_CYCLE.register(autonChooser); // FOLLOWERS - AutonConfig LEFT_FOLLOWER = new AutonConfig("Left Follower", LeftFollower::new, + AutonConfig LEFT_FOLLOWER = new AutonConfig("Left Follower", LeftFollower::new, prevWaitTime, "Left Follower To Bump", "Left Follower Over Bump", "Left Follower Bump To Corner"); LEFT_FOLLOWER.register(autonChooser); - AutonConfig RIGHT_FOLLOWER = new AutonConfig("Right Follower", RightFollower::new, + AutonConfig RIGHT_FOLLOWER = new AutonConfig("Right Follower", RightFollower::new, prevWaitTime, "Right Follower To Bump", "Right Follower Over Bump", "Right Follower Bump To Corner"); RIGHT_FOLLOWER.register(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); - - AutonConfig BOX = new AutonConfig("Skibidi", BoxTest::new, - "Right Trench To NZ"); - BOX.register(autonChooser); - - SmartDashboard.putData("Autonomous", autonChooser); } public boolean hasWaitTimeChanged() { From ec2261060fc2d9aae4f6869a97b0a53aaa451405 Mon Sep 17 00:00:00 2001 From: Brian Date: Sun, 19 Apr 2026 18:21:31 -0400 Subject: [PATCH 15/33] feat: memoize shouldStop, use status signals for total drive/steer current --- src/main/java/com/stuypulse/robot/Robot.java | 1 + .../superstructure/Superstructure.java | 33 ++++++++++++++----- .../swerve/CommandSwerveDrivetrain.java | 23 ++++++++++--- 3 files changed, 45 insertions(+), 12 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 70bd7de9..c68a135e 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -167,6 +167,7 @@ public void robotPeriodic() { //clearing memoized values InterpolationCalculator.clearMemoized(); CommandSwerveDrivetrain.getInstance().clearMemoized(); + Superstructure.getInstance().clearMemoized(); energyUtil.periodic(); } 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 26460c75..69eca4ec 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure; +import java.util.Optional; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; import com.stuypulse.robot.subsystems.handoff.Handoff; @@ -44,6 +46,8 @@ public static Superstructure getInstance(){ private SuperstructureState state; + private Optional shouldStop; + private final Hood hood; private final Shooter shooter; private final Turret turret; @@ -67,6 +71,8 @@ public Superstructure() { fotmStoppedTimer = new Timer(); fotmStoppedTimer.restart(); fotmStoppedTimer.stop(); + + this.shouldStop = Optional.empty(); } public enum SuperstructureState { @@ -162,6 +168,9 @@ public double getCurrentDraw() { } public boolean shouldStop() { + if (!shouldStop.isEmpty()) { + return shouldStop.get(); + } CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); boolean isSpindexerStopState = Spindexer.getInstance().getState() == SpindexerState.STOP; @@ -190,14 +199,22 @@ public boolean shouldStop() { DogLog.log("Spindexer/Should Stop/Turret Lagging SOTM", turretLaggingSOTM); DogLog.log("Spindexer/Should Stop/In Manual State", inManualState); - return isSpindexerStopState || - isHandOffStopState || - isTurretWrapping || - (isBehindHubWhileFerrying && !inManualState) || - turretLaggingSOTM || - (isOutsideAllianceZone && !inManualState) || - (isUnderTrench && !inManualState) || - isBehindTower; + boolean shouldStop = isSpindexerStopState || + isHandOffStopState || + isTurretWrapping || + (isBehindHubWhileFerrying && !inManualState) || + turretLaggingSOTM || + (isOutsideAllianceZone && !inManualState) || + (isUnderTrench && !inManualState) || + isBehindTower; + + this.shouldStop = Optional.of(shouldStop); + + return shouldStop; + } + + public void clearMemoized() { + this.shouldStop = Optional.empty(); } public void periodicAfterScheduler() { 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 0e03cc16..6724616b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -54,6 +54,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; @@ -78,6 +79,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private StatusSignal robotAccelerationX; private StatusSignal robotAccelerationY; + private StatusSignal[] driveMotorSupplyCurrents; + private StatusSignal[] steerMotorSupplyCurrents; + //TODO: might wanna change some of these initialized values like isBehindTower indicates that we are in the alliance zone private Optional isBehindHub = Optional.empty(); private Optional isOutsideAllianceZone = Optional.empty(); @@ -255,10 +259,21 @@ protected CommandSwerveDrivetrain( robotAccelerationX = this.getPigeon2().getAccelerationX(); robotAccelerationY = this.getPigeon2().getAccelerationY(); + driveMotorSupplyCurrents = new StatusSignal[4]; + steerMotorSupplyCurrents = new StatusSignal[4]; + + for (int i = 0; i < 4; i++) { + driveMotorSupplyCurrents[i] = getModule(i).getDriveMotor().getSupplyCurrent(); + steerMotorSupplyCurrents[i] = getModule(i).getSteerMotor().getSupplyCurrent(); + } + PhoenixUtil.registerToCanivore( robotAccelerationX, robotAccelerationY ); + + PhoenixUtil.registerToCanivore(driveMotorSupplyCurrents); + PhoenixUtil.registerToCanivore(steerMotorSupplyCurrents); } /** @@ -674,8 +689,8 @@ public void autonInit() { public double getTotalDriveSupplyCurrent() { double total = 0.0; - for (SwerveModule module : getModules()) { - total += Double.max(0, module.getDriveMotor().getSupplyCurrent().getValueAsDouble()); + for (StatusSignal s : driveMotorSupplyCurrents) { + total += Double.max(0, s.getValueAsDouble()); } return total; @@ -684,8 +699,8 @@ public double getTotalDriveSupplyCurrent() { public double getTotalSteerSupplyCurrent() { double total = 0.0; - for (SwerveModule module : getModules()) { - total += Double.max(0, module.getSteerMotor().getSupplyCurrent().getValueAsDouble()); + for (StatusSignal s : steerMotorSupplyCurrents) { + total += Double.max(0, s.getValueAsDouble()); } return total; From adc6cb8c40a987669622815d1dd54cb6efc7eb41 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 20 Apr 2026 17:24:12 -0400 Subject: [PATCH 16/33] ADD: autos, ferry changes, digest on stop sotm --- elastic-layout.json | 82 +++++------ ...epot Only.auto => Center Hub + Depot.auto} | 0 .../autos/{Left Middy.auto => Left Bump.auto} | 0 .../pathplanner/autos/Left Corner Bite.auto | 43 ++++++ .../{Left Follower.auto => Left Follow.auto} | 10 +- .../{Right Middy.auto => Right Bump.auto} | 0 .../pathplanner/autos/Right Corner Bite.auto | 43 ++++++ ...{Right Follower.auto => Right Follow.auto} | 10 +- .../paths/Left Bite Score To Score.path | 131 ++++++++++++++++++ .../paths/Left Corner Bite To Score.path | 87 ++++++++++++ .../pathplanner/paths/Left Corner Bite.path | 81 +++++++++++ ... To Bump.path => Left Follow To Bump.path} | 24 ++-- ...er Bump.path => Left Follow To Score.path} | 18 +-- .../paths/Left Follower Bump To Corner.path | 54 -------- .../pathplanner/paths/Left NZ To Score.path | 2 +- .../paths/Left Score To Score.path | 4 +- .../paths/Right Bite Score To Score.path | 131 ++++++++++++++++++ .../paths/Right Corner Bite To Score.path | 87 ++++++++++++ .../pathplanner/paths/Right Corner Bite.path | 81 +++++++++++ ...To Bump.path => Right Follow To Bump.path} | 22 +-- ...r Bump.path => Right Follow To Score.path} | 20 +-- .../paths/Right Follower Bump To Corner.path | 54 -------- src/main/deploy/pathplanner/settings.json | 2 +- .../com/stuypulse/robot/RobotContainer.java | 46 ++++-- .../regular/{LeftMiddy.java => LeftBump.java} | 4 +- .../{LeftFollower.java => LeftFollow.java} | 7 +- .../commands/auton/regular/LeftTwoCorner.java | 87 ++++++++++++ .../commands/auton/regular/LeftTwoCycle.java | 4 +- .../{RightMiddy.java => RightBump.java} | 4 +- .../{RightFollower.java => RightFollow.java} | 7 +- .../auton/regular/RightTwoCorner.java | 87 ++++++++++++ .../commands/auton/regular/RightTwoCycle.java | 4 +- .../commands/swerve/SwerveDriveSOTM.java | 36 +++-- .../stuypulse/robot/constants/Settings.java | 11 +- 34 files changed, 1025 insertions(+), 258 deletions(-) rename src/main/deploy/pathplanner/autos/{Depot Only.auto => Center Hub + Depot.auto} (100%) rename src/main/deploy/pathplanner/autos/{Left Middy.auto => Left Bump.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/Left Corner Bite.auto rename src/main/deploy/pathplanner/autos/{Left Follower.auto => Left Follow.auto} (59%) rename src/main/deploy/pathplanner/autos/{Right Middy.auto => Right Bump.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/Right Corner Bite.auto rename src/main/deploy/pathplanner/autos/{Right Follower.auto => Right Follow.auto} (58%) create mode 100644 src/main/deploy/pathplanner/paths/Left Bite Score To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Left Corner Bite.path rename src/main/deploy/pathplanner/paths/{Left Follower To Bump.path => Left Follow To Bump.path} (85%) rename src/main/deploy/pathplanner/paths/{Left Follower Over Bump.path => Left Follow To Score.path} (75%) delete mode 100644 src/main/deploy/pathplanner/paths/Left Follower Bump To Corner.path create mode 100644 src/main/deploy/pathplanner/paths/Right Bite Score To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Right Corner Bite To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Right Corner Bite.path rename src/main/deploy/pathplanner/paths/{Right Follower To Bump.path => Right Follow To Bump.path} (86%) rename src/main/deploy/pathplanner/paths/{Right Follower Over Bump.path => Right Follow To Score.path} (71%) delete mode 100644 src/main/deploy/pathplanner/paths/Right Follower Bump To Corner.path rename src/main/java/com/stuypulse/robot/commands/auton/regular/{LeftMiddy.java => LeftBump.java} (96%) rename src/main/java/com/stuypulse/robot/commands/auton/regular/{LeftFollower.java => LeftFollow.java} (90%) create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java rename src/main/java/com/stuypulse/robot/commands/auton/regular/{RightMiddy.java => RightBump.java} (96%) rename src/main/java/com/stuypulse/robot/commands/auton/regular/{RightFollower.java => RightFollow.java} (90%) create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java diff --git a/elastic-layout.json b/elastic-layout.json index 8bda01ed..3da1d2af 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -180,7 +180,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Handoff/At Tolerance?", + "topic": "/Robot/Handoff/At Tolerance?", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -196,7 +196,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Handoff/State", + "topic": "/Robot/Handoff/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -210,7 +210,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "topic": "/Robot/Intake/Pivot At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -227,7 +227,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Pivot State", + "topic": "/Robot/Intake/Pivot State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -241,7 +241,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/State", + "topic": "/Robot/Superstructure/Hood/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -255,7 +255,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter/State", + "topic": "/Robot/Superstructure/Shooter/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -269,7 +269,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret/State", + "topic": "/Robot/Superstructure/Turret/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -283,7 +283,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "topic": "/Robot/Superstructure/Hood At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -300,7 +300,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "topic": "/Robot/Superstructure/Shooter At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -317,7 +317,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "topic": "/Robot/Superstructure/Turret At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -406,7 +406,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Spindexer/At Tolerance", + "topic": "/Robot/Spindexer/At Tolerance", "period": 0.5, "true_color": 4283215696, "false_color": 4294198070, @@ -422,7 +422,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Spindexer/State", + "topic": "/Robot/Spindexer/State", "period": 0.5, "data_type": "string", "show_submit_button": false @@ -702,7 +702,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Handoff/At Tolerance?", + "topic": "/Robot/Handoff/At Tolerance?", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -718,7 +718,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Handoff/State", + "topic": "/Robot/Handoff/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -732,7 +732,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "topic": "/Robot/Intake/Pivot At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -749,7 +749,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Pivot State", + "topic": "/Robot/Intake/Pivot State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -763,7 +763,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/State", + "topic": "/Robot/Superstructure/Hood/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -777,7 +777,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret/State", + "topic": "/Robot/Superstructure/Turret/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -791,7 +791,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "topic": "/Robot/Superstructure/Hood At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -808,7 +808,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "topic": "/Robot/Superstructure/Shooter At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -825,7 +825,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "topic": "/Robot/Superstructure/Turret At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -933,7 +933,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Spindexer/At Tolerance", + "topic": "/Robot/Spindexer/At Tolerance", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -949,7 +949,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Spindexer/State", + "topic": "/Robot/Spindexer/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -963,7 +963,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Current Angle (deg)", + "topic": "/Robot/Intake/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -992,7 +992,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", + "topic": "/Robot/Superstructure/Hood/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -1048,7 +1048,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter/State", + "topic": "/Robot/Superstructure/Shooter/State", "period": 0.5, "data_type": "string", "show_submit_button": false @@ -1288,7 +1288,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Handoff/At Tolerance?", + "topic": "/Robot/Handoff/At Tolerance?", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -1304,7 +1304,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Handoff/State", + "topic": "/Robot/Handoff/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1318,7 +1318,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "topic": "/Robot/Intake/Pivot At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1335,7 +1335,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Pivot State", + "topic": "/Robot/Intake/Pivot State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1349,7 +1349,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/State", + "topic": "/Robot/Superstructure/Hood/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1363,7 +1363,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter/State", + "topic": "/Robot/Superstructure/Shooter/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1377,7 +1377,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret/State", + "topic": "/Robot/Superstructure/Turret/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1391,7 +1391,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "topic": "/Robot/Superstructure/Hood At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1408,7 +1408,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "topic": "/Robot/Superstructure/Shooter At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1425,7 +1425,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "topic": "/Robot/Superstructure/Turret At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1442,7 +1442,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Spindexer/At Tolerance", + "topic": "/Robot/Spindexer/At Tolerance", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -1458,7 +1458,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Spindexer/State", + "topic": "/Robot/Spindexer/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1486,7 +1486,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret/Current Angle", + "topic": "/Robot/Superstructure/Turret/Current Angle", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -1528,7 +1528,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", + "topic": "/Robot/Superstructure/Hood/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -1667,7 +1667,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Current Angle (deg)", + "topic": "/Robot/Intake/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false diff --git a/src/main/deploy/pathplanner/autos/Depot Only.auto b/src/main/deploy/pathplanner/autos/Center Hub + Depot.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Depot Only.auto rename to src/main/deploy/pathplanner/autos/Center Hub + Depot.auto diff --git a/src/main/deploy/pathplanner/autos/Left Middy.auto b/src/main/deploy/pathplanner/autos/Left Bump.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Left Middy.auto rename to src/main/deploy/pathplanner/autos/Left Bump.auto diff --git a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto new file mode 100644 index 00000000..c24faff3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Corner Bite.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 Corner Bite To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Bite Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To NZ (F)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Follower.auto b/src/main/deploy/pathplanner/autos/Left Follow.auto similarity index 59% rename from src/main/deploy/pathplanner/autos/Left Follower.auto rename to src/main/deploy/pathplanner/autos/Left Follow.auto index 936b5e45..065f1d16 100644 --- a/src/main/deploy/pathplanner/autos/Left Follower.auto +++ b/src/main/deploy/pathplanner/autos/Left Follow.auto @@ -7,19 +7,13 @@ { "type": "path", "data": { - "pathName": "Left Follower To Bump" + "pathName": "Left Follow To Bump" } }, { "type": "path", "data": { - "pathName": "Left Follower Over Bump" - } - }, - { - "type": "path", - "data": { - "pathName": "Left Follower Bump To Corner" + "pathName": "Left Follow To Score" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right Middy.auto b/src/main/deploy/pathplanner/autos/Right Bump.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Right Middy.auto rename to src/main/deploy/pathplanner/autos/Right Bump.auto diff --git a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto new file mode 100644 index 00000000..382787b8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Corner Bite.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 Corner Bite To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Bite Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To NZ (F)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Score" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Follower.auto b/src/main/deploy/pathplanner/autos/Right Follow.auto similarity index 58% rename from src/main/deploy/pathplanner/autos/Right Follower.auto rename to src/main/deploy/pathplanner/autos/Right Follow.auto index 6071e1ed..a0ed2adc 100644 --- a/src/main/deploy/pathplanner/autos/Right Follower.auto +++ b/src/main/deploy/pathplanner/autos/Right Follow.auto @@ -7,19 +7,13 @@ { "type": "path", "data": { - "pathName": "Right Follower To Bump" + "pathName": "Right Follow To Bump" } }, { "type": "path", "data": { - "pathName": "Right Follower Over Bump" - } - }, - { - "type": "path", - "data": { - "pathName": "Right Follower Bump To Corner" + "pathName": "Right Follow To Bump" } } ] 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 new file mode 100644 index 00000000..0a057cd8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 7.894778887303849, + "y": 7.509029957203994 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "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.0057346647646215, + "y": 7.664293865905849 + }, + "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.9104477611940158, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.6183368869935886, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.0533049040511733, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.3176972281449895, + "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, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file 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 new file mode 100644 index 00000000..af859c9d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path @@ -0,0 +1,87 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.231184022824536, + "y": 4.882482168330956 + }, + "prevControl": null, + "nextControl": { + "x": 6.648308686286153, + "y": 3.901477029087409 + }, + "isLocked": false, + "linkedName": "Left NZ Corner" + }, + { + "anchor": { + "x": 6.125962127480992, + "y": 6.067266414244474 + }, + "prevControl": { + "x": 6.122182596291013, + "y": 4.183794579172611 + }, + "nextControl": { + "x": 6.129040098834367, + "y": 7.601126584153624 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.743238231098433, + "y": 7.6125392296718974 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2899786780383839, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 0.7249466950959463, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.117270788912574, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.518123667377378, + "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, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite.path b/src/main/deploy/pathplanner/paths/Left Corner Bite.path new file mode 100644 index 00000000..40db3d61 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 6.613851640513552, + "y": 7.625477888730385 + }, + "isLocked": false, + "linkedName": "Left Trench Start" + }, + { + "anchor": { + "x": 8.231184022824536, + "y": 4.882482168330956 + }, + "prevControl": { + "x": 7.334057616386011, + "y": 7.394914974879136 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left NZ Corner" + } + ], + "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.6022957461174885, + "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": "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 Follower To Bump.path b/src/main/deploy/pathplanner/paths/Left Follow To Bump.path similarity index 85% rename from src/main/deploy/pathplanner/paths/Left Follower To Bump.path rename to src/main/deploy/pathplanner/paths/Left Follow To Bump.path index b677cf1b..1fa270e3 100644 --- a/src/main/deploy/pathplanner/paths/Left Follower To Bump.path +++ b/src/main/deploy/pathplanner/paths/Left Follow To Bump.path @@ -41,7 +41,7 @@ }, "nextControl": { "x": 10.395817994061506, - "y": 1.4573202864572623 + "y": 1.4573202864572625 }, "isLocked": false, "linkedName": null @@ -52,24 +52,24 @@ "y": 2.7605420827389446 }, "prevControl": { - "x": 8.38644793152639, - "y": 1.130271041369473 + "x": 8.439827858477573, + "y": 1.1277576859085485 }, "nextControl": { - "x": 8.676070675703448, - "y": 5.691829262158123 + "x": 8.580527817403707, + "y": 5.71055634807418 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.850470756062768, - "y": 5.503537803138374 + "x": 8.489957203994294, + "y": 7.4572753209700435 }, "prevControl": { - "x": 8.502895863052782, - "y": 5.6070470756062765 + "x": 8.489957203994294, + "y": 5.697617689015693 }, "nextControl": null, "isLocked": false, @@ -94,11 +94,11 @@ "rotationDegrees": 90.0 }, { - "waypointRelativePos": 3.232409381663122, + "waypointRelativePos": 3.5650319829424046, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 3.667377398720684, + "waypointRelativePos": 3.8379530916844278, "rotationDegrees": 180.0 } ], @@ -118,7 +118,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Follower", + "folder": "Follow", "idealStartingState": { "velocity": 0, "rotation": -90.0 diff --git a/src/main/deploy/pathplanner/paths/Left Follower Over Bump.path b/src/main/deploy/pathplanner/paths/Left Follow To Score.path similarity index 75% rename from src/main/deploy/pathplanner/paths/Left Follower Over Bump.path rename to src/main/deploy/pathplanner/paths/Left Follow To Score.path index 0505c62d..6c78c243 100644 --- a/src/main/deploy/pathplanner/paths/Left Follower Over Bump.path +++ b/src/main/deploy/pathplanner/paths/Left Follow To Score.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.850470756062768, - "y": 5.503537803138374 + "x": 8.489957203994294, + "y": 7.4572753209700435 }, "prevControl": null, "nextControl": { - "x": 5.268338214575557, - "y": 5.5146770668110285 + "x": 7.907824662507084, + "y": 7.468414584642698 }, "isLocked": false, "linkedName": "Left Follower Bump" }, { "anchor": { - "x": 2.9651497860199716, - "y": 5.477660485021398 + "x": 1.1666761768901568, + "y": 7.4572753209700435 }, "prevControl": { - "x": 4.136040206997414, - "y": 5.467941169189516 + "x": 2.3375665978676, + "y": 7.4475560051381615 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Follower", + "folder": "Follow", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Left Follower Bump To Corner.path b/src/main/deploy/pathplanner/paths/Left Follower Bump To Corner.path deleted file mode 100644 index ac1f8961..00000000 --- a/src/main/deploy/pathplanner/paths/Left Follower Bump To Corner.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.9651497860199716, - "y": 5.477660485021398 - }, - "prevControl": null, - "nextControl": { - "x": 1.9849260565692943, - "y": 3.8087102769799372 - }, - "isLocked": false, - "linkedName": "Left Follower Bump Post" - }, - { - "anchor": { - "x": 1.9171184022824534, - "y": 3.7180028530670475 - }, - "prevControl": { - "x": 2.1240710000936103, - "y": 4.068157127189754 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Left Corner" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "Follower", - "idealStartingState": { - "velocity": 0, - "rotation": 180.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 52c62217..88a55f71 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -36,7 +36,7 @@ "y": 7.440906488549619 }, "prevControl": { - "x": 6.018673323823109, + "x": 6.01867332382311, "y": 7.444336661911555 }, "nextControl": null, 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 8a3f8436..c87bcf66 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 6.717360912981454, + "x": 6.717360912981455, "y": 7.521968616262482 }, "isLocked": false, @@ -68,7 +68,7 @@ "y": 7.440906488549619 }, "prevControl": { - "x": 5.294108416547789, + "x": 5.29410841654779, "y": 7.399604078672524 }, "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 new file mode 100644 index 00000000..d2bc81c6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": null, + "nextControl": { + "x": 5.565820256776034, + "y": 0.5997860199714706 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "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.018673323823109, + "y": 3.174579172610556 + }, + "nextControl": { + "x": 6.053793108756849, + "y": 0.22451723817625546 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 6.1480599144079875, + "y": 0.5739087018544944 + }, + "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": [], + "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/Right Corner Bite To Score.path b/src/main/deploy/pathplanner/paths/Right Corner Bite To Score.path new file mode 100644 index 00000000..0a000990 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite To Score.path @@ -0,0 +1,87 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.244122681883024, + "y": 3.174579172610556 + }, + "prevControl": null, + "nextControl": { + "x": 7.525958036561901, + "y": 4.151225380451245 + }, + "isLocked": false, + "linkedName": "Right NZ Corner" + }, + { + "anchor": { + "x": 6.1480599144079875, + "y": 2.372382310984309 + }, + "prevControl": { + "x": 6.070427960057061, + "y": 4.377874465049929 + }, + "nextControl": { + "x": 6.231289621491587, + "y": 0.2222815446579549 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 6.62679029957204, + "y": 0.5739087018544944 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4179104477611916, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 0.7889125799573445, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.181236673773988, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.5820895522387919, + "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, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite.path b/src/main/deploy/pathplanner/paths/Right Corner Bite.path new file mode 100644 index 00000000..7b8bd169 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 7.0667047075606275, + "y": 0.4315834522111266 + }, + "isLocked": false, + "linkedName": "Right Trench Start" + }, + { + "anchor": { + "x": 8.244122681883024, + "y": 3.174579172610556 + }, + "prevControl": { + "x": 7.694160604322244, + "y": 1.874021386157379 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right NZ Corner" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2665245202558647, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.8102345415778156, + "rotationDegrees": 55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.4780553679945946, + "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": "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 Follower To Bump.path b/src/main/deploy/pathplanner/paths/Right Follow To Bump.path similarity index 86% rename from src/main/deploy/pathplanner/paths/Right Follower To Bump.path rename to src/main/deploy/pathplanner/paths/Right Follow To Bump.path index 5215aa48..aa7bda16 100644 --- a/src/main/deploy/pathplanner/paths/Right Follower To Bump.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Bump.path @@ -52,24 +52,24 @@ "y": 2.838174037089873 }, "prevControl": { - "x": 8.55912562999586, - "y": 5.126464291255471 + "x": 8.670199738246035, + "y": 5.126691502098343 }, "nextControl": { - "x": 8.632282453637663, - "y": 2.3465049928673345 + "x": 8.606405135520683, + "y": 2.2559343794579183 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.018673323823109, - "y": 2.501768901569187 + "x": 8.619343794579173, + "y": 0.6256633380884444 }, "prevControl": { - "x": 6.976134094151213, - "y": 2.4888302425106996 + "x": 8.606405135520685, + "y": 2.048915834522113 }, "nextControl": null, "isLocked": false, @@ -94,11 +94,11 @@ "rotationDegrees": -90.0 }, { - "waypointRelativePos": 2.874200426439231, + "waypointRelativePos": 3.445628997867779, "rotationDegrees": -90.0 }, { - "waypointRelativePos": 3.3859275053305025, + "waypointRelativePos": 3.8550106609807893, "rotationDegrees": 180.0 } ], @@ -118,7 +118,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Follower", + "folder": "Follow", "idealStartingState": { "velocity": 0, "rotation": 90.0 diff --git a/src/main/deploy/pathplanner/paths/Right Follower Over Bump.path b/src/main/deploy/pathplanner/paths/Right Follow To Score.path similarity index 71% rename from src/main/deploy/pathplanner/paths/Right Follower Over Bump.path rename to src/main/deploy/pathplanner/paths/Right Follow To Score.path index 33c9e281..d6551a28 100644 --- a/src/main/deploy/pathplanner/paths/Right Follower Over Bump.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Score.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 6.018673323823109, - "y": 2.501768901569187 + "x": 8.619343794579173, + "y": 0.6256633380884444 }, "prevControl": null, "nextControl": { - "x": 4.70357217256972, - "y": 2.5005480781874923 + "x": 7.062214424080558, + "y": 0.6304982186835882 }, "isLocked": false, "linkedName": "Right Follower Bump" }, { "anchor": { - "x": 2.9263338088445074, - "y": 2.514707560627676 + "x": 1.2313694721825963, + "y": 0.6256633380884444 }, "prevControl": { - "x": 4.156424440701705, - "y": 2.4750602578513616 + "x": 3.249209389945311, + "y": 0.6251768362147326 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Follower Bump Post" + "linkedName": "Right Corner" } ], "rotationTargets": [], @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Follower", + "folder": "Follow", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Right Follower Bump To Corner.path b/src/main/deploy/pathplanner/paths/Right Follower Bump To Corner.path deleted file mode 100644 index ffa5d749..00000000 --- a/src/main/deploy/pathplanner/paths/Right Follower Bump To Corner.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.9263338088445074, - "y": 2.514707560627676 - }, - "prevControl": null, - "nextControl": { - "x": 0.9596576319543506, - "y": 2.320627674750357 - }, - "isLocked": false, - "linkedName": "Right Follower Bump Post" - }, - { - "anchor": { - "x": 1.3219400855920111, - "y": 0.8844365192582018 - }, - "prevControl": { - "x": 1.4383880171184018, - "y": 1.3114122681883031 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Right Corner" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "Follower", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 8feeed13..551c8ead 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,7 +4,7 @@ "holonomicMode": true, "pathFolders": [ "Bump Stuff", - "Follower", + "Follow", "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 e0e3be82..6e0ea40f 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -8,11 +8,13 @@ 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.LeftFollower; -import com.stuypulse.robot.commands.auton.regular.LeftMiddy; +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.LeftTwoCycle; -import com.stuypulse.robot.commands.auton.regular.RightFollower; -import com.stuypulse.robot.commands.auton.regular.RightMiddy; +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.RightTwoCycle; import com.stuypulse.robot.commands.auton.test.BoxTest; import com.stuypulse.robot.commands.handoff.HandoffReverse; @@ -365,19 +367,23 @@ private void configureElasticButtons() { /*** AUTONS ***/ /**************/ + public void configureAutons() { + autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); // DEPOT AutonConfig DEPOT_ONLY = new AutonConfig("Depot Only", Depot::new, "Center Hub To Depot"); DEPOT_ONLY.register(autonChooser); - AutonConfig LEFT_MIDDY = new AutonConfig("Left Middy", LeftMiddy::new, + + AutonConfig LEFT_BUMP = new AutonConfig("Left Bump", LeftBump::new, "Left Bump To Score (Start)", "Left Bump To Score", "Left Bump Score To Depot"); - LEFT_MIDDY.register(autonChooser); - AutonConfig RIGHT_MIDDY = new AutonConfig("Right Middy", RightMiddy::new, + LEFT_BUMP.register(autonChooser); + + AutonConfig RIGHT_BUMP = new AutonConfig("Right Bump", RightBump::new, "Right Bump To Score (Start)", "Right Bump To Score", "Right Bump Score To Depot"); - RIGHT_MIDDY.register(autonChooser); + RIGHT_BUMP.register(autonChooser); // TWO CYCLES (TRENCH) AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, @@ -388,16 +394,26 @@ public void configureAutons() { "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Score To NZ (F)", "Right NZ To Score"); RIGHT_TWO_CYCLE.register(autonChooser); - // FOLLOWERS - AutonConfig LEFT_FOLLOWER = new AutonConfig("Left Follower", LeftFollower::new, prevWaitTime, - "Left Follower To Bump", "Left Follower Over Bump", "Left Follower Bump To Corner"); - LEFT_FOLLOWER.register(autonChooser); + // TWO CYCLES (CORNER) + AutonConfig LEFT_TWO_CORNER = new AutonConfig("Left Two Corner", LeftTwoCorner::new, + "Left Corner Bite", "Left Corner Bite To Score", "Left Bite Score To Score", "Left Score To NZ (F)", "Left NZ To Score"); + LEFT_TWO_CORNER.register(autonChooser); - AutonConfig RIGHT_FOLLOWER = new AutonConfig("Right Follower", RightFollower::new, prevWaitTime, - "Right Follower To Bump", "Right Follower Over Bump", "Right Follower Bump To Corner"); - RIGHT_FOLLOWER.register(autonChooser); + AutonConfig RIGHT_TWO_CORNER = new AutonConfig("Right Two Corner", RightTwoCorner::new, + "Right Corner Bite", "Right Corner Bite To Score", "Right Bite Score To Score", "Right Score To NZ (F)", "Right NZ To Score"); + RIGHT_TWO_CORNER.register(autonChooser); + + // FOLLOWS + AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTime, + "Left Follow To Bump", "Left Follow To Score"); + LEFT_FOLLOW.register(autonChooser); + + AutonConfig RIGHT_FOLLOW = new AutonConfig("Right Follow", RightFollow::new, prevWaitTime, + "Right Follow To Bump", "Right Follow To Score"); + RIGHT_FOLLOW.register(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); + } public boolean hasWaitTimeChanged() { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java similarity index 96% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java index dc8240cb..99c797dd 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java @@ -19,9 +19,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class LeftMiddy extends SequentialCommandGroup { +public class LeftBump extends SequentialCommandGroup { - public LeftMiddy(PathPlannerPath... paths) { + public LeftBump(PathPlannerPath... paths) { addCommands( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollower.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java similarity index 90% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollower.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java index 29a40f5d..3f8c4faf 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollower.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java @@ -20,9 +20,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class LeftFollower extends SequentialCommandGroup { +public class LeftFollow extends SequentialCommandGroup { - public LeftFollower(PathPlannerPath... paths) { + public LeftFollow(PathPlannerPath... paths) { addCommands( @@ -56,8 +56,7 @@ public LeftFollower(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( new HandoffRun(), - new SpindexerRun(), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]) + new SpindexerRun() ) ); 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 new file mode 100644 index 00000000..c3d9d127 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java @@ -0,0 +1,87 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.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.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class LeftTwoCorner extends SequentialCommandGroup { + + public LeftTwoCorner(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.5).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new HandoffRun().andThen( + new SpindexerRun() + ).andThen(new WaitCommand(0.5) + .andThen(new IntakeAutoDigest()).repeatedly()).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 HandoffRun().andThen( + new SpindexerRun() + ).andThen(new WaitCommand(0.5) + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffStop(), + new SpindexerStop() + ) + + // new SuperstructureSOTM(), + // new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + // new HandoffRun().andThen( + // new SpindexerRun() + // ).andThen(new WaitCommand(2.5) + // .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), + // new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index d2ca9149..307c1e90 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 @@ -47,7 +47,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().andThen( new SpindexerRun() - ).andThen(new WaitCommand(1.75) + ).andThen(new WaitCommand(0.5) .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -62,7 +62,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().andThen( new SpindexerRun() - ).andThen(new WaitCommand(1.75) + ).andThen(new WaitCommand(0.5) .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightBump.java similarity index 96% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/RightBump.java index e0be98b3..8a64f16e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightBump.java @@ -19,9 +19,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class RightMiddy extends SequentialCommandGroup { +public class RightBump extends SequentialCommandGroup { - public RightMiddy(PathPlannerPath... paths) { + public RightBump(PathPlannerPath... paths) { addCommands( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollower.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java similarity index 90% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollower.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java index 8b17b1aa..0cacd4be 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollower.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java @@ -20,9 +20,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class RightFollower extends SequentialCommandGroup { +public class RightFollow extends SequentialCommandGroup { - public RightFollower(PathPlannerPath... paths) { + public RightFollow(PathPlannerPath... paths) { addCommands( @@ -56,8 +56,7 @@ public RightFollower(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( new HandoffRun(), - new SpindexerRun(), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]) + new SpindexerRun() ) ); 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 new file mode 100644 index 00000000..c8890b4f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java @@ -0,0 +1,87 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.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.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class RightTwoCorner extends SequentialCommandGroup { + + public RightTwoCorner(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.5).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new HandoffRun().andThen( + new SpindexerRun() + ).andThen(new WaitCommand(0.5) + .andThen(new IntakeAutoDigest()).repeatedly()).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 HandoffRun().andThen( + new SpindexerRun() + ).andThen(new WaitCommand(0.5) + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffStop(), + new SpindexerStop() + ) + + // new SuperstructureSOTM(), + // new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + // new HandoffRun().andThen( + // new SpindexerRun() + // ).andThen(new WaitCommand(2.5) + // .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), + // new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index 2224300c..7954d264 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 @@ -46,7 +46,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().andThen( new SpindexerRun() - ).andThen(new WaitCommand(1.75) + ).andThen(new WaitCommand(0.5) .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -61,7 +61,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().andThen( new SpindexerRun() - ).andThen(new WaitCommand(1.75) + ).andThen(new WaitCommand(0.5) .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), 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 c9b75562..fed4c0f6 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -8,6 +8,8 @@ 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.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Swerve; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -27,6 +29,10 @@ import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; public class SwerveDriveSOTM extends Command { @@ -39,11 +45,13 @@ public class SwerveDriveSOTM extends Command { private final IStream turn; private final BStream isIdle; + private final Trigger isIdleTrigger; public SwerveDriveSOTM(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); superstructure = Superstructure.getInstance(); + speed = VStream.create(this::getDriverInputAsVelocity) .filtered( new VDeadZone(Drive.DEADBAND), @@ -67,8 +75,14 @@ public SwerveDriveSOTM(Gamepad driver) { .filtered(new BDebounce.Rising(0.5), new BDebounce.Falling(0.1)); this.driver = driver; - - addRequirements(swerve); + isIdleTrigger = new Trigger(() -> isIdle.get()); + isIdleTrigger + .onTrue(new IntakeAutoDigest().repeatedly()) + .onTrue(new SwerveXMode()) + .onFalse(new IntakeDeploy()) + .onFalse(new SwerveDriveDrive(driver)); + + addRequirements(swerve); } private Vector2D getDriverInputAsVelocity() { @@ -77,18 +91,18 @@ private Vector2D getDriverInputAsVelocity() { @Override public void execute() { - if (isIdle.get()) { - swerve.setControl(new SwerveRequest.SwerveDriveBrake()); - } else { - Vector2D velocity = speed.get(); + DogLog.log("Swerve/SOTM/Idle?", isIdle.get()); - swerve.setControl(swerve.getFieldCentricSwerveRequest() - .withVelocityX(velocity.x) - .withVelocityY(velocity.y) - .withRotationalRate(-turn.get())); + if (isIdle.get()) { + return; } + Vector2D velocity = speed.get(); + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(velocity.x) + .withVelocityY(velocity.y) + .withRotationalRate(-turn.get())); + - DogLog.log("Swerve/SOTM/Idle?", isIdle.get()); } @Override diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 1d6bff50..4858219b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -160,18 +160,19 @@ public interface TOFInterpolation{ public interface FerryRPMInterpolation { double[][] ferryDistanceRPMInterpolation = { + //Lab {1, 2000}, {5.16, 3300.0}, - {6.94, 3600.0}, - {7.87, 3800.0}, - {9.77, 4300.0}, + {6.94, 3400}, + {9.77, 3400}, //TODO: USE THE BELOW THREE LINES INTEAD OF THESE + // {6.94, 3600.0}, + // {7.87, 3800.0}, + // {9.77, 4300.0}, {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS UNRELIABLE!!! {11.516, 4900.0}, {12.416, 5200.0}, {13.316, 5500.0}, {14.216, 5600.0} - // {15.148, 5200.0}, - // {16.54, 5300} //FIELD LENGTH }; } From dcac3b7230a4d16ff263a60f0a8d9e2cc64bc2b6 Mon Sep 17 00:00:00 2001 From: Apetrock Date: Mon, 20 Apr 2026 18:37:05 -0400 Subject: [PATCH 17/33] ADD: Swerve drive digest (untested) --- .../commands/swerve/SwerveDriveSOTM.java | 30 ++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) 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 fed4c0f6..9a1a9482 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -28,6 +28,7 @@ import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; import dev.doglog.DogLog; +import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -45,7 +46,7 @@ public class SwerveDriveSOTM extends Command { private final IStream turn; private final BStream isIdle; - private final Trigger isIdleTrigger; + private boolean isIdleInit; public SwerveDriveSOTM(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); @@ -75,12 +76,7 @@ public SwerveDriveSOTM(Gamepad driver) { .filtered(new BDebounce.Rising(0.5), new BDebounce.Falling(0.1)); this.driver = driver; - isIdleTrigger = new Trigger(() -> isIdle.get()); - isIdleTrigger - .onTrue(new IntakeAutoDigest().repeatedly()) - .onTrue(new SwerveXMode()) - .onFalse(new IntakeDeploy()) - .onFalse(new SwerveDriveDrive(driver)); + isIdleInit = false; addRequirements(swerve); } @@ -94,14 +90,19 @@ public void execute() { DogLog.log("Swerve/SOTM/Idle?", isIdle.get()); if (isIdle.get()) { - return; + if (!isIdleInit) { + 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; } - Vector2D velocity = speed.get(); - swerve.setControl(swerve.getFieldCentricSwerveRequest() - .withVelocityX(velocity.x) - .withVelocityY(velocity.y) - .withRotationalRate(-turn.get())); - } @@ -114,4 +115,5 @@ public boolean isFinished() { return true; } } + } From de942c2acec8c232ae29de1029b2fc88ebfa328b Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 20 Apr 2026 19:40:03 -0400 Subject: [PATCH 18/33] feat: final touches on autons, more logging on cameras, limit ferrying in the lab (PLEASE PLEASE PLEASE REVERT FERRYING CHANGES FOR CHAMPS TY) --- elastic-layout.json | 34 ++++++--- .../pathplanner/autos/Right Corner Bite.auto | 2 +- .../pathplanner/autos/Right Follow.auto | 2 +- .../paths/Left Corner Bite To Score.path | 4 +- .../pathplanner/paths/Left Corner Bite.path | 12 +-- .../paths/Left Follow To Bump.path | 76 ++++++++++++------- .../paths/Right Bite Score To Score.path | 4 +- .../paths/Right Corner Bite To Score.path | 10 +-- .../pathplanner/paths/Right Corner Bite.path | 12 +-- .../paths/Right Follow To Bump.path | 74 +++++++++++------- .../paths/Right Follow To Score.path | 8 +- src/main/java/com/stuypulse/robot/Robot.java | 2 +- .../com/stuypulse/robot/RobotContainer.java | 54 ++++++++----- .../commands/auton/regular/LeftFollow.java | 12 +-- .../commands/auton/regular/LeftTwoCorner.java | 6 +- .../commands/auton/regular/LeftTwoCycle.java | 1 + .../commands/auton/regular/RightFollow.java | 12 +-- .../auton/regular/RightTwoCorner.java | 6 +- .../commands/swerve/SwerveDriveSOTM.java | 2 - .../stuypulse/robot/constants/Cameras.java | 9 ++- .../stuypulse/robot/constants/Settings.java | 12 +-- .../com/stuypulse/robot/util/PathUtil.java | 14 ++-- 22 files changed, 219 insertions(+), 149 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 3da1d2af..94c5295c 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -362,9 +362,8 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/FMSUtil/time left in shift", + "topic": "/Robot/FMSUtil/time left in shift", "period": 0.06, - "data_type": "double", "time_display_mode": "Minutes and Seconds", "red_start_time": 20, "yellow_start_time": 30 @@ -436,7 +435,7 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Robot/Match Time", + "topic": "/Robot/Match Time", "period": 0.5, "time_display_mode": "Minutes and Seconds", "red_start_time": 20, @@ -508,14 +507,28 @@ } }, { - "title": "Wait Time", + "title": "Wait Time 2", + "x": 980.0, + "y": 560.0, + "width": 140.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "SmartDashboard/Robot/Auton/Wait Time 2", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Wait Time 1", "x": 840.0, "y": 560.0, "width": 140.0, "height": 140.0, "type": "Text Display", "properties": { - "topic": "SmartDashboard/Robot/Auton/Wait Time", + "topic": "SmartDashboard/Robot/Auton/Wait Time 1", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -842,7 +855,7 @@ "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/is active shift", + "topic": "/Robot/is active shift", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -858,7 +871,7 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/FMSUtil/time left in shift", + "topic": "/Robot/FMSUtil/time left in shift", "period": 0.06, "data_type": "double", "time_display_mode": "Minutes and Seconds", @@ -874,9 +887,8 @@ "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/FMSUtil/No Auto Winner Data", + "topic": "/Robot/FMSUtil/No Auto Winner Data", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -977,7 +989,7 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Robot/Match Time", + "topic": "/Robot/Match Time", "period": 0.5, "time_display_mode": "Minutes and Seconds", "red_start_time": 20, @@ -1103,7 +1115,7 @@ "height": 140.0, "type": "Large Text Display", "properties": { - "topic": "/SmartDashboard/FMSUtil/Field State", + "topic": "/Robot/FMSUtil/Field State", "period": 0.06 } } diff --git a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto index 382787b8..2cc4ce28 100644 --- a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto @@ -13,7 +13,7 @@ { "type": "path", "data": { - "pathName": "Right Corner Bite To Score" + "pathName": "Right NZ To Score" } }, { diff --git a/src/main/deploy/pathplanner/autos/Right Follow.auto b/src/main/deploy/pathplanner/autos/Right Follow.auto index a0ed2adc..4857d51d 100644 --- a/src/main/deploy/pathplanner/autos/Right Follow.auto +++ b/src/main/deploy/pathplanner/autos/Right Follow.auto @@ -13,7 +13,7 @@ { "type": "path", "data": { - "pathName": "Right Follow To Bump" + "pathName": "Right Follow To Score" } } ] 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 af859c9d..c91331b9 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 @@ -4,12 +4,12 @@ { "anchor": { "x": 8.231184022824536, - "y": 4.882482168330956 + "y": 5.257703281027104 }, "prevControl": null, "nextControl": { "x": 6.648308686286153, - "y": 3.901477029087409 + "y": 4.276698141783557 }, "isLocked": false, "linkedName": "Left NZ Corner" diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite.path b/src/main/deploy/pathplanner/paths/Left Corner Bite.path index 40db3d61..9ec5ab2a 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.613851640513552, - "y": 7.625477888730385 + "x": 6.639728958630526, + "y": 7.677232524964337 }, "isLocked": false, "linkedName": "Left Trench Start" @@ -17,11 +17,11 @@ { "anchor": { "x": 8.231184022824536, - "y": 4.882482168330956 + "y": 5.257703281027104 }, "prevControl": { - "x": 7.334057616386011, - "y": 7.394914974879136 + "x": 6.782054208273893, + "y": 7.276134094151212 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.6022957461174885, + "minWaypointRelativePos": 0.6212018906144481, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 1.5, diff --git a/src/main/deploy/pathplanner/paths/Left Follow To Bump.path b/src/main/deploy/pathplanner/paths/Left Follow To Bump.path index 1fa270e3..89e92769 100644 --- a/src/main/deploy/pathplanner/paths/Left Follow To Bump.path +++ b/src/main/deploy/pathplanner/paths/Left Follow To Bump.path @@ -3,61 +3,61 @@ "waypoints": [ { "anchor": { - "x": 3.534450784593438, - "y": 7.677232524964337 + "x": 3.63796005706134, + "y": 7.586661911554922 }, "prevControl": null, "nextControl": { - "x": 6.264507845934379, - "y": 7.716048502139801 + "x": 7.0796433666191145, + "y": 7.53490727532097 }, "isLocked": false, "linkedName": "Left Follower Start" }, { "anchor": { - "x": 10.404878744650498, - "y": 5.9952068473609135 + "x": 8.489957203994294, + "y": 5.956390870185448 }, "prevControl": { - "x": 10.404878744650498, - "y": 7.573723252496435 + "x": 8.489957203994294, + "y": 7.534907275320969 }, "nextControl": { - "x": 10.404878744650498, - "y": 4.583409998537509 + "x": 8.489957203994294, + "y": 4.544594021362044 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 10.404878744650498, - "y": 2.191241084165479 + "x": 8.489957203994294, + "y": 2.126547788873039 }, "prevControl": { - "x": 10.430756062767475, - "y": 4.287303851640513 + "x": 8.515834522111271, + "y": 4.222610556348073 }, "nextControl": { - "x": 10.395817994061506, - "y": 1.4573202864572625 + "x": 8.480896453405302, + "y": 1.3926269911648226 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.489957203994294, - "y": 2.7605420827389446 + "x": 7.260784593437946, + "y": 2.644094151212554 }, "prevControl": { - "x": 8.439827858477573, - "y": 1.1277576859085485 + "x": 7.210655247921225, + "y": 1.0113097543821579 }, "nextControl": { - "x": 8.580527817403707, - "y": 5.71055634807418 + "x": 7.3513552068473595, + "y": 5.59410841654779 }, "isLocked": false, "linkedName": null @@ -68,8 +68,8 @@ "y": 7.4572753209700435 }, "prevControl": { - "x": 8.489957203994294, - "y": 5.697617689015693 + "x": 6.976134094151213, + "y": 7.625477888730385 }, "nextControl": null, "isLocked": false, @@ -78,7 +78,11 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.5884861407249452, + "waypointRelativePos": 0.40085287846482387, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.742004264392322, "rotationDegrees": -90.0 }, { @@ -86,11 +90,11 @@ "rotationDegrees": -90.0 }, { - "waypointRelativePos": 2.4051172707889257, + "waypointRelativePos": 2.2004264392324204, "rotationDegrees": 180.0 }, { - "waypointRelativePos": 2.908315565031985, + "waypointRelativePos": 3.0191897654584188, "rotationDegrees": 90.0 }, { @@ -102,7 +106,21 @@ "rotationDegrees": 180.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7778528021606868, + "maxWaypointRelativePos": 3.6623902768399907, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -121,7 +139,7 @@ "folder": "Follow", "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path index d2bc81c6..2a778a76 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.053793108756849, - "y": 0.22451723817625546 + "y": 0.22451723817625524 }, "isLocked": false, "linkedName": null @@ -73,7 +73,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Right Trench Score" + "linkedName": null } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite To Score.path b/src/main/deploy/pathplanner/paths/Right Corner Bite To Score.path index 0a000990..c533a95b 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner Bite To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.244122681883024, - "y": 3.174579172610556 + "x": 8.295877318116975, + "y": 3.45922967189729 }, "prevControl": null, "nextControl": { - "x": 7.525958036561901, - "y": 4.151225380451245 + "x": 7.577712672795853, + "y": 4.435875879737979 }, "isLocked": false, "linkedName": "Right NZ Corner" @@ -41,7 +41,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Right Trench Score" + "linkedName": null } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite.path b/src/main/deploy/pathplanner/paths/Right Corner Bite.path index 7b8bd169..b8e70915 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 8.244122681883024, - "y": 3.174579172610556 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": { - "x": 7.694160604322244, - "y": 1.874021386157379 + "x": 7.687760342368046, + "y": 2.1265477888730384 }, "nextControl": null, "isLocked": false, - "linkedName": "Right NZ Corner" + "linkedName": "Right NZ" } ], "rotationTargets": [ @@ -45,7 +45,7 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.4780553679945946, + "minWaypointRelativePos": 0.5077650236326793, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 1.5, diff --git a/src/main/deploy/pathplanner/paths/Right Follow To Bump.path b/src/main/deploy/pathplanner/paths/Right Follow To Bump.path index aa7bda16..e3194c4f 100644 --- a/src/main/deploy/pathplanner/paths/Right Follow To Bump.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Bump.path @@ -3,73 +3,73 @@ "waypoints": [ { "anchor": { - "x": 3.4826961483594863, - "y": 0.4315834522111266 + "x": 3.6767760342368048, + "y": 0.48333808844507875 }, "prevControl": null, "nextControl": { - "x": 5.876348074179743, - "y": 0.36689015691868687 + "x": 6.1351212553495, + "y": 0.6256633380884444 }, "isLocked": false, "linkedName": "Right Follower Start" }, { "anchor": { - "x": 10.469572039942939, - "y": 1.6090014265335242 + "x": 8.282938659058486, + "y": 1.4666761768901564 }, "prevControl": { - "x": 10.452937586581934, - "y": 0.6386583138082187 + "x": 8.282938659058486, + "y": 0.12105563480741921 }, "nextControl": { - "x": 10.547203994293866, - "y": 6.13753209700428 + "x": 8.282938659058486, + "y": 3.6299100829375885 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 9.395663338088445, - "y": 6.551569186875891 + "x": 7.674821683309556, + "y": 4.533138373751783 }, "prevControl": { - "x": 10.598737663708622, - "y": 6.525127992906218 + "x": 8.68395914376938, + "y": 4.515118061957858 }, "nextControl": { - "x": 8.218245363766048, - "y": 6.577446504992866 + "x": 6.9502567760342355, + "y": 4.546077032810271 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.619343794579173, - "y": 2.838174037089873 + "x": 7.273723252496433, + "y": 2.5794008559201154 }, "prevControl": { - "x": 8.670199738246035, - "y": 5.126691502098343 + "x": 7.325477888730384, + "y": 3.135763195435093 }, "nextControl": { - "x": 8.606405135520683, - "y": 2.2559343794579183 + "x": 7.219780939723599, + "y": 1.999520993612133 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.619343794579173, - "y": 0.6256633380884444 + "x": 8.282938659058486, + "y": 0.5868473609129818 }, "prevControl": { - "x": 8.606405135520685, - "y": 2.048915834522113 + "x": 7.5066191155492135, + "y": 0.4057061340941528 }, "nextControl": null, "isLocked": false, @@ -77,6 +77,10 @@ } ], "rotationTargets": [ + { + "waypointRelativePos": 0.4690831556503221, + "rotationDegrees": 0.0 + }, { "waypointRelativePos": 1.0234541577825158, "rotationDegrees": 90.0 @@ -102,7 +106,21 @@ "rotationDegrees": 180.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.961512491559757, + "maxWaypointRelativePos": 3.500337609723156, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -121,7 +139,7 @@ "folder": "Follow", "idealStartingState": { "velocity": 0, - "rotation": 90.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Follow To Score.path b/src/main/deploy/pathplanner/paths/Right Follow To Score.path index d6551a28..4355d0c5 100644 --- a/src/main/deploy/pathplanner/paths/Right Follow To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.619343794579173, - "y": 0.6256633380884444 + "x": 8.282938659058486, + "y": 0.5868473609129818 }, "prevControl": null, "nextControl": { - "x": 7.062214424080558, - "y": 0.6304982186835882 + "x": 6.725809288559871, + "y": 0.5916822415081257 }, "isLocked": false, "linkedName": "Right Follower Bump" diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index c68a135e..72ead732 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -204,7 +204,7 @@ public void disabledPeriodic() { } } - if (periodicCounter % 50 == 0 && robot.hasWaitTimeChanged()) { + if ((periodicCounter % 50 == 0 && robot.hasWaitTimeOneChanged()) || (periodicCounter % 50 == 0 && robot.hasWaitTimeTwoChanged())) { robot.configureAutons(); } } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 6e0ea40f..89207b94 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -135,9 +135,12 @@ public interface EnabledSubsystems { // Autons private static SendableChooser autonChooser = new SendableChooser<>(); - private static SmartNumber waitTime = new SmartNumber("Robot/Auton/Wait Time", 0.0); - private static double prevWaitTime = 0.0; - private static boolean hasWaitTimeChanged = false; + private static SmartNumber waitTimeOne = new SmartNumber("Robot/Auton/Wait Time 1", 0.0); + private static SmartNumber waitTimeTwo = new SmartNumber("Robot/Auton/Wait Time 2", 0.0); + private static double prevWaitTimeOne = 0.0; + private static double prevWaitTimeTwo = 0.0; + private static boolean hasWaitTimeOneChanged = false; + private static boolean hasWaitTimeTwoChanged = false; // Robot container public RobotContainer() { @@ -373,42 +376,42 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); // DEPOT - AutonConfig DEPOT_ONLY = new AutonConfig("Depot Only", Depot::new, + 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, + 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, + 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, + 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 NZ (F)", "Left NZ To Score"); LEFT_TWO_CYCLE.register(autonChooser); - AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, + 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 NZ (F)", "Right NZ To Score"); RIGHT_TWO_CYCLE.register(autonChooser); // TWO CYCLES (CORNER) - AutonConfig LEFT_TWO_CORNER = new AutonConfig("Left Two Corner", LeftTwoCorner::new, - "Left Corner Bite", "Left Corner Bite To Score", "Left Bite Score To Score", "Left Score To NZ (F)", "Left NZ To Score"); + 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 NZ (F)", "Left NZ To Score"); LEFT_TWO_CORNER.register(autonChooser); - AutonConfig RIGHT_TWO_CORNER = new AutonConfig("Right Two Corner", RightTwoCorner::new, - "Right Corner Bite", "Right Corner Bite To Score", "Right Bite Score To Score", "Right Score To NZ (F)", "Right NZ To Score"); + 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 NZ (F)", "Right NZ To Score"); RIGHT_TWO_CORNER.register(autonChooser); // FOLLOWS - AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTime, + AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, "Left Follow To Bump", "Left Follow To Score"); LEFT_FOLLOW.register(autonChooser); - AutonConfig RIGHT_FOLLOW = new AutonConfig("Right Follow", RightFollow::new, prevWaitTime, + AutonConfig RIGHT_FOLLOW = new AutonConfig("Right Follow", RightFollow::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Follow To Bump", "Right Follow To Score"); RIGHT_FOLLOW.register(autonChooser); @@ -416,10 +419,17 @@ public void configureAutons() { } - public boolean hasWaitTimeChanged() { - hasWaitTimeChanged = prevWaitTime != getWaitTime(); - prevWaitTime = getWaitTime(); - return hasWaitTimeChanged; + 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() { @@ -486,8 +496,12 @@ public Command getAutonomousCommand() { } } - public double getWaitTime() { - return waitTime.get(); + public static double getWaitTimeOne() { + return waitTimeOne.get(); + } + + public static double getWaitTimeTwo() { + return waitTimeTwo.get(); } public void periodicAfterScheduler() { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java index 3f8c4faf..99fb2499 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.commands.auton.regular; 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; @@ -34,7 +35,7 @@ public LeftFollow(PathPlannerPath... paths) { new ParallelCommandGroup( new HandoffRun(), new SpindexerRun(), - new WaitCommand(3.0) + new WaitCommand(RobotContainer.getWaitTimeOne() + 1.0) ), // To NZ @@ -42,21 +43,20 @@ public LeftFollow(PathPlannerPath... paths) { new HandoffStop(), new SpindexerStop(), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new WaitCommand(0.75).andThen(new IntakeDeploy()) + new WaitCommand(0.5).andThen(new IntakeDeploy()) ), - new WaitCommand(1.0), + new WaitCommand(RobotContainer.getWaitTimeTwo()), // Back CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new WaitCommand(0.5), - // SOTM To Corner new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( new HandoffRun(), - new SpindexerRun() + new SpindexerRun(), + new IntakeAutoDigest().repeatedly() ) ); 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 c3d9d127..92efabdd 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 @@ -10,6 +10,7 @@ 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.IntakeStopRollers; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; @@ -41,14 +42,13 @@ public LeftTwoCorner(PathPlannerPath... paths) { // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), + new SuperstructureAutoInterpolation()), new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(5.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 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 307c1e90..d41f349a 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 @@ -5,6 +5,7 @@ /***************************************************************/ 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; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java index 0cacd4be..fd919cb3 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.commands.auton.regular; 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; @@ -34,7 +35,7 @@ public RightFollow(PathPlannerPath... paths) { new ParallelCommandGroup( new HandoffRun(), new SpindexerRun(), - new WaitCommand(3.0) + new WaitCommand(RobotContainer.getWaitTimeOne() + 1.0) ), // To NZ @@ -42,21 +43,20 @@ public RightFollow(PathPlannerPath... paths) { new HandoffStop(), new SpindexerStop(), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new WaitCommand(0.75).andThen(new IntakeDeploy()) + new WaitCommand(0.5).andThen(new IntakeDeploy()) ), - new WaitCommand(1.0), + new WaitCommand(RobotContainer.getWaitTimeTwo()), // Back CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new WaitCommand(0.5), - // SOTM To Corner new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( new HandoffRun(), - new SpindexerRun() + new SpindexerRun(), + new IntakeAutoDigest().repeatedly() ) ); 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 c8890b4f..2558b245 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 @@ -10,6 +10,7 @@ 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.IntakeStopRollers; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; @@ -41,14 +42,13 @@ public RightTwoCorner(PathPlannerPath... paths) { // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() - ), + new SuperstructureAutoInterpolation()), new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(5.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 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 9a1a9482..85eb387b 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -52,7 +52,6 @@ public SwerveDriveSOTM(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); superstructure = Superstructure.getInstance(); - speed = VStream.create(this::getDriverInputAsVelocity) .filtered( new VDeadZone(Drive.DEADBAND), @@ -115,5 +114,4 @@ public boolean isFinished() { return true; } } - } diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index ab5650a5..7e8971db 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -12,6 +12,7 @@ import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; import com.stuypulse.robot.util.vision.LimelightHelpers; +import com.stuypulse.robot.util.vision.LimelightHelpers.RawFiducial; import com.stuypulse.stuylib.network.SmartBoolean; import dev.doglog.DogLog; @@ -124,13 +125,19 @@ public void log() { DogLog.log(keyName + "# Rejected Invalid Position", rejectedCounterInvalidPosition); DogLog.log(keyName + "Heartbeat", LimelightHelpers.getHeartbeat(name)); - DogLog.log(keyName + "Temp (C)", LimelightHelpers.getLimelightNTString(name, "hw")); + DogLog.log(keyName + "Temp (C)", LimelightHelpers.getLimelightDoubleArrayEntry(name, "hw").get()); DogLog.log(keyName + "Pose MT1", (Robot.isBlue() ? LimelightHelpers.getBotPoseEstimate_wpiBlue(name).pose : LimelightHelpers.getBotPoseEstimate_wpiRed(name).pose)); DogLog.log(keyName + "Pose MT2", (Robot.isBlue() ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).pose : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(name).pose)); + DogLog.log(keyName + "Pipeline", LimelightHelpers.getCurrentPipelineIndex(name)); + + RawFiducial[] rawFiducials = LimelightHelpers.getRawFiducials(name); + for(Integer i = 0; i < rawFiducials.length; i++) { + DogLog.log(keyName + "Raw Fiducials[" + i.toString() + "]", rawFiducials[i].id); + } } public String getName() { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 4858219b..934ce7f1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -164,15 +164,15 @@ public interface FerryRPMInterpolation { {1, 2000}, {5.16, 3300.0}, {6.94, 3400}, - {9.77, 3400}, //TODO: USE THE BELOW THREE LINES INTEAD OF THESE + {9.77, 3600}, //TODO: USE THE BELOW THREE LINES INTEAD OF THESE // {6.94, 3600.0}, // {7.87, 3800.0}, // {9.77, 4300.0}, - {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS UNRELIABLE!!! - {11.516, 4900.0}, - {12.416, 5200.0}, - {13.316, 5500.0}, - {14.216, 5600.0} + // {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS UNRELIABLE!!! + // {11.516, 4900.0}, + // {12.416, 5200.0}, + // {13.316, 5500.0}, + // {14.216, 5600.0} }; } diff --git a/src/main/java/com/stuypulse/robot/util/PathUtil.java b/src/main/java/com/stuypulse/robot/util/PathUtil.java index b536cc17..93427029 100644 --- a/src/main/java/com/stuypulse/robot/util/PathUtil.java +++ b/src/main/java/com/stuypulse/robot/util/PathUtil.java @@ -30,13 +30,15 @@ public static class AutonConfig { private final String name; private final Function auton; private final String[] paths; - private final Optional waitTime; + private final Optional waitTimeOne; + private final Optional waitTimeTwo; - public AutonConfig(String name, Function auton, double waitTime, String... paths) { + public AutonConfig(String name, Function auton, double waitTimeOne, double waitTimeTwo, String... paths) { this.name = name; this.auton = auton; this.paths = paths; - this.waitTime = Optional.of(waitTime); + this.waitTimeOne = Optional.of(waitTimeOne); + this.waitTimeTwo = Optional.of(waitTimeTwo); for (String path : paths) { try { @@ -48,13 +50,13 @@ public AutonConfig(String name, Function auton, doub } public AutonConfig(String name, Function auton, String... paths) { - this(name, auton, 0.0, paths); + this(name, auton, 0.0, 0.0, paths); } private Command buildCommand() { Command autonCommand = auton.apply(loadPaths(paths)); - if (waitTime.isPresent() && waitTime.get() > 0.0) { - return Commands.sequence(new WaitCommand(waitTime.get()), autonCommand); + if (waitTimeOne.isPresent() && waitTimeOne.get() > 0.0) { + return Commands.sequence(new WaitCommand(waitTimeOne.get()), autonCommand); } return autonCommand; } From 475eb178cf863090b43fa678f35894e68c92a48e Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 21 Apr 2026 14:49:54 -0400 Subject: [PATCH 19/33] ADD: Auto work --- .../paths/Right Follow To Bump.path | 44 +++++++++---------- .../paths/Right Follow To Score.path | 12 ++--- .../commands/auton/regular/RightFollow.java | 6 +-- 3 files changed, 31 insertions(+), 31 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Right Follow To Bump.path b/src/main/deploy/pathplanner/paths/Right Follow To Bump.path index e3194c4f..bef14c14 100644 --- a/src/main/deploy/pathplanner/paths/Right Follow To Bump.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Bump.path @@ -8,68 +8,68 @@ }, "prevControl": null, "nextControl": { - "x": 6.1351212553495, - "y": 0.6256633380884444 + "x": 7.739514978601996, + "y": 0.6903566333808839 }, "isLocked": false, "linkedName": "Right Follower Start" }, { "anchor": { - "x": 8.282938659058486, + "x": 8.593466476462195, "y": 1.4666761768901564 }, "prevControl": { - "x": 8.282938659058486, + "x": 8.593466476462195, "y": 0.12105563480741921 }, "nextControl": { - "x": 8.282938659058486, - "y": 3.6299100829375885 + "x": 8.593466476462195, + "y": 3.629910082937589 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.674821683309556, - "y": 4.533138373751783 + "x": 8.14061340941512, + "y": 4.416690442225392 }, "prevControl": { - "x": 8.68395914376938, - "y": 4.515118061957858 + "x": 9.149750869874945, + "y": 4.398670130431467 }, "nextControl": { - "x": 6.9502567760342355, - "y": 4.546077032810271 + "x": 7.4160485021398, + "y": 4.42962910128388 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.273723252496433, - "y": 2.5794008559201154 + "x": 7.95947218259629, + "y": 2.230057061340942 }, "prevControl": { - "x": 7.325477888730384, - "y": 3.135763195435093 + "x": 7.95947218259629, + "y": 2.7888214080939244 }, "nextControl": { - "x": 7.219780939723599, - "y": 1.999520993612133 + "x": 7.95947218259629, + "y": 1.6607560627674771 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.282938659058486, - "y": 0.5868473609129818 + "x": 7.95947218259629, + "y": 0.6256633380884444 }, "prevControl": { - "x": 7.5066191155492135, - "y": 0.4057061340941528 + "x": 7.933594864479314, + "y": 1.0397004279600561 }, "nextControl": null, "isLocked": false, 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 4355d0c5..91643fef 100644 --- a/src/main/deploy/pathplanner/paths/Right Follow To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Score.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 8.282938659058486, - "y": 0.5868473609129818 + "x": 7.95947218259629, + "y": 0.6256633380884444 }, "prevControl": null, "nextControl": { - "x": 6.725809288559871, - "y": 0.5916822415081257 + "x": 6.402342812097675, + "y": 0.6304982186835882 }, "isLocked": false, "linkedName": "Right Follower Bump" }, { "anchor": { - "x": 1.2313694721825963, + "x": 1.140798858773181, "y": 0.6256633380884444 }, "prevControl": { - "x": 3.249209389945311, + "x": 3.1586387765358954, "y": 0.6251768362147326 }, "nextControl": null, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java index fd919cb3..76e73a64 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java @@ -35,15 +35,15 @@ public RightFollow(PathPlannerPath... paths) { new ParallelCommandGroup( new HandoffRun(), new SpindexerRun(), - new WaitCommand(RobotContainer.getWaitTimeOne() + 1.0) + new WaitCommand(RobotContainer.getWaitTimeOne() + 1.0), + new WaitCommand(1.0).andThen(new IntakeDeploy()) ), // To NZ new ParallelCommandGroup( new HandoffStop(), new SpindexerStop(), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new WaitCommand(0.5).andThen(new IntakeDeploy()) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) ), new WaitCommand(RobotContainer.getWaitTimeTwo()), From f1ec9703ea142ee68901ad86603def5c6308197c Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Tue, 21 Apr 2026 21:29:57 -0400 Subject: [PATCH 20/33] FIX: fixed double delay in follow autons. also made left follow go to depot at end --- .../paths/Left Corner To Depot.path | 54 +++++++++++++++++++ .../paths/Left Follow To Score.path | 22 ++++++-- .../paths/Right Follow To Bump.path | 40 +++++++------- .../com/stuypulse/robot/RobotContainer.java | 2 +- .../robot/commands/auton/regular/Depot.java | 6 +++ .../commands/auton/regular/LeftBump.java | 6 +++ .../commands/auton/regular/LeftFollow.java | 12 +++-- .../commands/auton/regular/LeftTwoCorner.java | 6 +++ .../commands/auton/regular/LeftTwoCycle.java | 5 ++ .../commands/auton/regular/RightBump.java | 6 +++ .../commands/auton/regular/RightFollow.java | 7 ++- .../auton/regular/RightTwoCorner.java | 6 +++ .../commands/auton/regular/RightTwoCycle.java | 6 +++ .../stuypulse/robot/constants/Cameras.java | 5 -- .../superstructure/Superstructure.java | 2 - .../swerve/CommandSwerveDrivetrain.java | 2 - .../subsystems/vision/LimelightVision.java | 2 - .../com/stuypulse/robot/util/PathUtil.java | 6 +-- 18 files changed, 151 insertions(+), 44 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Left Corner To Depot.path diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Depot.path b/src/main/deploy/pathplanner/paths/Left Corner To Depot.path new file mode 100644 index 00000000..af090dfd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner To Depot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4701897983392653, + "y": 5.966275207591934 + }, + "prevControl": null, + "nextControl": { + "x": 1.0089571213446336, + "y": 5.9663124527470375 + }, + "isLocked": false, + "linkedName": "Left Follower Bump Post" + }, + { + "anchor": { + "x": 0.5556583629893244, + "y": 5.966275207591934 + }, + "prevControl": { + "x": 1.0669518189123153, + "y": 5.97347056501368 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.75, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To Depot", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file 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 6c78c243..3ba689eb 100644 --- a/src/main/deploy/pathplanner/paths/Left Follow To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Follow To Score.path @@ -16,12 +16,28 @@ }, { "anchor": { - "x": 1.1666761768901568, + "x": 3.5036773428232504, "y": 7.4572753209700435 }, "prevControl": { - "x": 2.3375665978676, - "y": 7.4475560051381615 + "x": 5.1102575154516465, + "y": 7.487520923728483 + }, + "nextControl": { + "x": 2.0296678529062877, + "y": 7.429525504151838 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4701897983392653, + "y": 5.966275207591934 + }, + "prevControl": { + "x": 1.5239857651245559, + "y": 7.730782918149466 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Follow To Bump.path b/src/main/deploy/pathplanner/paths/Right Follow To Bump.path index bef14c14..546cc6a6 100644 --- a/src/main/deploy/pathplanner/paths/Right Follow To Bump.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Bump.path @@ -16,48 +16,48 @@ }, { "anchor": { - "x": 8.593466476462195, - "y": 1.4666761768901564 + "x": 8.50670225385528, + "y": 1.3183036773428227 }, "prevControl": { - "x": 8.593466476462195, - "y": 0.12105563480741921 + "x": 8.549739027283511, + "y": 0.21010676156583608 }, "nextControl": { - "x": 8.593466476462195, - "y": 3.629910082937589 + "x": 8.422756447854804, + "y": 3.4799081818551842 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.14061340941512, - "y": 4.416690442225392 + "x": 7.95947218259629, + "y": 4.427710557532621 }, "prevControl": { - "x": 9.149750869874945, - "y": 4.398670130431467 + "x": 8.968609643056114, + "y": 4.4096902457386955 }, "nextControl": { - "x": 7.4160485021398, - "y": 4.42962910128388 + "x": 7.234907275320969, + "y": 4.440649216591109 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.95947218259629, - "y": 2.230057061340942 + "x": 7.656725978647687, + "y": 2.297390272835112 }, "prevControl": { - "x": 7.95947218259629, - "y": 2.7888214080939244 + "x": 7.656725978647687, + "y": 2.8561546195880942 }, "nextControl": { - "x": 7.95947218259629, - "y": 1.6607560627674771 + "x": 7.656725978647687, + "y": 1.728089274261647 }, "isLocked": false, "linkedName": null @@ -68,8 +68,8 @@ "y": 0.6256633380884444 }, "prevControl": { - "x": 7.933594864479314, - "y": 1.0397004279600561 + "x": 7.656725978647687, + "y": 1.1784341637010676 }, "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 89207b94..c4fa3321 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -408,7 +408,7 @@ public void configureAutons() { // FOLLOWS AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Follow To Bump", "Left Follow To Score"); + "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, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/Depot.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/Depot.java index 601d6e34..02bf7230 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/Depot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/Depot.java @@ -1,6 +1,9 @@ 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; @@ -14,6 +17,7 @@ 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; @@ -27,6 +31,8 @@ public Depot(PathPlannerPath... paths) { new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new SpindexerRun()), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java index 99c797dd..e62711b5 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java @@ -1,6 +1,9 @@ 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; @@ -14,6 +17,7 @@ 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; @@ -27,6 +31,8 @@ public LeftBump(PathPlannerPath... paths) { new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java index 99fb2499..60769e66 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java @@ -1,5 +1,7 @@ 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; @@ -16,6 +18,7 @@ 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; @@ -35,21 +38,22 @@ public LeftFollow(PathPlannerPath... paths) { new ParallelCommandGroup( new HandoffRun(), new SpindexerRun(), - new WaitCommand(RobotContainer.getWaitTimeOne() + 1.0) + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne() + 1.0), Set.of()), + new WaitCommand(1.0).andThen(new IntakeDeploy()) ), // To NZ new ParallelCommandGroup( new HandoffStop(), new SpindexerStop(), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new WaitCommand(0.5).andThen(new IntakeDeploy()) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) ), - new WaitCommand(RobotContainer.getWaitTimeTwo()), + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeTwo()), Set.of()), // Back CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), // SOTM To Corner new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), 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 92efabdd..6d2665cc 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 @@ -5,6 +5,7 @@ /***************************************************************/ 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; @@ -20,11 +21,14 @@ 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; +import java.util.Set; + import com.pathplanner.lib.path.PathPlannerPath; public class LeftTwoCorner extends SequentialCommandGroup { @@ -35,6 +39,8 @@ public LeftTwoCorner(PathPlannerPath... paths) { 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.5).andThen(new IntakeDeploy()) 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 d41f349a..b074d4f0 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 @@ -20,11 +20,14 @@ 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; +import java.util.Set; + import com.pathplanner.lib.path.PathPlannerPath; public class LeftTwoCycle extends SequentialCommandGroup { @@ -35,6 +38,8 @@ public LeftTwoCycle(PathPlannerPath... paths) { 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.5).andThen(new IntakeDeploy()) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightBump.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightBump.java index 8a64f16e..7b9d645a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightBump.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightBump.java @@ -1,6 +1,9 @@ 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; @@ -14,6 +17,7 @@ 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; @@ -27,6 +31,8 @@ public RightBump(PathPlannerPath... paths) { new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java index 76e73a64..42f40bd3 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java @@ -1,5 +1,7 @@ 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; @@ -16,6 +18,7 @@ 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; @@ -35,7 +38,7 @@ public RightFollow(PathPlannerPath... paths) { new ParallelCommandGroup( new HandoffRun(), new SpindexerRun(), - new WaitCommand(RobotContainer.getWaitTimeOne() + 1.0), + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne() + 1.0), Set.of()), new WaitCommand(1.0).andThen(new IntakeDeploy()) ), @@ -46,7 +49,7 @@ public RightFollow(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) ), - new WaitCommand(RobotContainer.getWaitTimeTwo()), + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeTwo()), Set.of()), // Back CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), 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 2558b245..324387f0 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 @@ -5,6 +5,7 @@ /***************************************************************/ 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; @@ -20,11 +21,14 @@ 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; +import java.util.Set; + import com.pathplanner.lib.path.PathPlannerPath; public class RightTwoCorner extends SequentialCommandGroup { @@ -35,6 +39,8 @@ public RightTwoCorner(PathPlannerPath... paths) { 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.5).andThen(new IntakeDeploy()) 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 7954d264..819e0559 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 @@ -5,6 +5,7 @@ /***************************************************************/ 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; @@ -18,11 +19,14 @@ 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; +import java.util.Set; + import com.pathplanner.lib.path.PathPlannerPath; public class RightTwoCycle extends SequentialCommandGroup { @@ -32,6 +36,8 @@ public RightTwoCycle(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( diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 7e8971db..fc9e2cfb 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -5,12 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import org.jspecify.annotations.Nullable; - import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer; -import com.stuypulse.robot.subsystems.vision.LimelightVision; -import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.robot.util.vision.LimelightHelpers.RawFiducial; import com.stuypulse.stuylib.network.SmartBoolean; @@ -19,7 +15,6 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.struct.StructSerializable; /** This interface stores information about each camera. */ public interface Cameras { 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 69eca4ec..ba7f3214 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -20,8 +20,6 @@ import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -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.Rotation2d; 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 6724616b..790c9e94 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -52,8 +52,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.wpilibj.Notifier; 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 28f853e8..c7b02853 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -26,8 +26,6 @@ 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.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/com/stuypulse/robot/util/PathUtil.java b/src/main/java/com/stuypulse/robot/util/PathUtil.java index 93427029..074501e5 100644 --- a/src/main/java/com/stuypulse/robot/util/PathUtil.java +++ b/src/main/java/com/stuypulse/robot/util/PathUtil.java @@ -55,9 +55,9 @@ public AutonConfig(String name, Function auton, Stri private Command buildCommand() { Command autonCommand = auton.apply(loadPaths(paths)); - if (waitTimeOne.isPresent() && waitTimeOne.get() > 0.0) { - return Commands.sequence(new WaitCommand(waitTimeOne.get()), autonCommand); - } + // if (waitTimeOne.isPresent() && waitTimeOne.get() > 0.0) { + // return Commands.sequence(new WaitCommand(waitTimeOne.get()), autonCommand); + // } return autonCommand; } From 316dd29c0ac72cae5d1c7bbff2b651ed49329e82 Mon Sep 17 00:00:00 2001 From: Danx3mer Date: Wed, 22 Apr 2026 01:08:19 -0400 Subject: [PATCH 21/33] feat: (superstruct) isShooting() detection --- .../subsystems/superstructure/Superstructure.java | 10 ++++++++++ 1 file changed, 10 insertions(+) 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 ba7f3214..be648703 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -20,6 +20,8 @@ import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +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.Rotation2d; @@ -50,6 +52,7 @@ public static Superstructure getInstance(){ private final Shooter shooter; private final Turret turret; + private final BStream currentlyShooting; // private final BStream readyToShoot; public Superstructure() { @@ -61,6 +64,9 @@ public Superstructure() { // readyToShoot = BStream.create(this::atTolerance) // .filtered(new BDebounce.Both(0.05)); + currentlyShooting = BStream.create(() -> (!isShooterAtTolerance())) + .filtered(new BDebounce.Falling(0.5)); + sotmStoppedTimer = new Timer(); sotmStoppedTimer.restart(); sotmStoppedTimer.stop(); @@ -215,6 +221,10 @@ public void clearMemoized() { this.shouldStop = Optional.empty(); } + public boolean isShooting() { + return currentlyShooting.get(); + } + public void periodicAfterScheduler() { SuperstructureState state = getState(); From 13a89337fe9aa4e6ca56a83dd16a6f0e5bfb0f4e Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 22 Apr 2026 14:30:58 -0400 Subject: [PATCH 22/33] feat: auto changes --- .../paths/Right Follow To Bump.path | 46 +++++++++---------- .../paths/Right Follow To Score.path | 22 +++++++-- .../com/stuypulse/robot/RobotContainer.java | 9 ++-- .../commands/auton/regular/LeftFollow.java | 15 +++++- .../commands/auton/regular/LeftTwoCorner.java | 8 ---- .../commands/auton/regular/LeftTwoCycle.java | 8 ---- .../commands/auton/regular/RightFollow.java | 20 ++++---- .../auton/regular/RightTwoCorner.java | 8 ---- .../commands/auton/regular/RightTwoCycle.java | 8 ---- .../commands/swerve/SwerveDriveSOTM.java | 6 +-- .../com/stuypulse/robot/constants/Gains.java | 2 +- .../robot/subsystems/intake/IntakeImpl.java | 11 ++++- 12 files changed, 84 insertions(+), 79 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Right Follow To Bump.path b/src/main/deploy/pathplanner/paths/Right Follow To Bump.path index 546cc6a6..04285bbe 100644 --- a/src/main/deploy/pathplanner/paths/Right Follow To Bump.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Bump.path @@ -16,48 +16,48 @@ }, { "anchor": { - "x": 8.50670225385528, - "y": 1.3183036773428227 + "x": 8.451141226818828, + "y": 1.4278601997146938 }, "prevControl": { - "x": 8.549739027283511, - "y": 0.21010676156583608 + "x": 8.451141226818828, + "y": 0.08223965763195662 }, "nextControl": { - "x": 8.422756447854804, - "y": 3.4799081818551842 + "x": 8.451141226818828, + "y": 3.591094105762126 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.95947218259629, - "y": 4.427710557532621 + "x": 7.791269614835947, + "y": 4.403751783166904 }, "prevControl": { - "x": 8.968609643056114, - "y": 4.4096902457386955 + "x": 8.800407075295771, + "y": 4.385731471372979 }, "nextControl": { - "x": 7.234907275320969, - "y": 4.440649216591109 + "x": 7.066704707560627, + "y": 4.416690442225392 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.656725978647687, - "y": 2.297390272835112 + "x": 7.4678031383737515, + "y": 2.1653637660485026 }, "prevControl": { - "x": 7.656725978647687, - "y": 2.8561546195880942 + "x": 7.4678031383737515, + "y": 2.7346647646219684 }, "nextControl": { - "x": 7.656725978647687, - "y": 1.728089274261647 + "x": 7.4678031383737515, + "y": 1.0765927118872947 }, "isLocked": false, "linkedName": null @@ -68,8 +68,8 @@ "y": 0.6256633380884444 }, "prevControl": { - "x": 7.656725978647687, - "y": 1.1784341637010676 + "x": 7.4548644793152645, + "y": 0.910313837375179 }, "nextControl": null, "isLocked": false, @@ -100,10 +100,6 @@ { "waypointRelativePos": 3.445628997867779, "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 3.8550106609807893, - "rotationDegrees": 180.0 } ], "constraintZones": [ @@ -133,7 +129,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "Follow", 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 91643fef..b7ba2acd 100644 --- a/src/main/deploy/pathplanner/paths/Right Follow To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Score.path @@ -20,7 +20,7 @@ "y": 0.6256633380884444 }, "prevControl": { - "x": 3.1586387765358954, + "x": 3.158638776535895, "y": 0.6251768362147326 }, "nextControl": null, @@ -29,7 +29,21 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5455773126266057, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -42,13 +56,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "Follow", "idealStartingState": { "velocity": 0, - "rotation": 180.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 c4fa3321..f1c81f05 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -97,6 +97,7 @@ import edu.wpi.first.wpilibj2.command.RepeatCommand; 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 { @@ -433,10 +434,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)); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java index 60769e66..14b53aec 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java @@ -52,7 +52,20 @@ public LeftFollow(PathPlannerPath... paths) { Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeTwo()), Set.of()), // Back - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new WaitCommand(2.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + .andThen( + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun()) + ) + ) + ), + + new HandoffStop().alongWith(new SpindexerStop()), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), // SOTM To Corner 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 6d2665cc..e9c5393a 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 @@ -77,14 +77,6 @@ public LeftTwoCorner(PathPlannerPath... paths) { new HandoffStop(), new SpindexerStop() ) - - // new SuperstructureSOTM(), - // new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - // new HandoffRun().andThen( - // new SpindexerRun() - // ).andThen(new WaitCommand(2.5) - // .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), - // new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()) ); 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 b074d4f0..f1780d4c 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 @@ -77,14 +77,6 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffStop(), new SpindexerStop() ) - - // new SuperstructureSOTM(), - // new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - // new HandoffRun().andThen( - // new SpindexerRun() - // ).andThen(new WaitCommand(2.5) - // .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), - // new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java index 42f40bd3..13785df2 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java @@ -51,16 +51,20 @@ public RightFollow(PathPlannerPath... paths) { Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeTwo()), Set.of()), - // Back - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - // SOTM To Corner - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - new HandoffRun(), - new SpindexerRun(), - new IntakeAutoDigest().repeatedly() - ) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new WaitCommand(2.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + .andThen( + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun()) + ) + ) + ), + + new IntakeAutoDigest().repeatedly() ); 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 324387f0..6da3cbef 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 @@ -77,14 +77,6 @@ public RightTwoCorner(PathPlannerPath... paths) { new HandoffStop(), new SpindexerStop() ) - - // new SuperstructureSOTM(), - // new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - // new HandoffRun().andThen( - // new SpindexerRun() - // ).andThen(new WaitCommand(2.5) - // .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), - // new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()) ); 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 819e0559..947d3662 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 @@ -76,14 +76,6 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffStop(), new SpindexerStop() ) - - // new SuperstructureSOTM(), - // new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - // new HandoffRun().andThen( - // new SpindexerRun() - // ).andThen(new WaitCommand(2.5) - // .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), - // new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()) ); 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 85eb387b..89303369 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -89,10 +89,10 @@ public void execute() { DogLog.log("Swerve/SOTM/Idle?", isIdle.get()); if (isIdle.get()) { - if (!isIdleInit) { - CommandScheduler.getInstance().schedule(new IntakeAutoDigest().repeatedly().onlyWhile(() -> isIdle.get()).andThen(new IntakeDeploy())); + // if (!isIdleInit) { + // CommandScheduler.getInstance().schedule(new IntakeAutoDigest().repeatedly().onlyWhile(() -> isIdle.get()).andThen(new IntakeDeploy())); swerve.setControl(new SwerveRequest.SwerveDriveBrake()); - } + // } isIdleInit = true; } else { Vector2D velocity = speed.get(); diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 6d6e6132..c375402e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -71,7 +71,7 @@ public interface Spindexer { public interface Intake { public interface Pivot { - SmartNumber kP = new SmartNumber("Intake/Pivot/Gains/kP", 200.0); + SmartNumber kP = new SmartNumber("Intake/Pivot/Gains/kP", 125.0); SmartNumber kI = new SmartNumber("Intake/Pivot/Gains/kI", 0.0); SmartNumber kD = new SmartNumber("Intake/Pivot/Gains/kD", 10.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 422b6b40..330d5fa1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -56,6 +56,7 @@ public class IntakeImpl extends Intake { private PositionVoltage positionVoltage; private BStream pivotStalling; + private final BStream isPivotBelowPushDownThreshold; StatusSignal pivotSupplyCurrent; StatusSignal pivotStatorCurrent; @@ -139,6 +140,9 @@ public IntakeImpl() { pivotStalling = BStream.create( () -> Math.abs(pivotSupplyCurrent.getValueAsDouble()) > Settings.Intake.PIVOT_STALL_CURRENT) .filtered(new BDebounce.Both(Settings.Intake.PIVOT_STALL_DEBOUNCE)); + isPivotBelowPushDownThreshold = BStream.create(() -> isBelowPushDownThreshold()) + .filtered(new BDebounce.Rising(0.5)) + .filtered(new BDebounce.Falling(0.1)); } @Override @@ -167,6 +171,10 @@ public void seedPivotDeployed() { pivot.setPosition(Settings.Intake.PIVOT_MIN_ANGLE.getRotations()); } + private boolean isBelowPushDownThreshold() { + return getPivotAngle().getDegrees() <= Settings.Intake.ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE.getDegrees(); + } + @Override public void periodicAfterScheduler() { super.periodicAfterScheduler(); @@ -191,7 +199,7 @@ public void periodicAfterScheduler() { } else { // PIVOT if (pivotState == PivotState.DEPLOY && - getPivotAngle().getDegrees() <= Settings.Intake.ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE.getDegrees() + isPivotBelowPushDownThreshold.get() && rollerState != RollerState.STOP) { // pivot.setControl(new VoltageOut(Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts double pushdownCurrent = @@ -263,6 +271,7 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge pivotSupplyCurrent.getValueAsDouble()); DogLog.log("Intake/Pivot Stator Current (amps)", pivotStatorCurrent.getValueAsDouble()); + DogLog.log("Intake/Pivot is below pushdown Threshold", isPivotBelowPushDownThreshold.get()); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log("Robot/CAN/Main/Intake Pivot Motor Connected? (ID " From 9042adc21e76c026e8adc8606fd46be55ab113c4 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Wed, 22 Apr 2026 16:26:52 -0400 Subject: [PATCH 23/33] FIX: intake deploy time --- .../robot/commands/auton/regular/LeftTwoCorner.java | 11 +++++++++-- .../robot/commands/auton/regular/LeftTwoCycle.java | 2 +- .../robot/commands/auton/regular/RightTwoCorner.java | 11 +++++++++-- .../robot/commands/auton/regular/RightTwoCycle.java | 2 +- 4 files changed, 20 insertions(+), 6 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 e9c5393a..1b16d92a 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 @@ -43,8 +43,7 @@ public LeftTwoCorner(PathPlannerPath... paths) { // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.5).andThen(new IntakeDeploy()) - ), + (new WaitCommand(0.2)).andThen(new IntakeDeploy())), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( @@ -77,6 +76,14 @@ public LeftTwoCorner(PathPlannerPath... paths) { new HandoffStop(), new SpindexerStop() ) + + // new SuperstructureSOTM(), + // new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + // new HandoffRun().andThen( + // new SpindexerRun() + // ).andThen(new WaitCommand(2.5) + // .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), + // new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()) ); 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 f1780d4c..588d49a6 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 @@ -42,7 +42,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.5).andThen(new IntakeDeploy()) + new WaitCommand(0.2).andThen(new IntakeDeploy()) ), // Trip 1 To Score 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 6da3cbef..d81d7bb0 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 @@ -43,8 +43,7 @@ public RightTwoCorner(PathPlannerPath... paths) { // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.5).andThen(new IntakeDeploy()) - ), + (new WaitCommand(0.2)).andThen(new IntakeDeploy())), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( @@ -77,6 +76,14 @@ public RightTwoCorner(PathPlannerPath... paths) { new HandoffStop(), new SpindexerStop() ) + + // new SuperstructureSOTM(), + // new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + // new HandoffRun().andThen( + // new SpindexerRun() + // ).andThen(new WaitCommand(2.5) + // .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), + // new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()) ); 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 947d3662..d40b92cc 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 @@ -41,7 +41,7 @@ public RightTwoCycle(PathPlannerPath... paths) { // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.5).andThen(new IntakeDeploy()) + new WaitCommand(0.2).andThen(new IntakeDeploy()) ), // Trip 1 To Score From f27a7e978ccbfaa72265500553956fcb33a3722d Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 22 Apr 2026 17:16:21 -0400 Subject: [PATCH 24/33] feat: use hopper empty detection + fms util change to show shift 4 correctly --- .../robot/commands/auton/regular/LeftTwoCycle.java | 2 +- .../robot/commands/auton/regular/RightTwoCycle.java | 7 +++++-- src/main/java/com/stuypulse/robot/util/FMSUtil.java | 6 +++++- 3 files changed, 11 insertions(+), 4 deletions(-) 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 588d49a6..dcf6bf96 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 @@ -54,7 +54,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(3.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 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 d40b92cc..d581dc89 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 @@ -15,6 +15,7 @@ import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveResetPose; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -38,7 +39,7 @@ public RightTwoCycle(PathPlannerPath... paths) { 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()) @@ -53,7 +54,8 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(3.0), + // .andThen(new IntakeAutoDigest()).repeatedly()).alongWith(new WaitUntilCommand(() -> !Superstructure.getInstance().isShooting()).withTimeout(4.0)), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 @@ -69,6 +71,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new SpindexerRun() ).andThen(new WaitCommand(0.5) .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), + // .andThen(new IntakeAutoDigest()).repeatedly()).alongWith(new WaitUntilCommand(() -> !Superstructure.getInstance().isShooting()).withTimeout(4.0)), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/util/FMSUtil.java b/src/main/java/com/stuypulse/robot/util/FMSUtil.java index 09ccbb59..d28a6535 100644 --- a/src/main/java/com/stuypulse/robot/util/FMSUtil.java +++ b/src/main/java/com/stuypulse/robot/util/FMSUtil.java @@ -42,7 +42,11 @@ public boolean isActive(double time) { } public double timeLeft(double time) { - return Math.max(0.0, endTime - time); + if (this == FieldState.SHIFT_4) { + return Math.max(0.0, 140.0 - time); + } else { + return Math.max(0.0, endTime - time); + } } public double timeElapsed(double time) { From 5e227785c051d64212af79e664792bacefe5e28c Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Wed, 22 Apr 2026 17:24:32 -0400 Subject: [PATCH 25/33] feat: change shooter is shooting detection logic to use stator current --- .../java/com/stuypulse/robot/constants/Settings.java | 2 ++ .../subsystems/superstructure/Superstructure.java | 6 +----- .../subsystems/superstructure/shooter/Shooter.java | 4 ++-- .../superstructure/shooter/ShooterImpl.java | 12 ++++++++++++ .../superstructure/shooter/ShooterSim.java | 5 +++++ 5 files changed, 22 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 934ce7f1..3d360f56 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -194,6 +194,8 @@ public interface FerryTOFInterpolation { public interface Shooter { + public final double IS_SHOOTING_CURRENT = 25.0; + public final double GEAR_RATIO = 1.0; public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2.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 be648703..c45fc6b0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -52,7 +52,6 @@ public static Superstructure getInstance(){ private final Shooter shooter; private final Turret turret; - private final BStream currentlyShooting; // private final BStream readyToShoot; public Superstructure() { @@ -64,9 +63,6 @@ public Superstructure() { // readyToShoot = BStream.create(this::atTolerance) // .filtered(new BDebounce.Both(0.05)); - currentlyShooting = BStream.create(() -> (!isShooterAtTolerance())) - .filtered(new BDebounce.Falling(0.5)); - sotmStoppedTimer = new Timer(); sotmStoppedTimer.restart(); sotmStoppedTimer.stop(); @@ -222,7 +218,7 @@ public void clearMemoized() { } public boolean isShooting() { - return currentlyShooting.get(); + return shooter.isShooting(); } public void periodicAfterScheduler() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index 3441eeb3..e780985d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -108,14 +108,14 @@ public boolean shooterReadyToShoot() { return readyToShoot.get(); } - - public abstract double getRPM(); public abstract SysIdRoutine getShooterSysIdRoutine(); public abstract double getCurrentDraw(); + public abstract boolean isShooting(); + public void periodicAfterScheduler() { DogLog.log("Superstructure/Shooter/State", state.name()); 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 c2fb64ca..75fd4cac 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 @@ -23,6 +23,8 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.SysId; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import dev.doglog.DogLog; import edu.wpi.first.units.measure.AngularVelocity; @@ -52,6 +54,8 @@ public class ShooterImpl extends Shooter { private StatusSignal shooterLeaderClosedLoopError; private StatusSignal shooterLeaderTemperature; private StatusSignal shooterFollowerTemperature; + + private final BStream currentlyShooting; public ShooterImpl() { shooterConfig = new Motors.TalonFXConfig() @@ -106,6 +110,9 @@ public ShooterImpl() { shooterLeaderTemperature = shooterLeader.getDeviceTemp(); shooterFollowerTemperature = shooterFollower.getDeviceTemp(); + currentlyShooting = BStream.create(() -> (shooterLeadStatorCurrent.getValueAsDouble() > Settings.Superstructure.Shooter.IS_SHOOTING_CURRENT)) + .filtered(new BDebounce.Falling(0.5)); + PhoenixUtil.registerToRio(shooterLeaderSpeed, shooterFollowerSpeed, shooterFollowSupplyCurrent, shooterFollowStatorCurrent, shooterLeadSupplyCurrent, shooterLeadStatorCurrent, shooterLeaderVoltage, shooterFollowerVoltage, shooterLeaderClosedLoopError, @@ -216,4 +223,9 @@ public double getCurrentDraw() { return Double.max(0, shooterLeadSupplyCurrent.getValueAsDouble()) + Double.max(0, shooterFollowSupplyCurrent.getValueAsDouble()); } + + @Override + public boolean isShooting() { + return currentlyShooting.get(); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java index 8b9be7c2..61a2afe1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java @@ -105,4 +105,9 @@ public SysIdRoutine getShooterSysIdRoutine() { public double getCurrentDraw() { return 0; } + + @Override + public boolean isShooting() { + return true; + } } \ No newline at end of file From 83e05d35ba1b5b23ad6eb35cf2f85a9eb5c52afd Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 22 Apr 2026 18:06:41 -0400 Subject: [PATCH 26/33] FEAT: added slow backout to autons --- .../paths/Left Corner Bite To Score.path | 6 +- .../paths/Left Trench Score To Corner.path | 59 +++++++++++++++++++ .../paths/Right Bite Score To Score.path | 8 +-- .../paths/Right Trench Score To Corner.path | 54 +++++++++++++++++ .../com/stuypulse/robot/RobotContainer.java | 4 +- .../auton/regular/RightTwoCorner.java | 21 ++----- .../commands/auton/regular/RightTwoCycle.java | 14 ++--- .../superstructure/shooter/Shooter.java | 3 +- 8 files changed, 134 insertions(+), 35 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path create mode 100644 src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path 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 c91331b9..5e2c9e3f 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 @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.648308686286153, - "y": 4.276698141783557 + "x": 7.403109843081312, + "y": 4.339058487874465 }, "isLocked": false, "linkedName": "Left NZ Corner" @@ -46,7 +46,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.2899786780383839, + "waypointRelativePos": 0.2558635394456302, "rotationDegrees": 180.0 }, { diff --git a/src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path b/src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path new file mode 100644 index 00000000..d107fc1a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 2.046504992867332, + "y": 5.6070470756062765 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 0.4421112696148356, + "y": 5.956390870185448 + }, + "prevControl": { + "x": 1.5809557774607694, + "y": 6.055106990014265 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "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": 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 Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path index 2a778a76..0fced9f9 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 @@ -52,12 +52,12 @@ "y": 2.0877318116975756 }, "prevControl": { - "x": 6.018673323823109, - "y": 3.174579172610556 + "x": 6.003751284310849, + "y": 3.174299055956712 }, "nextControl": { - "x": 6.053793108756849, - "y": 0.22451723817625524 + "x": 6.083366619115549, + "y": 0.06930099857346717 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path b/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path new file mode 100644 index 00000000..9f6c17c1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": null, + "nextControl": { + "x": 3.2537189925345644, + "y": 0.5992999515606565 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 1.140798858773181, + "y": 0.6256633380884444 + }, + "prevControl": { + "x": 1.9885243443856553, + "y": 0.6007951416921778 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Corner" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.1, + "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": 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 f1c81f05..7d82a8c3 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -395,7 +395,7 @@ public void configureAutons() { 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 NZ (F)", "Right NZ To Score"); + "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Trench Score To Corner"); RIGHT_TWO_CYCLE.register(autonChooser); // TWO CYCLES (CORNER) @@ -404,7 +404,7 @@ public void configureAutons() { 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 NZ (F)", "Right NZ To Score"); + "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Trench Score To Corner"); RIGHT_TWO_CORNER.register(autonChooser); // FOLLOWS 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 d81d7bb0..dbe89d8d 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 @@ -65,25 +65,14 @@ public RightTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), - new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffStop(), - new SpindexerStop() + new HandoffRun().andThen( + new SpindexerRun() + ).andThen(new WaitCommand(0.5) + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0) + // .andThen(new IntakeAutoDigest()).repeatedly()).alongWith(new WaitUntilCommand(() -> !Superstructure.getInstance().isShooting()).withTimeout(4.0)), ) - - // new SuperstructureSOTM(), - // new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - // new HandoffRun().andThen( - // new SpindexerRun() - // ).andThen(new WaitCommand(2.5) - // .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), - // new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()) ); 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 d581dc89..eeab1038 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 @@ -67,17 +67,13 @@ public RightTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), - // .andThen(new IntakeAutoDigest()).repeatedly()).alongWith(new WaitUntilCommand(() -> !Superstructure.getInstance().isShooting()).withTimeout(4.0)), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), - new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffStop(), - new SpindexerStop() + new HandoffRun().andThen( + new SpindexerRun() + ).andThen(new WaitCommand(0.5) + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0) + // .andThen(new IntakeAutoDigest()).repeatedly()).alongWith(new WaitUntilCommand(() -> !Superstructure.getInstance().isShooting()).withTimeout(4.0)), ) ); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index e780985d..92dd682d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -7,6 +7,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.stuylib.streams.booleans.BStream; @@ -118,8 +119,8 @@ public boolean shooterReadyToShoot() { public void periodicAfterScheduler() { DogLog.log("Superstructure/Shooter/State", state.name()); - DogLog.log("Superstructure/Shooter/Current RPM (Leader)", getRPM()); DogLog.log("Superstructure/Shooter/Target RPM", getTargetRPM()); + DogLog.log("Superstructure/Shooter/Is Shooting", Superstructure.getInstance().isShooting()); } } From be5acffd07b72d8fbf1ff652d61c8b82b8595a45 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 22 Apr 2026 18:56:13 -0400 Subject: [PATCH 27/33] feat: more accurate energy util --- .../swerve/CommandSwerveDrivetrain.java | 5 +++-- .../com/stuypulse/robot/util/EnergyUtil.java | 19 +++++-------------- 2 files changed, 8 insertions(+), 16 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 790c9e94..1368e99a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -737,6 +737,9 @@ public void periodicAfterScheduler() { Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); + Robot.getEnergyUtil().logEnergyUsage(getName() + " Drive", getTotalDriveSupplyCurrent()); + Robot.getEnergyUtil().logEnergyUsage(getName() + " Turn", getTotalSteerSupplyCurrent()); + if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { DogLog.log("Swerve/Robot Accel X", robotAccelerationX.getValueAsDouble() * 9.81); DogLog.log("Swerve/Robot Accel Y", robotAccelerationY.getValueAsDouble() * 9.81); @@ -769,8 +772,6 @@ public void periodicAfterScheduler() { getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble()); } } - Robot.getEnergyUtil().logEnergyUsage(getName() + " Drive", getTotalDriveSupplyCurrent()); - Robot.getEnergyUtil().logEnergyUsage(getName() + " Turn", getTotalSteerSupplyCurrent()); // CAN SIGNAL LOGGING if (Settings.DEBUG_MODE.get() && Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { diff --git a/src/main/java/com/stuypulse/robot/util/EnergyUtil.java b/src/main/java/com/stuypulse/robot/util/EnergyUtil.java index 350cfd1a..cf367a6b 100644 --- a/src/main/java/com/stuypulse/robot/util/EnergyUtil.java +++ b/src/main/java/com/stuypulse/robot/util/EnergyUtil.java @@ -18,9 +18,7 @@ public class EnergyUtil { private double totalCurrent = 0.0; private double totalPowerWatts = 0.0; - private double totalCurrentCurrent = 0.0; - private double totalCurrentPowerWatts = 0.0; - private double totalEnergyWattHours = 0.0; + private double totalEnergyJoules = 0.0; private double batteryVoltage = 12.6; private Map subsytemCurrents = new HashMap<>(); @@ -33,7 +31,7 @@ public void logEnergyUsage(String subsystem, double amps) { totalCurrent += amps; totalPowerWatts += powerWatts; - totalEnergyWattHours += energyWattHours; + totalEnergyJoules += energyWattHours; subsytemCurrents.put(subsystem, amps); subsytemPowers.put(subsystem, powerWatts); @@ -57,18 +55,12 @@ public void logEnergyUsage(String subsystem, double amps) { } public void periodic() { - // energy used over the total time the robot has been on - DogLog.log("EnergyUtil/Total used Supply Current draw Amps", totalCurrent); - DogLog.log("EnergyUtil/total used Power Watts", totalPowerWatts); - DogLog.log("EnergyUtil/Total Used Energy Watt Hours", joulesToWattHours(totalEnergyWattHours)); - + DogLog.log("EnergyUtil/Total Used Energy", joulesToWattHours(totalEnergyJoules), "Watt Hours"); // energy used over the current command schedule loop - totalCurrentCurrent = totalCurrent - totalCurrentCurrent; - totalCurrentPowerWatts = totalPowerWatts - totalCurrentPowerWatts; - DogLog.log("EnergyUtil/Total current Current", totalCurrentCurrent); - DogLog.log("EnergyUtil/Total current Power Watts", totalCurrentCurrent); + DogLog.log("EnergyUtil/Total Current", totalCurrent, "Amps"); + DogLog.log("EnergyUtil/Total Power", totalPowerWatts, "Watts"); for (var entry : subsytemCurrents.entrySet()) { DogLog.log("EnergyUtil/Supply Current Amps/" + entry.getKey(), entry.getValue()); @@ -94,5 +86,4 @@ private double joulesToWattHours(double joules) { public void setBatteryVoltage(double voltage) { this.batteryVoltage = voltage; } - } From 2192104a72740575056e233caf5c7ffc5edfeade Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 22 Apr 2026 19:42:46 -0400 Subject: [PATCH 28/33] feat: is hopper empty stuff + auton stuff --- .../paths/Center Hub To Depot.path | 8 ++--- .../pathplanner/paths/Depot To Score.path | 10 +++---- .../paths/Left Bump Score To Depot.path | 10 +++---- .../paths/Left Trench Score To Corner.path | 29 ++++++++----------- .../paths/Right Bump Score To Depot.path | 8 ++--- .../paths/Right Trench Score To Corner.path | 2 +- .../com/stuypulse/robot/RobotContainer.java | 9 ++++-- .../commands/auton/regular/LeftTwoCorner.java | 21 ++++---------- .../commands/auton/regular/LeftTwoCycle.java | 13 ++++----- .../robot/commands/auton/test/EmptyTest.java | 17 ++++++++++- .../superstructure/Superstructure.java | 5 ++-- .../superstructure/shooter/Shooter.java | 1 - .../superstructure/shooter/ShooterImpl.java | 2 +- 13 files changed, 68 insertions(+), 67 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Center Hub To Depot.path b/src/main/deploy/pathplanner/paths/Center Hub To Depot.path index 2acfdcd0..11090489 100644 --- a/src/main/deploy/pathplanner/paths/Center Hub To Depot.path +++ b/src/main/deploy/pathplanner/paths/Center Hub To Depot.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 0.4421112696148356, - "y": 5.956390870185448 + "x": 0.5456205420827387, + "y": 5.930513552068473 }, "prevControl": { - "x": 1.5548359486447927, - "y": 6.085777460770328 + "x": 1.6583452211126959, + "y": 6.059900142653353 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Depot To Score.path b/src/main/deploy/pathplanner/paths/Depot To Score.path index cdfac99d..ee4f9b3e 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Score.path +++ b/src/main/deploy/pathplanner/paths/Depot To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.4421112696148356, - "y": 5.956390870185448 + "x": 0.5456205420827387, + "y": 5.930513552068473 }, "prevControl": null, "nextControl": { - "x": 1.4799968459060167, - "y": 5.930523224579741 + "x": 1.5835061183739199, + "y": 5.904645906462766 }, "isLocked": false, "linkedName": "Depot" @@ -20,7 +20,7 @@ "y": 5.959713740458016 }, "prevControl": { - "x": 0.6133229987217921, + "x": 0.6133229987217922, "y": 5.954351642472394 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path index e4edaad7..ccdab241 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.0114122681883018, + "x": 1.0114122681883013, "y": 6.1375320970042795 }, "isLocked": false, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.4421112696148356, - "y": 5.956390870185448 + "x": 0.5456205420827387, + "y": 5.930513552068473 }, "prevControl": { - "x": 1.32915325434766, - "y": 5.956390870185448 + "x": 1.4326625268155633, + "y": 5.930513552068473 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path b/src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path index d107fc1a..5a7d881e 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path @@ -8,46 +8,41 @@ }, "prevControl": null, "nextControl": { - "x": 2.046504992867332, - "y": 5.6070470756062765 + "x": 1.6682626001441125, + "y": 7.463868043077666 }, "isLocked": false, "linkedName": "Left Trench Score" }, { "anchor": { - "x": 0.4421112696148356, - "y": 5.956390870185448 + "x": 1.0761055634807417, + "y": 7.440906488549619 }, "prevControl": { - "x": 1.5809557774607694, - "y": 6.055106990014265 + "x": 2.128123079917189, + "y": 7.407014232727067 }, "nextControl": null, "isLocked": false, - "linkedName": "Depot" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 0.0 + "linkedName": "Left Corner" } ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.19, + "maxVelocity": 0.1, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": null, @@ -55,5 +50,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 Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path index 3d29cedf..3a10d5d4 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.4421112696148356, - "y": 5.956390870185448 + "x": 0.5456205420827387, + "y": 5.930513552068473 }, "prevControl": { - "x": 2.3570328102710407, - "y": 5.801126961483593 + "x": 2.460542082738944, + "y": 5.775249643366618 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path b/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path index 9f6c17c1..4c139be2 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.1, + "maxVelocity": 0.5, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 7d82a8c3..50ff93dc 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -17,6 +17,7 @@ import com.stuypulse.robot.commands.auton.regular.RightTwoCorner; 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; @@ -391,7 +392,7 @@ public void configureAutons() { // 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 NZ (F)", "Left NZ To Score"); + "Left Trench To NZ", "Left NZ To Score", "Left Score To Score", "Left Trench Score To Corner"); LEFT_TWO_CYCLE.register(autonChooser); AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, prevWaitTimeOne, prevWaitTimeTwo, @@ -400,7 +401,7 @@ public void configureAutons() { // 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 NZ (F)", "Left NZ To Score"); + "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Trench Score To Corner"); LEFT_TWO_CORNER.register(autonChooser); AutonConfig RIGHT_TWO_CORNER = new AutonConfig("Right Two Corner", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, @@ -416,6 +417,10 @@ 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); + SmartDashboard.putData("Autonomous", autonChooser); } 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 1b16d92a..f76c8c9e 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 @@ -65,25 +65,14 @@ public LeftTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), - new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffStop(), - new SpindexerStop() + new HandoffRun().andThen( + new SpindexerRun() + ).andThen(new WaitCommand(0.5) + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0) + // .andThen(new IntakeAutoDigest()).repeatedly()).alongWith(new WaitUntilCommand(() -> !Superstructure.getInstance().isShooting()).withTimeout(4.0)), ) - - // new SuperstructureSOTM(), - // new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - // new HandoffRun().andThen( - // new SpindexerRun() - // ).andThen(new WaitCommand(2.5) - // .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), - // new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()) ); 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 dcf6bf96..791472ca 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 @@ -66,16 +66,13 @@ public LeftTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), - new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffStop(), - new SpindexerStop() + new HandoffRun().andThen( + new SpindexerRun() + ).andThen(new WaitCommand(0.5) + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0) + // .andThen(new IntakeAutoDigest()).repeatedly()).alongWith(new WaitUntilCommand(() -> !Superstructure.getInstance().isShooting()).withTimeout(4.0)), ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java index f692f5f7..285c9687 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java @@ -5,9 +5,17 @@ /***************************************************************/ package com.stuypulse.robot.commands.auton.test; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import com.pathplanner.lib.path.PathPlannerPath; @@ -17,7 +25,14 @@ public EmptyTest(PathPlannerPath... paths) { addCommands( - + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new HandoffRun().alongWith(new SpindexerRun()).andThen(new WaitCommand(0.5)) + .andThen(new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty())), + + new HandoffStop().alongWith(new SpindexerStop()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[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 c45fc6b0..c2c07c70 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -217,8 +217,8 @@ public void clearMemoized() { this.shouldStop = Optional.empty(); } - public boolean isShooting() { - return shooter.isShooting(); + public boolean isHopperEmpty() { + return !shooter.isShooting(); } public void periodicAfterScheduler() { @@ -255,6 +255,7 @@ else if (getState() == SuperstructureState.FOTM && shouldStop() && DriverStation DogLog.log("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); DogLog.log("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); + DogLog.log("Superstructure/Is Hopper Empty?", isHopperEmpty()); DogLog.log("Superstructure/Is Ready To Shoot?", isReadyToShoot()); DogLog.log("Superstructure/Should Stop?", shouldStop()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index 92dd682d..7d359959 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -121,6 +121,5 @@ public void periodicAfterScheduler() { DogLog.log("Superstructure/Shooter/State", state.name()); DogLog.log("Superstructure/Shooter/Current RPM (Leader)", getRPM()); DogLog.log("Superstructure/Shooter/Target RPM", getTargetRPM()); - DogLog.log("Superstructure/Shooter/Is Shooting", Superstructure.getInstance().isShooting()); } } 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 75fd4cac..30bdae5b 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(0.5)); + .filtered(new BDebounce.Falling(1.0)); PhoenixUtil.registerToRio(shooterLeaderSpeed, shooterFollowerSpeed, shooterFollowSupplyCurrent, shooterFollowStatorCurrent, shooterLeadSupplyCurrent, shooterLeadStatorCurrent, From 2d84082bc24908a30e92f1adf696d72101e7d176 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 23 Apr 2026 16:16:31 -0400 Subject: [PATCH 29/33] FEAT: added auton jiggle to end of first pass --- simgui.json | 9 +- .../pathplanner/autos/Left Corner Bite.auto | 6 +- .../pathplanner/autos/Left Two Cycle.auto | 6 +- .../pathplanner/autos/Right Corner Bite.auto | 6 +- .../pathplanner/autos/Right Two Cycle.auto | 6 +- .../paths/Left Follow To Score.path | 16 +++- .../paths/Left Trench Score Jiggle.path | 86 +++++++++++++++++++ .../paths/Right Bite Score To Score.path | 12 +-- .../paths/Right Score To Score.path | 8 +- .../paths/Right Trench Score Jiggle.path | 86 +++++++++++++++++++ .../paths/Right Trench Score To Corner.path | 2 +- .../com/stuypulse/robot/RobotContainer.java | 8 +- .../commands/auton/regular/LeftTwoCorner.java | 25 +++--- .../commands/auton/regular/LeftTwoCycle.java | 18 ++-- .../auton/regular/RightTwoCorner.java | 25 +++--- .../commands/auton/regular/RightTwoCycle.java | 19 ++-- .../robot/commands/auton/test/EmptyTest.java | 6 +- .../stuypulse/robot/constants/Settings.java | 2 +- 18 files changed, 278 insertions(+), 68 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Left Trench Score Jiggle.path create mode 100644 src/main/deploy/pathplanner/paths/Right Trench Score Jiggle.path diff --git a/simgui.json b/simgui.json index 0d40fac0..92d7c18b 100644 --- a/simgui.json +++ b/simgui.json @@ -94,6 +94,9 @@ } }, "NetworkTables": { + "Persistent Values": { + "open": false + }, "transitory": { "SmartDashboard": { "FieldPositions": { @@ -105,8 +108,7 @@ "Robot": { "Auton": { "open": true - }, - "open": true + } }, "Spindexer": { "open": true @@ -115,6 +117,9 @@ "open": false }, "Superstructure": { + "Turret": { + "open": true + }, "open": true }, "open": true diff --git a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto index c24faff3..5b6ddf0f 100644 --- a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto @@ -19,19 +19,19 @@ { "type": "path", "data": { - "pathName": "Left Bite Score To Score" + "pathName": "Left Trench Score Jiggle" } }, { "type": "path", "data": { - "pathName": "Left Score To NZ (F)" + "pathName": "Left Bite Score To Score" } }, { "type": "path", "data": { - "pathName": "Left NZ To Score" + "pathName": "Left Trench Score Jiggle" } } ] diff --git a/src/main/deploy/pathplanner/autos/Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto index 4a2aa495..a9a1e9db 100644 --- a/src/main/deploy/pathplanner/autos/Left Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto @@ -19,19 +19,19 @@ { "type": "path", "data": { - "pathName": "Left Score To Score" + "pathName": "Left Trench Score Jiggle" } }, { "type": "path", "data": { - "pathName": "Left Score To NZ (F)" + "pathName": "Left Score To Score" } }, { "type": "path", "data": { - "pathName": "Left NZ To Score" + "pathName": "Left Trench Score Jiggle" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto index 2cc4ce28..dfafda69 100644 --- a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto @@ -19,19 +19,19 @@ { "type": "path", "data": { - "pathName": "Right Bite Score To Score" + "pathName": "Right Trench Score Jiggle" } }, { "type": "path", "data": { - "pathName": "Right Score To NZ (F)" + "pathName": "Right Bite Score To Score" } }, { "type": "path", "data": { - "pathName": "Right NZ To Score" + "pathName": "Right Trench Score Jiggle" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right Two Cycle.auto b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto index 4c0a5a8e..669d3572 100644 --- a/src/main/deploy/pathplanner/autos/Right Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto @@ -19,19 +19,19 @@ { "type": "path", "data": { - "pathName": "Right Score To Score" + "pathName": "Right Trench Score Jiggle" } }, { "type": "path", "data": { - "pathName": "Right Score To NZ (F)" + "pathName": "Right Score To Score" } }, { "type": "path", "data": { - "pathName": "Right NZ To Score" + "pathName": "Right Trench Score Jiggle" } } ] 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 3ba689eb..6983bce4 100644 --- a/src/main/deploy/pathplanner/paths/Left Follow To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Follow To Score.path @@ -45,7 +45,21 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0, + "constraints": { + "maxVelocity": 1.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 Trench Score Jiggle.path b/src/main/deploy/pathplanner/paths/Left Trench Score Jiggle.path new file mode 100644 index 00000000..dd2f9652 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Trench Score Jiggle.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 3.392844384394676, + "y": 7.39173020348081 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 3.9495162804987527, + "y": 7.470935135495777 + }, + "nextControl": { + "x": 3.389113233229541, + "y": 7.416921946260355 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 3.393315206008684, + "y": 7.389439053666736 + }, + "nextControl": { + "x": 3.882604908113996, + "y": 7.492373923432501 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 3.8804292237195184, + "y": 7.501805611860949 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Jiggle" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 2.5, + "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 Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path index 0fced9f9..2d259744 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 @@ -12,7 +12,7 @@ "y": 0.5997860199714706 }, "isLocked": false, - "linkedName": "Right Trench Score" + "linkedName": "Right Jiggle" }, { "anchor": { @@ -52,12 +52,12 @@ "y": 2.0877318116975756 }, "prevControl": { - "x": 6.003751284310849, - "y": 3.174299055956712 + "x": 6.009732033987853, + "y": 3.174435940086786 }, "nextControl": { - "x": 6.083366619115549, - "y": 0.06930099857346717 + "x": 6.070427960057061, + "y": 0.15987161198288136 }, "isLocked": false, "linkedName": null @@ -73,7 +73,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Right Trench Score" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index d0f23ac2..116345d3 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -8,11 +8,11 @@ }, "prevControl": null, "nextControl": { - "x": 6.575035663338087, - "y": 0.6515406562054205 + "x": 7.0796433666191145, + "y": 0.5868473609129818 }, "isLocked": false, - "linkedName": "Right Trench Score" + "linkedName": "Right Jiggle" }, { "anchor": { @@ -52,7 +52,7 @@ "y": 0.5868473609129818 }, "prevControl": { - "x": 7.673089129599265, + "x": 7.673089129599264, "y": 0.5487960706826538 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Right Trench Score Jiggle.path b/src/main/deploy/pathplanner/paths/Right Trench Score Jiggle.path new file mode 100644 index 00000000..b892e51d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Trench Score Jiggle.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": null, + "nextControl": { + "x": 3.3670531645130364, + "y": 0.6364508653658119 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 3.9226105563480753, + "y": 0.6256633380884473 + }, + "nextControl": { + "x": 3.3640132697659486, + "y": 0.5558386772656785 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 4.024624178228986, + "y": 0.6167198866124293 + }, + "nextControl": { + "x": 3.1995412996597437, + "y": 0.5569748352135345 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 3.9594059536548416, + "y": 0.5862281453061209 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Jiggle" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 2.5, + "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/Right Trench Score To Corner.path b/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path index 4c139be2..9f6c17c1 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.5, + "maxVelocity": 0.1, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 50ff93dc..5b9a0152 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -392,20 +392,20 @@ public void configureAutons() { // 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 Trench Score To Corner"); + "Left Trench To NZ", "Left NZ To Score", "Left Trench Score Jiggle", "Left Score To Score", "Left Trench Score To Corner"); 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 Trench Score To Corner"); + "Right Trench To NZ", "Right NZ To Score", "Right Trench Score Jiggle", "Right Score To Score", "Right Trench Score To Corner"); RIGHT_TWO_CYCLE.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 Trench Score To Corner"); + "Left Corner Bite", "Left NZ To Score", "Left Trench Score Jiggle", "Left Bite Score To Score", "Left Trench Score To Corner"); 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 Trench Score To Corner"); + "Right Corner Bite", "Right NZ To Score", "Right Trench Score Jiggle", "Right Bite Score To Score", "Right Trench Score To Corner"); RIGHT_TWO_CORNER.register(autonChooser); // FOLLOWS 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 f76c8c9e..15288fd6 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 @@ -11,7 +11,6 @@ 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.IntakeStopRollers; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; @@ -43,22 +42,29 @@ public LeftTwoCorner(PathPlannerPath... paths) { // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - (new WaitCommand(0.2)).andThen(new IntakeDeploy())), + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation()), + new SuperstructureAutoInterpolation() + ), new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(5.0), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest()).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.0), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(2.5)) + ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffStop(), new SpindexerStop() ), @@ -66,12 +72,11 @@ public LeftTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly(), new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(0.5) .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0) - // .andThen(new IntakeAutoDigest()).repeatedly()).alongWith(new WaitUntilCommand(() -> !Superstructure.getInstance().isShooting()).withTimeout(4.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 791472ca..97466dab 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 @@ -51,15 +51,20 @@ public LeftTwoCycle(PathPlannerPath... paths) { ), new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(3.0), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest()).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.0), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(2.5)) + ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffStop(), new SpindexerStop() ), @@ -67,12 +72,11 @@ public LeftTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly(), new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(0.5) .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0) - // .andThen(new IntakeAutoDigest()).repeatedly()).alongWith(new WaitUntilCommand(() -> !Superstructure.getInstance().isShooting()).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 dbe89d8d..76a6d69e 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 @@ -11,7 +11,6 @@ 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.IntakeStopRollers; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; @@ -43,22 +42,29 @@ public RightTwoCorner(PathPlannerPath... paths) { // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - (new WaitCommand(0.2)).andThen(new IntakeDeploy())), + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation()), + new SuperstructureAutoInterpolation() + ), new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(5.0), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest()).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.0), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(2.5)) + ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffStop(), new SpindexerStop() ), @@ -66,12 +72,11 @@ public RightTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly(), new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(0.5) .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0) - // .andThen(new IntakeAutoDigest()).repeatedly()).alongWith(new WaitUntilCommand(() -> !Superstructure.getInstance().isShooting()).withTimeout(4.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 eeab1038..7190b7f4 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 @@ -51,16 +51,20 @@ public RightTwoCycle(PathPlannerPath... paths) { ), new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(3.0), - // .andThen(new IntakeAutoDigest()).repeatedly()).alongWith(new WaitUntilCommand(() -> !Superstructure.getInstance().isShooting()).withTimeout(4.0)), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest()).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.0), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(2.5)) + ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffStop(), new SpindexerStop() ), @@ -68,12 +72,11 @@ public RightTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly(), new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(0.5) .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0) - // .andThen(new IntakeAutoDigest()).repeatedly()).alongWith(new WaitUntilCommand(() -> !Superstructure.getInstance().isShooting()).withTimeout(4.0)), ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java index 285c9687..896031f0 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java @@ -7,6 +7,8 @@ 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.SuperstructureSOTM; @@ -28,9 +30,9 @@ public EmptyTest(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new SpindexerRun()).andThen(new WaitCommand(0.5)) - .andThen(new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty())), + .andThen(new IntakeAutoDigest().alongWith(new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()))), - new HandoffStop().alongWith(new SpindexerStop()), + new IntakeDeploy().alongWith(new HandoffStop()).alongWith(new SpindexerStop()), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 3d360f56..8d7b08dc 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -90,7 +90,7 @@ public interface Intake { } public interface Spindexer { - double FORWARD_DUTY_CYCLE = 1.0; //TODO: GET + double FORWARD_DUTY_CYCLE = 1.0; double ANTI_POPCORN_DUTY_CYCLE = 0.2; double REVERSE_DUTY_CYCLE = -1.0; double STOP_SPEED = 0.0; From 350d2baf21f20193f76ea23d7f733f7c3fc30a40 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 24 Apr 2026 14:18:52 -0400 Subject: [PATCH 30/33] TESTING: Auto work --- .../pathplanner/autos/Left Corner Bite.auto | 8 +- .../pathplanner/autos/Left Two Cycle.auto | 8 +- .../pathplanner/autos/Right Corner Bite.auto | 8 +- .../pathplanner/autos/Right Two Cycle.auto | 8 +- .../paths/Left Score To Score.path | 2 +- .../paths/Left Trench Score Jiggle.path | 86 ------------------- .../paths/Left Trench Score To Corner.path | 6 +- .../paths/Right Score To Score.path | 6 +- .../paths/Right Trench Score Jiggle.path | 86 ------------------- .../paths/Right Trench Score To Corner.path | 4 +- .../com/stuypulse/robot/RobotContainer.java | 8 +- .../commands/auton/regular/LeftTwoCorner.java | 12 +-- .../commands/auton/regular/LeftTwoCycle.java | 12 +-- .../auton/regular/RightTwoCorner.java | 12 +-- .../commands/auton/regular/RightTwoCycle.java | 26 +++--- .../superstructure/shooter/ShooterImpl.java | 2 +- 16 files changed, 52 insertions(+), 242 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/Left Trench Score Jiggle.path delete mode 100644 src/main/deploy/pathplanner/paths/Right Trench 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 5b6ddf0f..8a495169 100644 --- a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto @@ -16,12 +16,6 @@ "pathName": "Left Corner Bite To Score" } }, - { - "type": "path", - "data": { - "pathName": "Left Trench Score Jiggle" - } - }, { "type": "path", "data": { @@ -31,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Left Trench Score Jiggle" + "pathName": "Left Trench 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 a9a1e9db..d4ac3941 100644 --- a/src/main/deploy/pathplanner/autos/Left Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto @@ -16,12 +16,6 @@ "pathName": "Left NZ To Score" } }, - { - "type": "path", - "data": { - "pathName": "Left Trench Score Jiggle" - } - }, { "type": "path", "data": { @@ -31,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Left Trench Score Jiggle" + "pathName": "Left Trench 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 dfafda69..a37cc078 100644 --- a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto @@ -16,12 +16,6 @@ "pathName": "Right NZ To Score" } }, - { - "type": "path", - "data": { - "pathName": "Right Trench Score Jiggle" - } - }, { "type": "path", "data": { @@ -31,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Right Trench Score Jiggle" + "pathName": "Right Trench 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 669d3572..faf7225e 100644 --- a/src/main/deploy/pathplanner/autos/Right Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto @@ -16,12 +16,6 @@ "pathName": "Right NZ To Score" } }, - { - "type": "path", - "data": { - "pathName": "Right Trench Score Jiggle" - } - }, { "type": "path", "data": { @@ -31,7 +25,7 @@ { "type": "path", "data": { - "pathName": "Right Trench Score Jiggle" + "pathName": "Right Trench Score To Corner" } } ] 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 c87bcf66..7e3d5fa7 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -98,7 +98,7 @@ "rotationDegrees": 90.0 }, { - "waypointRelativePos": 3.2, + "waypointRelativePos": 2.882729211087425, "rotationDegrees": 0.0 } ], diff --git a/src/main/deploy/pathplanner/paths/Left Trench Score Jiggle.path b/src/main/deploy/pathplanner/paths/Left Trench Score Jiggle.path deleted file mode 100644 index dd2f9652..00000000 --- a/src/main/deploy/pathplanner/paths/Left Trench Score Jiggle.path +++ /dev/null @@ -1,86 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.63796005706134, - "y": 7.440906488549619 - }, - "prevControl": null, - "nextControl": { - "x": 3.392844384394676, - "y": 7.39173020348081 - }, - "isLocked": false, - "linkedName": "Left Trench Score" - }, - { - "anchor": { - "x": 3.63796005706134, - "y": 7.440906488549619 - }, - "prevControl": { - "x": 3.9495162804987527, - "y": 7.470935135495777 - }, - "nextControl": { - "x": 3.389113233229541, - "y": 7.416921946260355 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.63796005706134, - "y": 7.440906488549619 - }, - "prevControl": { - "x": 3.393315206008684, - "y": 7.389439053666736 - }, - "nextControl": { - "x": 3.882604908113996, - "y": 7.492373923432501 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.63796005706134, - "y": 7.440906488549619 - }, - "prevControl": { - "x": 3.8804292237195184, - "y": 7.501805611860949 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Left Jiggle" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 2.5, - "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/Left Trench Score To Corner.path b/src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path index 5a7d881e..b403a950 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.6682626001441125, + "x": 1.6682626001441123, "y": 7.463868043077666 }, "isLocked": false, @@ -34,10 +34,10 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 0.1, - "maxAcceleration": 10.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 116345d3..22f7f845 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.673089129599264, + "x": 7.673089129599263, "y": 0.5487960706826538 }, "nextControl": null, @@ -78,11 +78,11 @@ "rotationDegrees": -90.0 }, { - "waypointRelativePos": 2.289978678038381, + "waypointRelativePos": 2.1812366737739914, "rotationDegrees": -90.0 }, { - "waypointRelativePos": 2.6703967446591785, + "waypointRelativePos": 2.4179104477611943, "rotationDegrees": 0.0 } ], diff --git a/src/main/deploy/pathplanner/paths/Right Trench Score Jiggle.path b/src/main/deploy/pathplanner/paths/Right Trench Score Jiggle.path deleted file mode 100644 index b892e51d..00000000 --- a/src/main/deploy/pathplanner/paths/Right Trench Score Jiggle.path +++ /dev/null @@ -1,86 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 - }, - "prevControl": null, - "nextControl": { - "x": 3.3670531645130364, - "y": 0.6364508653658119 - }, - "isLocked": false, - "linkedName": "Right Trench Score" - }, - { - "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 - }, - "prevControl": { - "x": 3.9226105563480753, - "y": 0.6256633380884473 - }, - "nextControl": { - "x": 3.3640132697659486, - "y": 0.5558386772656785 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 - }, - "prevControl": { - "x": 4.024624178228986, - "y": 0.6167198866124293 - }, - "nextControl": { - "x": 3.1995412996597437, - "y": 0.5569748352135345 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 - }, - "prevControl": { - "x": 3.9594059536548416, - "y": 0.5862281453061209 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Right Jiggle" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 2.5, - "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/Right Trench Score To Corner.path b/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path index 9f6c17c1..95f39b73 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path @@ -34,10 +34,10 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 0.1, - "maxAcceleration": 10.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5b9a0152..fe61e022 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -392,20 +392,20 @@ public void configureAutons() { // 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 Trench Score Jiggle", "Left Score To Score", "Left Trench Score To Corner"); + "Left Trench To NZ", "Left NZ To Score", "Left Score To Score", "Left Trench Score To Corner"); 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 Trench Score Jiggle", "Right Score To Score", "Right Trench Score To Corner"); + "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Trench Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CYCLE.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 Trench Score Jiggle", "Left Bite Score To Score", "Left Trench Score To Corner"); + "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Trench Score To Corner"); 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 Trench Score Jiggle", "Right Bite Score To Score", "Right Trench Score To Corner"); + "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Trench Score To Corner"); RIGHT_TWO_CORNER.register(autonChooser); // FOLLOWS 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 15288fd6..f73a41df 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 @@ -52,19 +52,19 @@ public LeftTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.0), + .andThen(new IntakeAutoDigest().repeatedly().withTimeout(4.0)) + .until(() -> Superstructure.getInstance().isHopperEmpty()), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(2.5)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), new HandoffStop(), new SpindexerStop() ), @@ -72,8 +72,8 @@ public LeftTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly(), - new HandoffRun().andThen( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun().alongWith( new SpindexerRun() ).andThen(new WaitCommand(0.5) .andThen(new IntakeAutoDigest()).repeatedly()).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 97466dab..5dbc1f83 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 @@ -52,19 +52,19 @@ public LeftTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.0), + .andThen(new IntakeAutoDigest().repeatedly().withTimeout(4.0)) + .until(() -> Superstructure.getInstance().isHopperEmpty()), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(2.5)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), new HandoffStop(), new SpindexerStop() ), @@ -72,8 +72,8 @@ public LeftTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly(), - new HandoffRun().andThen( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun().alongWith( new SpindexerRun() ).andThen(new WaitCommand(0.5) .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(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 76a6d69e..5f1d2277 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 @@ -52,19 +52,19 @@ public RightTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.0), + .andThen(new IntakeAutoDigest().repeatedly().withTimeout(4.0)) + .until(() -> Superstructure.getInstance().isHopperEmpty()), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(2.5)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), new HandoffStop(), new SpindexerStop() ), @@ -72,8 +72,8 @@ public RightTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly(), - new HandoffRun().andThen( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun().alongWith( new SpindexerRun() ).andThen(new WaitCommand(0.5) .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(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 7190b7f4..647cab61 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 @@ -20,6 +20,7 @@ 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; @@ -52,19 +53,19 @@ public RightTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.0), + .andThen(new IntakeAutoDigest().repeatedly().withTimeout(4.0)) + .until(() -> Superstructure.getInstance().isHopperEmpty()), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(2.5)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), new HandoffStop(), new SpindexerStop() ), @@ -72,12 +73,17 @@ public RightTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).repeatedly(), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0) - ) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().repeatedly().withTimeout(15.0)) + .until(() -> Superstructure.getInstance().isHopperEmpty()), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)) + ), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]) ); 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..9142a37d 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(0.75)); PhoenixUtil.registerToRio(shooterLeaderSpeed, shooterFollowerSpeed, shooterFollowSupplyCurrent, shooterFollowStatorCurrent, shooterLeadSupplyCurrent, shooterLeadStatorCurrent, From 2792e2feaf1a58a90a404edf0dc293794db629aa Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Fri, 24 Apr 2026 15:02:31 -0400 Subject: [PATCH 31/33] FIX: auto digest working again --- .../robot/commands/auton/regular/RightTwoCycle.java | 9 +++------ .../robot/commands/intake/IntakeAutoDigest.java | 4 +++- 2 files changed, 6 insertions(+), 7 deletions(-) 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 647cab61..33aef561 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 @@ -56,8 +56,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().repeatedly().withTimeout(4.0)) - .until(() -> Superstructure.getInstance().isHopperEmpty()), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), @@ -77,10 +76,8 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().repeatedly().withTimeout(15.0)) - .until(() -> Superstructure.getInstance().isHopperEmpty()), - new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)) + .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/intake/IntakeAutoDigest.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java index 1006ce74..e74cc945 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java @@ -9,7 +9,9 @@ public IntakeAutoDigest() { addCommands( - new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.5)) + new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.5)).andThen( + new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.5))).andThen( + new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.5))) ); From 5a5aeb8401d55665c1a0c7f2917ea99de4cb9f28 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Fri, 24 Apr 2026 15:16:54 -0400 Subject: [PATCH 32/33] FEAT: applied updates to all usable autons --- .../com/stuypulse/robot/RobotContainer.java | 6 +++--- .../commands/auton/regular/LeftTwoCorner.java | 19 +++++++++++-------- .../commands/auton/regular/LeftTwoCycle.java | 19 +++++++++++-------- .../auton/regular/RightTwoCorner.java | 19 +++++++++++-------- 4 files changed, 36 insertions(+), 27 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index fe61e022..db8281aa 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -392,7 +392,7 @@ public void configureAutons() { // 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 Trench Score To Corner"); + "Left Trench To NZ", "Left NZ To Score", "Left Score To Score", "Left Trench 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, @@ -401,11 +401,11 @@ public void configureAutons() { // 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 Trench Score To Corner"); + "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Trench Score To Corner", "Left Score To NZ (F)"); 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 Trench Score To Corner"); + "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Trench Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CORNER.register(autonChooser); // FOLLOWS 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 f73a41df..9e1fa85e 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 @@ -20,6 +20,7 @@ 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; @@ -55,8 +56,7 @@ public LeftTwoCorner(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().repeatedly().withTimeout(4.0)) - .until(() -> Superstructure.getInstance().isHopperEmpty()), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), @@ -72,12 +72,15 @@ public LeftTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffRun().alongWith( - new SpindexerRun() - ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0) - ) + 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/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 5dbc1f83..90677d78 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 @@ -20,6 +20,7 @@ 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; @@ -55,8 +56,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().repeatedly().withTimeout(4.0)) - .until(() -> Superstructure.getInstance().isHopperEmpty()), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), @@ -72,12 +72,15 @@ public LeftTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffRun().alongWith( - new SpindexerRun() - ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0) - ) + 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/RightTwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java index 5f1d2277..f3e3a55c 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 @@ -20,6 +20,7 @@ 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; @@ -55,8 +56,7 @@ public RightTwoCorner(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().repeatedly().withTimeout(4.0)) - .until(() -> Superstructure.getInstance().isHopperEmpty()), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), new WaitCommand(1.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) ), @@ -72,12 +72,15 @@ public RightTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffRun().alongWith( - new SpindexerRun() - ).andThen(new WaitCommand(0.5) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0) - ) + 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 9c9865b0f9eda51bee80c50d43c64b3093b2eee1 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 24 Apr 2026 19:30:19 -0400 Subject: [PATCH 33/33] fix: a whole lotta autons that actually work now :D --- .../pathplanner/autos/Left Corner Bite.auto | 2 +- .../pathplanner/autos/Left Two Cycle.auto | 2 +- .../pathplanner/autos/Right Corner Bite.auto | 2 +- .../pathplanner/autos/Right Two Cycle.auto | 2 +- .../paths/Left Follow To Score.path | 8 +- .../pathplanner/paths/Left Score Jiggle.path | 134 ++++++++++++++++++ ... Corner.path => Left Score To Corner.path} | 12 +- .../paths/Right Follow To Score.path | 10 +- .../pathplanner/paths/Right Score Jiggle.path | 134 ++++++++++++++++++ ...Corner.path => Right Score To Corner.path} | 14 +- .../com/stuypulse/robot/RobotContainer.java | 11 +- .../commands/auton/regular/LeftFollow.java | 5 +- .../commands/auton/regular/LeftTwoCorner.java | 6 +- .../commands/auton/regular/LeftTwoCycle.java | 6 +- .../commands/auton/regular/RightFollow.java | 5 +- .../auton/regular/RightTwoCorner.java | 6 +- .../commands/auton/regular/RightTwoCycle.java | 6 +- .../commands/intake/IntakeAutoDigest.java | 7 +- .../commands/intake/IntakeTeleopDigest.java | 18 +++ .../stuypulse/robot/constants/Settings.java | 20 ++- .../superstructure/shooter/ShooterImpl.java | 2 +- 21 files changed, 348 insertions(+), 64 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Left Score Jiggle.path rename src/main/deploy/pathplanner/paths/{Left Trench Score To Corner.path => Left Score To Corner.path} (83%) create mode 100644 src/main/deploy/pathplanner/paths/Right Score Jiggle.path rename src/main/deploy/pathplanner/paths/{Right Trench Score To Corner.path => Right Score To Corner.path} (80%) create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeTeleopDigest.java diff --git a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto index 8a495169..55916030 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 Trench Score To Corner" + "pathName": "Left Score Jiggle" } } ] diff --git a/src/main/deploy/pathplanner/autos/Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto index d4ac3941..513b4a1c 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 Trench Score To Corner" + "pathName": "Left Score Jiggle" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto index a37cc078..9000ca89 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 Trench Score To Corner" + "pathName": "Right Score Jiggle" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right Two Cycle.auto b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto index faf7225e..78af2d36 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 Trench Score To Corner" + "pathName": "Right Score Jiggle" } } ] 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 6983bce4..5d36788c 100644 --- a/src/main/deploy/pathplanner/paths/Left Follow To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Follow To Score.path @@ -48,11 +48,11 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0, + "minWaypointRelativePos": 0.8317617866005099, + "maxWaypointRelativePos": 2.0, "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 10.0, + "maxVelocity": 1.75, + "maxAcceleration": 4.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Left Score Jiggle.path b/src/main/deploy/pathplanner/paths/Left Score Jiggle.path new file mode 100644 index 00000000..75ea4914 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Score Jiggle.path @@ -0,0 +1,134 @@ +{ + "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/Left Trench Score To Corner.path b/src/main/deploy/pathplanner/paths/Left Score To Corner.path similarity index 83% rename from src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path rename to src/main/deploy/pathplanner/paths/Left Score To Corner.path index b403a950..0be2c04f 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Corner.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.6682626001441123, - "y": 7.463868043077666 + "x": 3.2245896483318734, + "y": 7.343909351042221 }, "isLocked": false, "linkedName": "Left Trench Score" }, { "anchor": { - "x": 1.0761055634807417, + "x": 3.2808444444444445, "y": 7.440906488549619 }, "prevControl": { - "x": 2.128123079917189, - "y": 7.407014232727067 + "x": 3.524888980928413, + "y": 7.49514913056054 }, "nextControl": null, "isLocked": false, @@ -37,7 +37,7 @@ "maxAcceleration": 4.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 Follow To Score.path b/src/main/deploy/pathplanner/paths/Right Follow To Score.path index b7ba2acd..90adc8cf 100644 --- a/src/main/deploy/pathplanner/paths/Right Follow To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Score.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 1.140798858773181, + "x": 1.7509666666666668, "y": 0.6256633380884444 }, "prevControl": { - "x": 3.158638776535895, + "x": 3.7688065844293814, "y": 0.6251768362147326 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Corner" + "linkedName": null } ], "rotationTargets": [], @@ -35,8 +35,8 @@ "minWaypointRelativePos": 0.5455773126266057, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.75, + "maxAcceleration": 4.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, diff --git a/src/main/deploy/pathplanner/paths/Right Score Jiggle.path b/src/main/deploy/pathplanner/paths/Right Score Jiggle.path new file mode 100644 index 00000000..29c167c7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Score Jiggle.path @@ -0,0 +1,134 @@ +{ + "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/deploy/pathplanner/paths/Right Trench Score To Corner.path b/src/main/deploy/pathplanner/paths/Right Score To Corner.path similarity index 80% rename from src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path rename to src/main/deploy/pathplanner/paths/Right Score To Corner.path index 95f39b73..7697becc 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Corner.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 3.2537189925345644, - "y": 0.5992999515606565 + "x": 3.36543714002239, + "y": 0.5460313281265844 }, "isLocked": false, "linkedName": "Right Trench Score" }, { "anchor": { - "x": 1.140798858773181, - "y": 0.6256633380884444 + "x": 3.2711, + "y": 0.5868473609129818 }, "prevControl": { - "x": 1.9885243443856553, - "y": 0.6007951416921778 + "x": 3.717247367182559, + "y": 0.5630880724749401 }, "nextControl": null, "isLocked": false, @@ -37,7 +37,7 @@ "maxAcceleration": 4.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index db8281aa..f3c29829 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -32,6 +32,7 @@ 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; import com.stuypulse.robot.commands.intake.SeedPivotDeployed; import com.stuypulse.robot.commands.intake.SeedPivotStowed; import com.stuypulse.robot.commands.leds.LEDApplyPattern; @@ -195,7 +196,7 @@ private void configureButtonBindings() { // Digest (TR) driver.getTopButton() - .whileTrue(new IntakeAutoDigest().repeatedly()) + .whileTrue(new IntakeTeleopDigest().repeatedly()) .onFalse(new IntakeDeploy()); // Intake Stow @@ -392,20 +393,20 @@ public void configureAutons() { // 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 Trench Score To Corner", "Left Score To NZ (F)"); + "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 Trench Score To Corner", "Right Score To NZ (F)"); + "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); // 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 Trench Score To Corner", "Left Score To NZ (F)"); + "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 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 Trench Score To Corner", "Right Score To NZ (F)"); + "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); // FOLLOWS diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java index 14b53aec..822385c7 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java @@ -54,13 +54,12 @@ public LeftFollow(PathPlannerPath... paths) { // Back new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new WaitCommand(2.0).andThen( + new WaitCommand(3.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) .andThen( new ParallelCommandGroup( new HandoffRun(), - new SpindexerRun()) - ) + new SpindexerRun())) ) ), 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 9e1fa85e..c4e05f49 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 @@ -58,7 +58,7 @@ public LeftTwoCorner(PathPlannerPath... paths) { 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 WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -72,7 +72,7 @@ public LeftTwoCorner(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) @@ -80,7 +80,7 @@ public LeftTwoCorner(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) ); 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 90677d78..a6586094 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 @@ -58,7 +58,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { 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 WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -72,7 +72,7 @@ public LeftTwoCycle(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) @@ -80,7 +80,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java index 13785df2..6e0ea55e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java @@ -54,13 +54,12 @@ public RightFollow(PathPlannerPath... paths) { // SOTM To Corner new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new WaitCommand(2.0).andThen( + new WaitCommand(3.0).andThen( new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) .andThen( new ParallelCommandGroup( new HandoffRun(), - new SpindexerRun()) - ) + new SpindexerRun())) ) ), 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 f3e3a55c..d8aed42f 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 @@ -58,7 +58,7 @@ public RightTwoCorner(PathPlannerPath... paths) { 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 WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -72,7 +72,7 @@ public RightTwoCorner(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) @@ -80,7 +80,7 @@ public RightTwoCorner(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) ); 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 33aef561..2e860ec2 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 @@ -58,7 +58,7 @@ public RightTwoCycle(PathPlannerPath... paths) { 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 WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -72,7 +72,7 @@ public RightTwoCycle(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) @@ -80,7 +80,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) ); diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java index e74cc945..58b14871 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java @@ -9,9 +9,10 @@ public IntakeAutoDigest() { addCommands( - new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.5)).andThen( - new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.5))).andThen( - new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.5))) + new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.5)).andThen( + new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy())).andThen(new WaitCommand(0.5)).andThen( + new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()) + ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeTeleopDigest.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeTeleopDigest.java new file mode 100644 index 00000000..f805a619 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeTeleopDigest.java @@ -0,0 +1,18 @@ +package com.stuypulse.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class IntakeTeleopDigest extends SequentialCommandGroup { + + public IntakeTeleopDigest() { + + addCommands( + + new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.5)) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 8d7b08dc..7a504b27 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -64,7 +64,7 @@ public interface Handoff { public interface Intake { Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(71.0); Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(-10.0); - Rotation2d PIVOT_DIGEST_ANGLE = Rotation2d.fromDegrees(50); + Rotation2d PIVOT_DIGEST_ANGLE = Rotation2d.fromDegrees(30); Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(5.0); @@ -163,16 +163,14 @@ public interface FerryRPMInterpolation { //Lab {1, 2000}, {5.16, 3300.0}, - {6.94, 3400}, - {9.77, 3600}, //TODO: USE THE BELOW THREE LINES INTEAD OF THESE - // {6.94, 3600.0}, - // {7.87, 3800.0}, - // {9.77, 4300.0}, - // {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS UNRELIABLE!!! - // {11.516, 4900.0}, - // {12.416, 5200.0}, - // {13.316, 5500.0}, - // {14.216, 5600.0} + {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}, + {12.416, 5200.0}, + {13.316, 5500.0}, + {14.216, 5600.0} }; } 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 9142a37d..30bdae5b 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(0.75)); + .filtered(new BDebounce.Falling(1.0)); PhoenixUtil.registerToRio(shooterLeaderSpeed, shooterFollowerSpeed, shooterFollowSupplyCurrent, shooterFollowStatorCurrent, shooterLeadSupplyCurrent, shooterLeadStatorCurrent,