From 5323e48046fe81802c8d38dfc90e971eea970a9c Mon Sep 17 00:00:00 2001 From: bloobglob Date: Mon, 27 Nov 2023 18:20:39 -0800 Subject: [PATCH] commands in Trajectories.java --- .../pathplanner/r_cable_1Cone+1.5Cube.path | 225 --------------- .../pathplanner/r_cable_1Cone+1Cube.path | 144 ---------- .../pathplanner/r_cable_1Cone+2Cube.path | 258 ------------------ .../pathplanner/r_hp_1Cone+1.5Cube.path | 225 --------------- .../deploy/pathplanner/r_hp_1Cone+1Cube.path | 169 ------------ .../r_mid_1Cone+0.5Cube+Climb.path | 144 ---------- .../pathplanner/r_mid_1Cone+1Cube+Climb.path | 197 ------------- .../deploy/pathplanner/r_mid_1Cone+Climb.path | 81 ------ src/main/java/frc/team3128/Constants.java | 10 +- .../frc/team3128/autonomous/AutoPrograms.java | 6 +- .../frc/team3128/autonomous/Trajectories.java | 21 +- 11 files changed, 33 insertions(+), 1447 deletions(-) delete mode 100644 src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path delete mode 100644 src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path delete mode 100644 src/main/deploy/pathplanner/r_cable_1Cone+2Cube.path delete mode 100644 src/main/deploy/pathplanner/r_hp_1Cone+1.5Cube.path delete mode 100644 src/main/deploy/pathplanner/r_hp_1Cone+1Cube.path delete mode 100644 src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path delete mode 100644 src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path delete mode 100644 src/main/deploy/pathplanner/r_mid_1Cone+Climb.path diff --git a/src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path b/src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path deleted file mode 100644 index 222674c..0000000 --- a/src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path +++ /dev/null @@ -1,225 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.69, - "y": 0.5 - }, - "prevControl": null, - "nextControl": { - "x": 13.925675879064304, - "y": 0.5521063911206201 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.83, - "y": 0.6 - }, - "prevControl": { - "x": 13.453074555425331, - "y": 0.6 - }, - "nextControl": { - "x": 12.438365244862162, - "y": 0.6 - }, - "holonomicAngle": 180.01, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.85, - "y": 0.7 - }, - "prevControl": { - "x": 10.801405343067072, - "y": 0.6048694133149678 - }, - "nextControl": { - "x": 10.801405343067072, - "y": 0.6048694133149678 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.84, - "y": 0.6 - }, - "prevControl": { - "x": 12.30896602199829, - "y": 0.6 - }, - "nextControl": { - "x": 13.4317325721428, - "y": 0.6 - }, - "holonomicAngle": 179.99, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.69, - "y": 1.1 - }, - "prevControl": { - "x": 14.051726678100353, - "y": 0.6632923124532808 - }, - "nextControl": { - "x": 14.051726678100353, - "y": 0.6632923124532808 - }, - "holonomicAngle": 0.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [ - "ScoreCubeHigh" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.8, - "y": 0.7089983937457166 - }, - "prevControl": { - "x": 13.097321907054658, - "y": 0.7089983937457166 - }, - "nextControl": { - "x": 12.499180067821252, - "y": 0.7089983937457166 - }, - "holonomicAngle": 180.01, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.81, - "y": 1.9897696856769331 - }, - "prevControl": { - "x": 10.882701100548172, - "y": 0.20449525333979146 - }, - "nextControl": { - "x": 10.882701100548172, - "y": 0.20449525333979146 - }, - "holonomicAngle": 121.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 11.88, - "y": 0.5717728981820299 - }, - "prevControl": { - "x": 11.353965383041718, - "y": 0.49174802863363587 - }, - "nextControl": null, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 4.5, - "maxAcceleration": 2.3, - "isReversed": null, - "markers": [ - { - "position": 1.1272727272727252, - "names": [ - "PickupCube" - ] - }, - { - "position": 2.5090909090909514, - "names": [ - "Neutral" - ] - }, - { - "position": 5.141818181818192, - "names": [ - "PickupCube" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path b/src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path deleted file mode 100644 index d5ac369..0000000 --- a/src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path +++ /dev/null @@ -1,144 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.69, - "y": 0.5 - }, - "prevControl": null, - "nextControl": { - "x": 13.925675879064304, - "y": 0.5521063911206201 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.83, - "y": 0.6 - }, - "prevControl": { - "x": 13.453074555425331, - "y": 0.6 - }, - "nextControl": { - "x": 12.438365244862162, - "y": 0.6 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.85, - "y": 0.7 - }, - "prevControl": { - "x": 10.801405343067072, - "y": 0.6048694133149678 - }, - "nextControl": { - "x": 10.801405343067072, - "y": 0.6048694133149678 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.84, - "y": 0.6 - }, - "prevControl": { - "x": 12.30896602199829, - "y": 0.6 - }, - "nextControl": { - "x": 13.4317325721428, - "y": 0.6 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.69, - "y": 1.1 - }, - "prevControl": { - "x": 14.051726678100353, - "y": 0.6632923124532808 - }, - "nextControl": null, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreCubeHigh" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 4.5, - "maxAcceleration": 2.3, - "isReversed": null, - "markers": [ - { - "position": 1.1272727272727252, - "names": [ - "PickupCube" - ] - }, - { - "position": 2.5090909090909514, - "names": [ - "Neutral" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_cable_1Cone+2Cube.path b/src/main/deploy/pathplanner/r_cable_1Cone+2Cube.path deleted file mode 100644 index d145b75..0000000 --- a/src/main/deploy/pathplanner/r_cable_1Cone+2Cube.path +++ /dev/null @@ -1,258 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.700000000000001, - "y": 0.5 - }, - "prevControl": null, - "nextControl": { - "x": 12.722549247876273, - "y": 0.8020895069567917 - }, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreLow" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 11.2214148445658, - "y": 0.93 - }, - "prevControl": { - "x": 12.192453048899203, - "y": 0.93 - }, - "nextControl": { - "x": 10.696591741051684, - "y": 0.93 - }, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.9, - "y": 0.932272484336635 - }, - "prevControl": { - "x": 10.337740836954193, - "y": 0.932272484336635 - }, - "nextControl": { - "x": 10.337740836954193, - "y": 0.932272484336635 - }, - "holonomicAngle": 180, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 11.22, - "y": 0.75 - }, - "prevControl": { - "x": 10.780755885058197, - "y": 0.75 - }, - "nextControl": { - "x": 11.659244114941805, - "y": 0.75 - }, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.700000000000001, - "y": 1.1 - }, - "prevControl": { - "x": 13.205762945206446, - "y": 0.7980883374961188 - }, - "nextControl": { - "x": 13.205762945206446, - "y": 0.7980883374961188 - }, - "holonomicAngle": 360.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [ - "ScoreLow" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 11.22, - "y": 0.75 - }, - "prevControl": { - "x": 12.808395184872733, - "y": 0.5641657253120875 - }, - "nextControl": { - "x": 10.69992909152485, - "y": 0.8108456893997114 - }, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.75, - "y": 1.9 - }, - "prevControl": { - "x": 10.486982200329498, - "y": 1.1630177996705018 - }, - "nextControl": { - "x": 10.486982200329498, - "y": 1.1630177996705018 - }, - "holonomicAngle": 135.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 11.22, - "y": 0.75 - }, - "prevControl": { - "x": 10.889317779330003, - "y": 0.850451468264047 - }, - "nextControl": { - "x": 11.731159063760767, - "y": 0.5947250101102777 - }, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.700000000000001, - "y": 1.0969582998193774 - }, - "prevControl": { - "x": 13.513737017751815, - "y": 0.7111380545677755 - }, - "nextControl": null, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreCubeHigh" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 4.5, - "maxAcceleration": 2.3, - "isReversed": null, - "markers": [ - { - "position": 1.0536931818181818, - "names": [ - "PickupCube" - ] - }, - { - "position": 2.921875000000001, - "names": [ - "Neutral" - ] - }, - { - "position": 5.5576704545454545, - "names": [ - "PickupCube" - ] - }, - { - "position": 6.715056818181819, - "names": [ - "Neutral" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_hp_1Cone+1.5Cube.path b/src/main/deploy/pathplanner/r_hp_1Cone+1.5Cube.path deleted file mode 100644 index 0140a0f..0000000 --- a/src/main/deploy/pathplanner/r_hp_1Cone+1.5Cube.path +++ /dev/null @@ -1,225 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.69, - "y": 4.85 - }, - "prevControl": null, - "nextControl": { - "x": 13.92571244244595, - "y": 4.797360022722226 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.83, - "y": 4.9 - }, - "prevControl": { - "x": 13.453074555425331, - "y": 4.9 - }, - "nextControl": { - "x": 12.438365244862162, - "y": 4.9 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.75, - "y": 4.75 - }, - "prevControl": { - "x": 10.701405343067073, - "y": 4.845130586685032 - }, - "nextControl": { - "x": 10.701405343067073, - "y": 4.845130586685032 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.83, - "y": 4.75 - }, - "prevControl": { - "x": 12.29896602199829, - "y": 4.75 - }, - "nextControl": { - "x": 13.4217325721428, - "y": 4.75 - }, - "holonomicAngle": 180.01, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.71, - "y": 4.3 - }, - "prevControl": { - "x": 14.071726678100354, - "y": 4.736707687546719 - }, - "nextControl": { - "x": 14.071726678100354, - "y": 4.736707687546719 - }, - "holonomicAngle": 0.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [ - "ScoreCubeHigh" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.83, - "y": 4.75 - }, - "prevControl": { - "x": 13.127321907054657, - "y": 4.75 - }, - "nextControl": { - "x": 12.529180067821251, - "y": 4.75 - }, - "holonomicAngle": 179.99, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.81, - "y": 3.55 - }, - "prevControl": { - "x": 10.913695057970052, - "y": 5.316281310801438 - }, - "nextControl": { - "x": 10.913695057970052, - "y": 5.316281310801438 - }, - "holonomicAngle": -121.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.83, - "y": 4.75 - }, - "prevControl": { - "x": 12.303965383041717, - "y": 4.830024869548394 - }, - "nextControl": null, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 4.5, - "maxAcceleration": 2.3, - "isReversed": null, - "markers": [ - { - "position": 0.7636363636363053, - "names": [ - "PickupCube" - ] - }, - { - "position": 2.5090909090909514, - "names": [ - "Neutral" - ] - }, - { - "position": 5.141818181818192, - "names": [ - "PickupCube" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_hp_1Cone+1Cube.path b/src/main/deploy/pathplanner/r_hp_1Cone+1Cube.path deleted file mode 100644 index 0874e2e..0000000 --- a/src/main/deploy/pathplanner/r_hp_1Cone+1Cube.path +++ /dev/null @@ -1,169 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.700000000000001, - "y": 5.0 - }, - "prevControl": null, - "nextControl": { - "x": 12.722549247876273, - "y": 4.697910493043208 - }, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 11.197733779205077, - "y": 4.688537765092648 - }, - "prevControl": { - "x": 12.16877198353848, - "y": 4.688537765092648 - }, - "nextControl": { - "x": 10.67291067569096, - "y": 4.688537765092648 - }, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.9, - "y": 4.5 - }, - "prevControl": { - "x": 10.337740836954193, - "y": 4.5 - }, - "nextControl": { - "x": 10.337740836954193, - "y": 4.5 - }, - "holonomicAngle": 180, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 11.22, - "y": 4.75 - }, - "prevControl": { - "x": 10.780755885058197, - "y": 4.75 - }, - "nextControl": { - "x": 11.659244114941805, - "y": 4.75 - }, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.700000000000001, - "y": 4.4 - }, - "prevControl": { - "x": 13.414461624756584, - "y": 4.773758748906387 - }, - "nextControl": { - "x": 13.414461624756584, - "y": 4.773758748906387 - }, - "holonomicAngle": 360.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [ - "ScoreCubeHigh" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.432763239278263, - "y": 4.802892344729054 - }, - "prevControl": { - "x": 13.473389913969557, - "y": 4.585618643419883 - }, - "nextControl": null, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 4.5, - "maxAcceleration": 2.3, - "isReversed": null, - "markers": [ - { - "position": 1.1, - "names": [ - "PickupCube" - ] - }, - { - "position": 2.997869318181817, - "names": [ - "Neutral" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path b/src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path deleted file mode 100644 index e08c386..0000000 --- a/src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path +++ /dev/null @@ -1,144 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.700000000000001, - "y": 2.2 - }, - "prevControl": null, - "nextControl": { - "x": 14.481498135565715, - "y": 2.412705184785369 - }, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.14250050270934, - "y": 2.7256082342025794 - }, - "prevControl": { - "x": 14.4162036890046, - "y": 2.6430913625716137 - }, - "nextControl": { - "x": 13.884005006900399, - "y": 2.803540244909385 - }, - "holonomicAngle": 180, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.684045240312702, - "y": 2.7413229722292773 - }, - "prevControl": { - "x": 13.321278199996293, - "y": 2.7413229722292773 - }, - "nextControl": { - "x": 12.0984969607009, - "y": 2.7413229722292773 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": 4.0, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.75, - "y": 2.0 - }, - "prevControl": { - "x": 10.421616589760959, - "y": 2.283677115965343 - }, - "nextControl": { - "x": 10.421616589760959, - "y": 2.283677115965343 - }, - "holonomicAngle": 180, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 11.044029665142663, - "y": 2.797223368752293 - }, - "prevControl": { - "x": 10.658409709874975, - "y": 2.6871042266390086 - }, - "nextControl": null, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "Balance" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 1.0, - "maxAcceleration": 2.0, - "isReversed": null, - "markers": [ - { - "position": 2.4062500000000004, - "names": [ - "PickupCube" - ] - }, - { - "position": 3.614488636363634, - "names": [ - "Neutral" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path b/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path deleted file mode 100644 index f785c96..0000000 --- a/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path +++ /dev/null @@ -1,197 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.69, - "y": 2.2 - }, - "prevControl": null, - "nextControl": { - "x": 14.471498788558998, - "y": 2.4127058555720837 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.13, - "y": 2.73 - }, - "prevControl": { - "x": 14.403699108564984, - "y": 2.647469604007534 - }, - "nextControl": { - "x": 13.871508355351597, - "y": 2.807944783618144 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.67, - "y": 2.7413229722292773 - }, - "prevControl": { - "x": 13.307232959683589, - "y": 2.7413229722292773 - }, - "nextControl": { - "x": 12.084451720388195, - "y": 2.7413229722292773 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": 4.0, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.83, - "y": 1.5 - }, - "prevControl": { - "x": 10.501607686442625, - "y": 1.2163018060055075 - }, - "nextControl": { - "x": 10.501607686442625, - "y": 1.2163018060055075 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 11.04, - "y": 2.27 - }, - "prevControl": { - "x": 10.694057790033689, - "y": 2.2145271064967047 - }, - "nextControl": { - "x": 11.435976301859341, - "y": 2.3334960134671374 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "Balance" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.13, - "y": 2.27 - }, - "prevControl": { - "x": 13.711099087944042, - "y": 2.27 - }, - "nextControl": { - "x": 14.599271960427549, - "y": 2.27 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.69, - "y": 2.8 - }, - "prevControl": { - "x": 14.497861376779507, - "y": 2.8 - }, - "nextControl": null, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreCubeHigh", - "Balance2" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 1.5, - "maxAcceleration": 2.0, - "isReversed": null, - "markers": [ - { - "position": 2.4062500000000004, - "names": [ - "PickupCube" - ] - }, - { - "position": 3.614488636363634, - "names": [ - "Neutral" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path b/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path deleted file mode 100644 index a571804..0000000 --- a/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path +++ /dev/null @@ -1,81 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.700000000000001, - "y": 2.2 - }, - "prevControl": null, - "nextControl": { - "x": 13.564766425366185, - "y": 2.4664020387068555 - }, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.684045240312702, - "y": 2.7413229722292773 - }, - "prevControl": { - "x": 13.321274967897159, - "y": 2.7392933935173427 - }, - "nextControl": { - "x": 12.098499930650991, - "y": 2.743187935905513 - }, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": 4.0, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 10.65, - "y": 2.8069653465695823 - }, - "prevControl": { - "x": 11.05103488047852, - "y": 2.8069653465695823 - }, - "nextControl": null, - "holonomicAngle": 360.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "Balance" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 1.0, - "maxAcceleration": 2.0, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 04fe4b0..bca15cd 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -1,5 +1,6 @@ package frc.team3128; +import java.util.ArrayList; import java.util.HashMap; import java.util.List; @@ -13,6 +14,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.team3128.autonomous.Trajectories; import frc.team3128.common.hardware.camera.Camera; import frc.team3128.common.swerveNeo.SwerveModuleConstants; @@ -76,7 +78,13 @@ public static class AutoConstants { public static final double RAMP_THRESHOLD = 9; //8, 10 public static final double DRIVE_SPEED = Units.inchesToMeters(30); //30, 40 - // public static final HashMap> MODULARIZED_AUTOS = + public static final HashMap> MODULARIZED_AUTOS = new HashMap>(); + + // TODO: finish all autos + static { + MODULARIZED_AUTOS.put("b-cable_1Cone+1Cube", Trajectories.stringToList("b-cable_pickup-Cube1&score-Cube1")); + // MODULARIZED_AUTOS.put() + } } diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index aa8e04b..9629655 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -3,10 +3,14 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + import static edu.wpi.first.wpilibj2.command.Commands.*; import frc.team3128.PositionConstants.Position; import frc.team3128.commands.CmdAutoBalance; import frc.team3128.common.narwhaldashboard.NarwhalDashboard; + +import static frc.team3128.Constants.AutoConstants.MODULARIZED_AUTOS; import static frc.team3128.commands.CmdManager.*; /** @@ -54,7 +58,7 @@ else if (selectedAutoName == "scuffedClimb") { else { selectedAutoName = ((DriverStation.getAlliance() == Alliance.Red) ? "r_" : "b_") + selectedAutoName; - autoCommand = Trajectories.get(selectedAutoName); + autoCommand = Trajectories.get(MODULARIZED_AUTOS.get("b-cable_1Cone+1Cube")); } return autoCommand.beforeStarting(Trajectories.resetAuto()); diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index 0622a01..c9427d8 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -1,5 +1,6 @@ package frc.team3128.autonomous; +import java.util.ArrayList; import java.util.HashMap; import java.util.List; @@ -100,8 +101,24 @@ public static CommandBase generateAuto(PathPlannerTrajectory trajectory) { return builder.fullAuto(trajectory); } - public static CommandBase get(String name) { - return builder.fullAuto(trajectories.get(name)); + public static ArrayList stringToList(String name) { + String prefix = name.split("_")[0]; + String[] autoStrings = name.split("_")[1].split("&"); + ArrayList ret = new ArrayList(); + for (String curAuto : autoStrings) { + ret.add(prefix + "_" + curAuto); + } + return ret; + } + + public static CommandBase get(ArrayList names) { + ArrayList curTrajectories = new ArrayList(); + for (String name : names) { + for (PathPlannerTrajectory curTrajectory : trajectories.get(name)) { + curTrajectories.add(curTrajectory); + } + } + return builder.fullAuto(curTrajectories); } public static CommandBase resetAuto() {