From bd67c9e8d346411f26ec1134176f3956e5dd047f Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Fri, 28 Feb 2025 18:46:30 -0700 Subject: [PATCH] a small change, autos work. --- .../pathplanner/autos/Blue Top Cage.auto | 16 +----- .../pathplanner/autos/Red Bottom Cage.auto | 16 +----- .../pathplanner/autos/Red Center Cage.auto | 22 +++----- .../deploy/pathplanner/autos/Stupid auto.auto | 31 ----------- .../deploy/pathplanner/autos/Test Auto.auto | 19 ------- .../paths/Bottom Blue Cage to Reef.path | 4 +- .../paths/Bottom Feed to Reef.path | 16 +++--- .../paths/Bottom Reef to Feed.path | 12 ++--- .../paths/Center Barge to Reef.path | 12 ++--- .../paths/Center Blue Cage to Reef.path | 4 +- .../paths/Center Red Cage to Reef.path | 6 +-- .../paths/Center Reef to Bottom Feed.path | 12 ++--- .../paths/Center Reef to Feed.path | 24 ++++----- .../pathplanner/paths/Stupid auto 1.path | 54 ------------------- .../pathplanner/paths/Stupid auto 2.path | 54 ------------------- .../pathplanner/paths/Stupid auto 3.path | 54 ------------------- src/main/deploy/pathplanner/paths/Taxi.path | 4 +- .../paths/Top Blue Cage to Reef.path | 8 +-- .../pathplanner/paths/Top Feed to Reef.path | 20 +++---- .../paths/Top Red Cage to Reef.path | 12 ++--- .../pathplanner/paths/Top Reef to Feed.path | 16 +++--- .../java/frc4388/robot/RobotContainer.java | 15 ++++-- .../frc4388/robot/commands/waitFeedCoral.java | 36 +++++++++++++ .../frc4388/robot/subsystems/Elevator.java | 4 ++ 24 files changed, 139 insertions(+), 332 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Stupid auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/Test Auto.auto delete mode 100644 src/main/deploy/pathplanner/paths/Stupid auto 1.path delete mode 100644 src/main/deploy/pathplanner/paths/Stupid auto 2.path delete mode 100644 src/main/deploy/pathplanner/paths/Stupid auto 3.path create mode 100644 src/main/java/frc4388/robot/commands/waitFeedCoral.java diff --git a/src/main/deploy/pathplanner/autos/Blue Top Cage.auto b/src/main/deploy/pathplanner/autos/Blue Top Cage.auto index 3acc852..555bb6c 100644 --- a/src/main/deploy/pathplanner/autos/Blue Top Cage.auto +++ b/src/main/deploy/pathplanner/autos/Blue Top Cage.auto @@ -13,13 +13,7 @@ { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-left-l4" } }, { @@ -43,13 +37,7 @@ { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-left-l4" } } ] diff --git a/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto b/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto index 658c0b4..3439f6b 100644 --- a/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto +++ b/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto @@ -13,13 +13,7 @@ { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-right-l4" } }, { @@ -43,13 +37,7 @@ { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-left-l4" } } ] diff --git a/src/main/deploy/pathplanner/autos/Red Center Cage.auto b/src/main/deploy/pathplanner/autos/Red Center Cage.auto index 47338f7..7bfeb66 100644 --- a/src/main/deploy/pathplanner/autos/Red Center Cage.auto +++ b/src/main/deploy/pathplanner/autos/Red Center Cage.auto @@ -13,13 +13,7 @@ { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-right-l4" } }, { @@ -28,6 +22,12 @@ "pathName": "Bottom Reef to Feed" } }, + { + "type": "named", + "data": { + "name": "align-feed" + } + }, { "type": "named", "data": { @@ -43,13 +43,7 @@ { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-left-l4" } } ] diff --git a/src/main/deploy/pathplanner/autos/Stupid auto.auto b/src/main/deploy/pathplanner/autos/Stupid auto.auto deleted file mode 100644 index 06f26cf..0000000 --- a/src/main/deploy/pathplanner/autos/Stupid auto.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Stupid auto 1" - } - }, - { - "type": "path", - "data": { - "pathName": "Stupid auto 2" - } - }, - { - "type": "path", - "data": { - "pathName": "Stupid auto 3" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test Auto.auto b/src/main/deploy/pathplanner/autos/Test Auto.auto deleted file mode 100644 index 2e09e4c..0000000 --- a/src/main/deploy/pathplanner/autos/Test Auto.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Stupid auto 1" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path index 0670521..f63510a 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 60.35013649242443 + "rotation": -121.26879518614867 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 179.34296858150984 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path index cadc03f..25f3281 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.115768324814807, - "y": 1.5107373181346522 + "x": 4.39747159090909, + "y": 1.3027556818181814 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.692929292929293, - "y": 2.6833333333333327 + "x": 5.3048863636363635, + "y": 2.5292613636363637 }, "prevControl": { - "x": 3.11332763609877, - "y": 2.3131602797293294 + "x": 5.893210227272728, + "y": 1.9808238636363626 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -121.27770286686648 + "rotation": 114.22774531795413 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -126.57303097851931 + "rotation": 53.446462811652175 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path b/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path index 0e53fb5..6bb67cc 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.1565656565656575, - "y": 2.886616161616162 + "x": 5.344772727272727, + "y": 2.6688636363636364 }, "prevControl": null, "nextControl": { - "x": 5.532638888888889, - "y": 2.530871212121211 + "x": 6.4715625, + "y": 1.432386363636364 }, "isLocked": false, "linkedName": null @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -123.50343698241421 + "rotation": 55.05578949900953 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -59.036243467926624 + "rotation": 121.42956561483854 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Barge to Reef.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef.path index ebcc6cf..cfbbb95 100644 --- a/src/main/deploy/pathplanner/paths/Center Barge to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Barge to Reef.path @@ -8,19 +8,19 @@ }, "prevControl": null, "nextControl": { - "x": 8.114457611671098, - "y": 4.043616195025507 + "x": 7.269289772727273, + "y": 4.064886363636362 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.979861111111112, + "x": 6.172414772727272, "y": 4.045328282828282 }, "prevControl": { - "x": 5.5035379766577375, + "x": 5.696091638273898, "y": 4.0462802990619195 }, "nextControl": null, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.5456575934157175 + "rotation": -179.96096735022735 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -179.2890804030095 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path index 2e99234..434e054 100644 --- a/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 60.851928154286895 + "rotation": -120.86136963407526 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 1.1457628387483283 + "rotation": -179.99261564513898 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path index a2d7feb..7e6f511 100644 --- a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 5.96969696969697, - "y": 1.128219696969696 + "y": 1.1282196969696958 }, "isLocked": false, "linkedName": null @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -60.52411099675423 + "rotation": 122.87136694597014 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 1.97493401088202 + "rotation": 179.5295681977034 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path index 07e870b..200490f 100644 --- a/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.786742424242425, - "y": 4.035164141414142 + "x": 6.092642045454545, + "y": 3.9651704545454542 }, "prevControl": null, "nextControl": { - "x": 7.026767676767678, - "y": 2.185290404040403 + "x": 9.024289772727268, + "y": 3.446647727272728 }, "isLocked": false, "linkedName": null @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -124.91183021661 + "rotation": 52.857102599919905 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -1.6365770416167795 + "rotation": -179.46454101443553 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Reef to Feed.path b/src/main/deploy/pathplanner/paths/Center Reef to Feed.path index 891990b..5bee8d7 100644 --- a/src/main/deploy/pathplanner/paths/Center Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Center Reef to Feed.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 5.817234848484849, - "y": 4.045328282828282 + "x": 5.982954545454545, + "y": 4.03497159090909 }, "prevControl": null, "nextControl": { - "x": 6.62020202020202, - "y": 4.797474747474748 + "x": 7.109744318181819, + "y": 3.8355397727272718 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.2175505050505055, - "y": 5.45814393939394 + "x": 6.232244318181818, + "y": 5.281420454545455 }, "prevControl": { - "x": 5.45268897997343, - "y": 5.373232823449551 + "x": 7.1397939460232385, + "y": 4.676387369317844 }, "nextControl": { - "x": 3.388005050505051, - "y": 6.1188131313131295 + "x": 5.574119318181818, + "y": 5.720170454545454 }, "isLocked": false, "linkedName": null @@ -58,13 +58,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 126.3843518158359 + "rotation": -53.93780932479241 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 179.1574757392596 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Stupid auto 1.path b/src/main/deploy/pathplanner/paths/Stupid auto 1.path deleted file mode 100644 index afb7b0c..0000000 --- a/src/main/deploy/pathplanner/paths/Stupid auto 1.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 6.081502525252525, - "y": 7.267361111111111 - }, - "prevControl": null, - "nextControl": { - "x": 4.871969696969696, - "y": 8.070328282828283 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.081502525252525, - "y": 5.10239898989899 - }, - "prevControl": { - "x": 4.821148989898989, - "y": 5.10239898989899 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Stupid auto 2.path b/src/main/deploy/pathplanner/paths/Stupid auto 2.path deleted file mode 100644 index a849084..0000000 --- a/src/main/deploy/pathplanner/paths/Stupid auto 2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 6.101830808080808, - "y": 5.041414141414141 - }, - "prevControl": null, - "nextControl": { - "x": 7.10183080808081, - "y": 5.041414141414141 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.585795454545455, - "y": 6.149305555555555 - }, - "prevControl": { - "x": 8.175315656565658, - "y": 5.295517676767678 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Stupid auto 3.path b/src/main/deploy/pathplanner/paths/Stupid auto 3.path deleted file mode 100644 index c87eab2..0000000 --- a/src/main/deploy/pathplanner/paths/Stupid auto 3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.585795454545455, - "y": 6.200126262626262 - }, - "prevControl": null, - "nextControl": { - "x": 7.778914141414141, - "y": 7.033585858585859 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.081502525252525, - "y": 7.32834595959596 - }, - "prevControl": { - "x": 7.595959595959596, - "y": 7.724747474747475 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path index 7b32a44..c4ec989 100644 --- a/src/main/deploy/pathplanner/paths/Taxi.path +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -58,13 +58,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -1.9251837083231407 + "rotation": 179.59913485447024 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -1.1306909512379957 + "rotation": -179.85896618052556 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path index 321e817..66ebba7 100644 --- a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.464431818181818, - "y": 5.6403977272727275 + "x": 5.344772727272727, + "y": 5.391107954545454 }, "prevControl": { - "x": 5.240820707070706, - "y": 5.254160353535354 + "x": 5.121161616161616, + "y": 5.00487058080808 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Top Feed to Reef.path b/src/main/deploy/pathplanner/paths/Top Feed to Reef.path index be71b4a..fce25be 100644 --- a/src/main/deploy/pathplanner/paths/Top Feed to Reef.path +++ b/src/main/deploy/pathplanner/paths/Top Feed to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.0807449494949497, - "y": 6.972601010101011 + "x": 1.116818181818182, + "y": 6.956647727272727 }, "prevControl": null, "nextControl": { - "x": 2.0869949494949496, - "y": 6.291603535353536 + "x": 2.123068181818182, + "y": 6.275650252525252 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.764078282828282, - "y": 5.24469696969697 + "x": 3.6196875, + "y": 5.35122159090909 }, "prevControl": { - "x": 2.9836492542425197, - "y": 5.685899280282051 + "x": 2.8392584714142375, + "y": 5.792423901494171 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 121.34521414171566 + "rotation": -59.239047023115106 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 124.23611780915067 + "rotation": -52.93323259086456 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path index cfabb8b..0cefb5a 100644 --- a/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 8.242449416720314, - "y": 3.1104783377417067 + "x": 7.319147727272727, + "y": 2.3198579545454536 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.6934974747474745 }, "prevControl": { - "x": 4.985594317577342, - "y": 2.6542548990156853 + "x": 6.401761363636364, + "y": 1.5819602272727276 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -54.659893078442295 + "rotation": 123.49004753500587 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -178.9832812445108 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Reef to Feed.path b/src/main/deploy/pathplanner/paths/Top Reef to Feed.path index e9543c9..40748ed 100644 --- a/src/main/deploy/pathplanner/paths/Top Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Top Reef to Feed.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.17689393939394, - "y": 5.224368686868687 + "x": 5.284943181818181, + "y": 5.401079545454545 }, "prevControl": null, "nextControl": { - "x": 5.9669821266547345, - "y": 4.892012503312241 + "x": 4.844554924242423, + "y": 6.744466540404039 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 7.003093434343434 }, "prevControl": { - "x": 0.960567140436829, - "y": 7.086188101328755 + "x": 1.9644034090909093, + "y": 5.750085227272726 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 126.86989764584399 + "rotation": -54.83470564502973 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 57.21571913413089 + "rotation": -121.0370223454415 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 39cb625..3a4b441 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -47,6 +47,7 @@ import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.waitElevatorRefrence; import frc4388.robot.commands.waitEndefectorRefrence; +import frc4388.robot.commands.waitFeedCoral; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; @@ -295,6 +296,13 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + + NamedCommands.registerCommand("grab-coral", new waitFeedCoral(m_robotElevator)); + NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0, 1), + new Translation2d(), 200, true + ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); @@ -642,9 +650,9 @@ public class RobotContainer { // return autoCommand; // } catch (Exception e) { // DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); - // return autoCommand; + return autoCommand; // } - return new PathPlannerAuto("Line-up-no-arm"); + // return new PathPlannerAuto("Line-up-no-arm"); // zach told me to do the below comment //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); // return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); @@ -659,7 +667,8 @@ public class RobotContainer { for (String auto : autos) { if (auto.endsWith(".auto")) - autoChooser.addOption(auto.replaceAll(".auto", ""), auto); + autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", "")); + // System.out.println(auto); } autoChooser.onChange((filename) -> { diff --git a/src/main/java/frc4388/robot/commands/waitFeedCoral.java b/src/main/java/frc4388/robot/commands/waitFeedCoral.java new file mode 100644 index 0000000..e1e32f6 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/waitFeedCoral.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.Elevator; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class waitFeedCoral extends Command { + /** Creates a new waitElevatorRefrence. */ + private Elevator elevator; + public waitFeedCoral(Elevator elevator) { + this.elevator = elevator; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return elevator.hasCoral(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 0389d90..bd08f8b 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -301,6 +301,10 @@ public class Elevator extends Subsystem { return currentState == CoordinationState.PrimedThree; } + public boolean hasCoral(){ + return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get(); + } + public void armShuffle(){ if(!basinBeamBreak.get()){ //shuffle the coral with the arm until coral hits beam break