From c8b7359b996564d35b992dfd0347e432f5b4abea Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Mon, 9 Mar 2026 12:17:42 +0800 Subject: [PATCH] =?UTF-8?q?=E8=87=AA=E5=8A=A8=E7=9E=84=E5=87=86+=E5=B0=84?= =?UTF-8?q?=E7=90=83=E5=B7=B2=E5=B0=81=E8=A3=85=20=E5=A2=9E=E5=8A=A0convey?= =?UTF-8?q?or=E7=8B=AC=E7=AB=8B=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ...nt.auto => 8 Balls Auto Middle Start.auto} | 2 +- .../autos/8 Balls and Climb Middle Start.auto | 12 ++- .../pathplanner/autos/Copy of Depot Auto.auto | 101 ++++++++++++++++++ .../deploy/pathplanner/autos/Depot Auto.auto | 6 +- src/main/deploy/pathplanner/autos/Line.auto | 19 ---- .../pathplanner/autos/LineGoAndBack.auto | 37 ------- src/main/deploy/pathplanner/autos/Rotate.auto | 19 ---- .../pathplanner/autos/RotateAndLine.auto | 19 ---- ...edBackAndShoot.path => Back To Shoot.path} | 14 +-- ...Climb Path step2.path => Climb Step2.path} | 10 +- .../deploy/pathplanner/paths/Climb Step3.path | 54 ++++++++++ ...depotIntakePath.path => Depot Intake.path} | 2 +- ...epotShootPath.path => Depot to Shoot.path} | 10 +- .../deploy/pathplanner/paths/LineBack.path | 54 ---------- src/main/deploy/pathplanner/paths/Rotate.path | 54 ---------- .../pathplanner/paths/RotateAndLine.path | 54 ---------- ...h step1.path => Shoot to Climb Step1.path} | 22 ++-- .../pathplanner/paths/Shoot to Red Depot.path | 54 ++++++++++ .../{redDepotPath.path => To Red Depot.path} | 2 +- src/main/deploy/pathplanner/settings.json | 4 +- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 7 +- .../java/frc/robot/controls/Autocommand.java | 26 +++-- .../frc/robot/controls/DriveControls.java | 17 +++ .../frc/robot/controls/ShooterControls.java | 50 ++++++++- .../robot/subsystems/ClimberSubsystem.java | 7 ++ 26 files changed, 351 insertions(+), 306 deletions(-) rename src/main/deploy/pathplanner/autos/{8 Balls Auto Middle Start Point.auto => 8 Balls Auto Middle Start.auto} (89%) create mode 100644 src/main/deploy/pathplanner/autos/Copy of Depot Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/Line.auto delete mode 100644 src/main/deploy/pathplanner/autos/LineGoAndBack.auto delete mode 100644 src/main/deploy/pathplanner/autos/Rotate.auto delete mode 100644 src/main/deploy/pathplanner/autos/RotateAndLine.auto rename src/main/deploy/pathplanner/paths/{redBackAndShoot.path => Back To Shoot.path} (84%) rename src/main/deploy/pathplanner/paths/{Climb Path step2.path => Climb Step2.path} (85%) create mode 100644 src/main/deploy/pathplanner/paths/Climb Step3.path rename src/main/deploy/pathplanner/paths/{depotIntakePath.path => Depot Intake.path} (97%) rename src/main/deploy/pathplanner/paths/{depotShootPath.path => Depot to Shoot.path} (85%) delete mode 100644 src/main/deploy/pathplanner/paths/LineBack.path delete mode 100644 src/main/deploy/pathplanner/paths/Rotate.path delete mode 100644 src/main/deploy/pathplanner/paths/RotateAndLine.path rename src/main/deploy/pathplanner/paths/{Climb Path step1.path => Shoot to Climb Step1.path} (77%) create mode 100644 src/main/deploy/pathplanner/paths/Shoot to Red Depot.path rename src/main/deploy/pathplanner/paths/{redDepotPath.path => To Red Depot.path} (97%) diff --git a/src/main/deploy/pathplanner/autos/8 Balls Auto Middle Start Point.auto b/src/main/deploy/pathplanner/autos/8 Balls Auto Middle Start.auto similarity index 89% rename from src/main/deploy/pathplanner/autos/8 Balls Auto Middle Start Point.auto rename to src/main/deploy/pathplanner/autos/8 Balls Auto Middle Start.auto index 337a25e..0a13ea6 100644 --- a/src/main/deploy/pathplanner/autos/8 Balls Auto Middle Start Point.auto +++ b/src/main/deploy/pathplanner/autos/8 Balls Auto Middle Start.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "redBackAndShoot" + "pathName": "Back To Shoot" } }, { diff --git a/src/main/deploy/pathplanner/autos/8 Balls and Climb Middle Start.auto b/src/main/deploy/pathplanner/autos/8 Balls and Climb Middle Start.auto index 7414ac5..4e3ce49 100644 --- a/src/main/deploy/pathplanner/autos/8 Balls and Climb Middle Start.auto +++ b/src/main/deploy/pathplanner/autos/8 Balls and Climb Middle Start.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "redBackAndShoot" + "pathName": "Back To Shoot" } }, { @@ -19,13 +19,19 @@ { "type": "path", "data": { - "pathName": "Climb Path step1" + "pathName": "Shoot to Climb Step1" } }, { "type": "path", "data": { - "pathName": "Climb Path step2" + "pathName": "Climb Step2" + } + }, + { + "type": "path", + "data": { + "pathName": "Climb Step3" } } ] diff --git a/src/main/deploy/pathplanner/autos/Copy of Depot Auto.auto b/src/main/deploy/pathplanner/autos/Copy of Depot Auto.auto new file mode 100644 index 0000000..755d339 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Copy of Depot Auto.auto @@ -0,0 +1,101 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Back To Shoot" + } + }, + { + "type": "named", + "data": { + "name": "intaker_down" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Shoot to Red Depot" + } + }, + { + "type": "named", + "data": { + "name": "start_intake_timelimit" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "start_intake_timelimit" + } + }, + { + "type": "path", + "data": { + "pathName": "Depot Intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intaker_up" + } + }, + { + "type": "path", + "data": { + "pathName": "Depot to Shoot" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Depot Auto.auto b/src/main/deploy/pathplanner/autos/Depot Auto.auto index 4033630..0b2e80a 100644 --- a/src/main/deploy/pathplanner/autos/Depot Auto.auto +++ b/src/main/deploy/pathplanner/autos/Depot Auto.auto @@ -17,7 +17,7 @@ { "type": "path", "data": { - "pathName": "redDepotPath" + "pathName": "To Red Depot" } } ] @@ -42,7 +42,7 @@ { "type": "path", "data": { - "pathName": "depotIntakePath" + "pathName": "Depot Intake" } } ] @@ -67,7 +67,7 @@ { "type": "path", "data": { - "pathName": "depotShootPath" + "pathName": "Depot to Shoot" } } ] diff --git a/src/main/deploy/pathplanner/autos/Line.auto b/src/main/deploy/pathplanner/autos/Line.auto deleted file mode 100644 index 85713c6..0000000 --- a/src/main/deploy/pathplanner/autos/Line.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Climb Path step2" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LineGoAndBack.auto b/src/main/deploy/pathplanner/autos/LineGoAndBack.auto deleted file mode 100644 index 1ad38ed..0000000 --- a/src/main/deploy/pathplanner/autos/LineGoAndBack.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Climb Path step2" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "LineBack" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Rotate.auto b/src/main/deploy/pathplanner/autos/Rotate.auto deleted file mode 100644 index 20377f4..0000000 --- a/src/main/deploy/pathplanner/autos/Rotate.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Rotate" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RotateAndLine.auto b/src/main/deploy/pathplanner/autos/RotateAndLine.auto deleted file mode 100644 index b696103..0000000 --- a/src/main/deploy/pathplanner/autos/RotateAndLine.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "RotateAndLine" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/redBackAndShoot.path b/src/main/deploy/pathplanner/paths/Back To Shoot.path similarity index 84% rename from src/main/deploy/pathplanner/paths/redBackAndShoot.path rename to src/main/deploy/pathplanner/paths/Back To Shoot.path index 504e0c2..d7af0a4 100644 --- a/src/main/deploy/pathplanner/paths/redBackAndShoot.path +++ b/src/main/deploy/pathplanner/paths/Back To Shoot.path @@ -4,24 +4,24 @@ { "anchor": { "x": 13.01022182786158, - "y": 4.119503105590061 + "y": 4.0 }, "prevControl": null, "nextControl": { "x": 13.865906250551847, - "y": 4.119503105590061 + "y": 4.0 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 13.59, - "y": 4.12 + "x": 13.5, + "y": 4.0 }, "prevControl": { - "x": 13.321071353115471, - "y": 4.12 + "x": 13.231071353115471, + "y": 4.0 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Red Middle", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Climb Path step2.path b/src/main/deploy/pathplanner/paths/Climb Step2.path similarity index 85% rename from src/main/deploy/pathplanner/paths/Climb Path step2.path rename to src/main/deploy/pathplanner/paths/Climb Step2.path index fb829dd..6e68a52 100644 --- a/src/main/deploy/pathplanner/paths/Climb Path step2.path +++ b/src/main/deploy/pathplanner/paths/Climb Step2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 15.650000000000002, - "y": 3.365 + "x": 15.610835339716738, + "y": 3.3590426144037364 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 3.365 }, "prevControl": { - "x": 15.935706120733446, - "y": 3.365 + "x": 15.743555432745389, + "y": 3.3550226257587115 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Red Middle", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Climb Step3.path b/src/main/deploy/pathplanner/paths/Climb Step3.path new file mode 100644 index 0000000..bcac179 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Climb Step3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 15.467, + "y": 3.365 + }, + "prevControl": null, + "nextControl": { + "x": 15.716589593303949, + "y": 3.3506809597259686 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 15.467, + "y": 3.465 + }, + "prevControl": { + "x": 15.191301517038063, + "y": 3.4933990509259307 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.25, + "maxAcceleration": 0.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Red Middle", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depotIntakePath.path b/src/main/deploy/pathplanner/paths/Depot Intake.path similarity index 97% rename from src/main/deploy/pathplanner/paths/depotIntakePath.path rename to src/main/deploy/pathplanner/paths/Depot Intake.path index 9759181..477aaa1 100644 --- a/src/main/deploy/pathplanner/paths/depotIntakePath.path +++ b/src/main/deploy/pathplanner/paths/Depot Intake.path @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Red Middle", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/depotShootPath.path b/src/main/deploy/pathplanner/paths/Depot to Shoot.path similarity index 85% rename from src/main/deploy/pathplanner/paths/depotShootPath.path rename to src/main/deploy/pathplanner/paths/Depot to Shoot.path index 5e81694..3550f71 100644 --- a/src/main/deploy/pathplanner/paths/depotShootPath.path +++ b/src/main/deploy/pathplanner/paths/Depot to Shoot.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 16.145956799790877, - "y": 1.9416406158462824 + "x": 16.024211489072027, + "y": 1.797481861769637 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 4.0 }, "prevControl": { - "x": 13.28182215800934, - "y": 4.126069541450674 + "x": 13.313026105373341, + "y": 4.165954098256533 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Red Middle", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/LineBack.path b/src/main/deploy/pathplanner/paths/LineBack.path deleted file mode 100644 index c642657..0000000 --- a/src/main/deploy/pathplanner/paths/LineBack.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 14.314, - "y": 6.668017118402283 - }, - "prevControl": null, - "nextControl": { - "x": 14.299415121255347, - "y": 7.128787446504993 - }, - "isLocked": false, - "linkedName": "Point1" - }, - { - "anchor": { - "x": 14.314, - "y": 4.15 - }, - "prevControl": { - "x": 14.299415121255347, - "y": 4.597831669044222 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "StartingPointRed2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Rotate.path b/src/main/deploy/pathplanner/paths/Rotate.path deleted file mode 100644 index b509010..0000000 --- a/src/main/deploy/pathplanner/paths/Rotate.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 14.314, - "y": 4.15 - }, - "prevControl": null, - "nextControl": { - "x": 14.064, - "y": 4.15 - }, - "isLocked": false, - "linkedName": "StartingPointRed1" - }, - { - "anchor": { - "x": 14.314, - "y": 4.15 - }, - "prevControl": { - "x": 14.650181888846735, - "y": 4.15 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RotateAndLine.path b/src/main/deploy/pathplanner/paths/RotateAndLine.path deleted file mode 100644 index 968b692..0000000 --- a/src/main/deploy/pathplanner/paths/RotateAndLine.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 14.314, - "y": 4.15 - }, - "prevControl": null, - "nextControl": { - "x": 14.312353780313838, - "y": 4.74015691868759 - }, - "isLocked": false, - "linkedName": "StartingPointRed1" - }, - { - "anchor": { - "x": 14.314, - "y": 6.357489300998574 - }, - "prevControl": { - "x": 14.312353780313838, - "y": 6.098716119828817 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Climb Path step1.path b/src/main/deploy/pathplanner/paths/Shoot to Climb Step1.path similarity index 77% rename from src/main/deploy/pathplanner/paths/Climb Path step1.path rename to src/main/deploy/pathplanner/paths/Shoot to Climb Step1.path index f997ea9..109f8ca 100644 --- a/src/main/deploy/pathplanner/paths/Climb Path step1.path +++ b/src/main/deploy/pathplanner/paths/Shoot to Climb Step1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 13.59, - "y": 4.12 + "x": 13.5, + "y": 4.0 }, "prevControl": null, "nextControl": { - "x": 14.266378040870812, - "y": 3.6432882001482505 + "x": 13.951314803780512, + "y": 3.7446544288446453 }, "isLocked": true, "linkedName": null @@ -20,12 +20,12 @@ "y": 3.3650694444444444 }, "prevControl": { - "x": 14.336543591648573, - "y": 3.4619706510920394 + "x": 14.32971291116739, + "y": 3.4437767342805516 }, "nextControl": { - "x": 14.797456408351428, - "y": 3.2681682377968495 + "x": 14.804287088832611, + "y": 3.2863621546083372 }, "isLocked": true, "linkedName": null @@ -36,8 +36,8 @@ "y": 3.365 }, "prevControl": { - "x": 15.652649792454762, - "y": 3.401302545741686 + "x": 15.593954129696199, + "y": 3.387182687738287 }, "nextControl": null, "isLocked": false, @@ -61,7 +61,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Red Middle", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Shoot to Red Depot.path b/src/main/deploy/pathplanner/paths/Shoot to Red Depot.path new file mode 100644 index 0000000..f821231 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot to Red Depot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 13.5, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 13.72461981440462, + "y": 3.8902460070118927 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 15.400452528841061, + "y": 2.1 + }, + "prevControl": { + "x": 15.201612527520673, + "y": 2.25153433233069 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Red Middle", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/redDepotPath.path b/src/main/deploy/pathplanner/paths/To Red Depot.path similarity index 97% rename from src/main/deploy/pathplanner/paths/redDepotPath.path rename to src/main/deploy/pathplanner/paths/To Red Depot.path index 7da48cb..517c7f3 100644 --- a/src/main/deploy/pathplanner/paths/redDepotPath.path +++ b/src/main/deploy/pathplanner/paths/To Red Depot.path @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Red Middle", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index f80d618..bfb13e2 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,7 +2,9 @@ "robotWidth": 0.85, "robotLength": 0.85, "holonomicMode": true, - "pathFolders": [], + "pathFolders": [ + "Red Middle" + ], "autoFolders": [], "defaultMaxVel": 1.5, "defaultMaxAccel": 3.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 795dfda..ae377b5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -149,6 +149,7 @@ public final class Constants { public static double climbUpTicks = 94; // TODO: 实测后调整 public static double climbDownTicks = 0; // TODO: 实测后调整 + public static double climbDownAutoTicks = -5; // TODO: 实测后调整，自动爬时可能不需要完全降到底 public static final double climbVelocity = -200; public static final double holdToleranceRot = 0.5; public static final double deltaDownTicks = 1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eaabbdb..1222335 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -76,7 +77,8 @@ public class RobotContainer { climberSubsystem, IntakerSubsystem, drivetrain, - shooterSubsystem + shooterSubsystem, + driveControls ); SignalLogger.enableAutoLogging(false); @@ -162,7 +164,8 @@ public class RobotContainer { controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100))); controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets())); controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand())); - controllerupper.b().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand())); + controllerupper.b().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(10), shooterSubsystem)); + controllerupper.a().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(-25), shooterSubsystem)); } /* ====================== */ /* LED 绑定 */ diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index e004191..e3b194b 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.subsystems.ClimberSubsystem; import frc.robot.RobotStatusManager; import frc.robot.subsystems.IntakerSubsystem; @@ -26,7 +27,9 @@ import frc.robot.Constants.RobotStatus; ClimberSubsystem climber, IntakerSubsystem intaker, CommandSwerveDrivetrain drivetrain, - ShooterSubsystem shooter + ShooterSubsystem shooter, + DriveControls driveControls, + ShooterControls shooterControls ) { NamedCommands.registerCommand("climb_up", climb_up(climber)); NamedCommands.registerCommand("climb_down", climb_down(climber)); @@ -37,20 +40,22 @@ import frc.robot.Constants.RobotStatus; NamedCommands.registerCommand("intaker_up", intaker_up(intaker)); NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain)); NamedCommands.registerCommand("shoot", shoot(shooter)); + NamedCommands.registerCommand("auto_aim_and_shoot", autoAimAndShoot(driveControls, shooterControls)); } public static Command climb_up(ClimberSubsystem climber) { return new RunCommand(() -> { climber.climbUp(); // 控制爬升器上升 - }, climber).withTimeout(1.0) // - .andThen(new InstantCommand(() -> { + }, climber).withTimeout(1.0).andThen(new RunCommand(() -> { + climber.climbUpNonStopforAuto(); + }, climber).withTimeout(0.5)).andThen(new InstantCommand(() -> { climber.holdPosition(); // 停止并锁定当前爬升器位置 }, climber)); } public static Command climb_down(ClimberSubsystem climber) { return new RunCommand(() -> { - climber.climbDown(); // 控制爬升器下降 - }, climber).withTimeout(1.0) // + climber.climbDownAuto(); // 控制爬升器下降 + }, climber).withTimeout(2.0) // .andThen(new InstantCommand(() -> { climber.holdPosition(); // 停止并锁定当前爬升器位置 }, climber)); @@ -100,7 +105,7 @@ import frc.robot.Constants.RobotStatus; }, drivetrain ).until(() -> drivetrain.isAlignedToAprilTag())); - } + } public static Command shoot(ShooterSubsystem shooter) { return new RunCommand(() -> { @@ -125,9 +130,14 @@ import frc.robot.Constants.RobotStatus; //shooter.setBackboardPosition(0); // 恢复背板位置 shooter.setConveyorSpeedByRPS(0); // 停止输送 }, shooter))); - - } + + public static Command autoAimAndShoot(DriveControls driveControls, ShooterControls shooterControl) { + return driveControls.autoAimCommand() + .andThen(shooterControl.autoShootToHubCommand()); + } + + } diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 7676dde..4e00c21 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -12,6 +12,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; import frc.robot.RobotStatusManager; @@ -414,6 +415,22 @@ public class DriveControls { drivetrain.stop(); } } + public Command autoAimCommand() { + return drivetrain.applyRequest(() -> { + double[] distanceAndRotation = calculateDistanceAndRotationToHub(); + double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]); + return fieldCentric + .withVelocityX(0.0) + .withVelocityY(0.0) + .withRotationalRate(vomega); + }, () -> DriveMode.FIELD_CENTRIC) + .until(() -> Math.abs(calculateDistanceAndRotationToHub()[1]) < 1.5) + .withTimeout(1.0) + .andThen(new InstantCommand(() -> { + drivetrain.stop(); + }, drivetrain)); + } + } diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index d2db669..753eb94 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -1,9 +1,13 @@ package frc.robot.controls; +import java.util.Set; + import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; import frc.robot.RobotStatusManager; @@ -147,8 +151,50 @@ public class ShooterControls { flywheelSpeedOffset = 0.0; backboardPositionOffset = 0.0; } - public void startconveyor(){ - shooterSubsystem.setConveyorSpeedByRPS(10); + public Command autoShootToHubCommand() { + return Commands.defer(() -> { + double x = shooterSubsystem.getDistanceToHub(); + x = Math.max(1.2, Math.min(4.5, x)); + + double flywheelTargetSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759; + double backboardTargetPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827; + double conveyorTargetSpeed = Constants.Shooter.conveyorSpeed; + + return new RunCommand(() -> { + shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed); + shooterSubsystem.setConveyorSpeedByRPS(0.0); + shooterSubsystem.setBackboardPosition(backboardTargetPosition); + if (shooterSubsystem.isBackboardAtTarget()) { + shooterSubsystem.backboardMotor.set(0); + } else { + shooterSubsystem.outputBackboard(); + } + }, shooterSubsystem) + .until(() -> + shooterSubsystem.isBackboardAtTarget() + && shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9 + ) + .withTimeout(1.0) + .andThen(new RunCommand(() -> { + shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed); + shooterSubsystem.setBackboardPosition(backboardTargetPosition); + if (shooterSubsystem.isBackboardAtTarget()) { + shooterSubsystem.backboardMotor.set(0); + } else { + shooterSubsystem.outputBackboard(); + } + if (shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9) { + shooterSubsystem.setConveyorSpeedByRPS(conveyorTargetSpeed); + } else { + shooterSubsystem.setConveyorSpeedByRPS(0.0); + } + }, shooterSubsystem).withTimeout(3.0)) + .andThen(new InstantCommand(() -> { + shooterSubsystem.setFlywheelSpeedByRPS(0.0); + shooterSubsystem.setConveyorSpeedByRPS(0.0); + shooterSubsystem.backboardMotor.set(0.0); + }, shooterSubsystem)); + }, Set.of(shooterSubsystem)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 10436d9..669a15a 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -70,6 +70,10 @@ public class ClimberSubsystem extends SubsystemBase { targetPositionTicks = Constants.Climber.climbDownTicks; climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks)); } + public void climbDownAuto() { + targetPositionTicks = Constants.Climber.climbDownAutoTicks; + climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks)); + } public void climbDownNonStop(){ climberMotor.setControl(velocityRequest.withVelocity(Constants.Climber.climbVelocity)); @@ -78,6 +82,9 @@ public class ClimberSubsystem extends SubsystemBase { public void climbUpNonStop(){ climberMotor.setControl(velocityRequest.withVelocity(-Constants.Climber.climbVelocity)); } + public void climbUpNonStopforAuto(){ + climberMotor.set(0.07); + } public void captureHoldPosition() { holdPositionTicks = getPositionTicks();