diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index d311c2c..68cf7d1 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.66, - "robotLength": 0.66, + "robotWidth": 0.86, + "robotLength": 0.86, "holonomicMode": true, "generateJSON": false, "generateCSV": false diff --git a/src/main/.pathplanner/settings.json b/src/main/.pathplanner/settings.json index 5742b57..68cf7d1 100644 --- a/src/main/.pathplanner/settings.json +++ b/src/main/.pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.847, - "robotLength": 0.847, + "robotWidth": 0.86, + "robotLength": 0.86, "holonomicMode": true, "generateJSON": false, "generateCSV": false diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path deleted file mode 100644 index 6a949df..0000000 --- a/src/main/deploy/pathplanner/New Path.path +++ /dev/null @@ -1,99 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.4089817623287166, - "y": 4.1510712408015396 - }, - "prevControl": null, - "nextControl": { - "x": 2.408981762328715, - "y": 4.1510712408015396 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 2.769801260001949, - "y": 0.9377075530185282 - }, - "prevControl": { - "x": 3.221692273855302, - "y": 0.9184199247999054 - }, - "nextControl": { - "x": 2.3179102461485943, - "y": 0.9569951812371507 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.0, - "y": 5.0 - }, - "prevControl": { - "x": 3.0, - "y": 4.0 - }, - "nextControl": { - "x": 3.0, - "y": 4.0 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.080968912294458, - "y": 4.597054101383524 - }, - "prevControl": { - "x": 6.080968912294457, - "y": 4.597054101383524 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_bottom_1Cone+1Cube+Climb.path b/src/main/deploy/pathplanner/b_bottom_1Cone+1Cube+Climb.path deleted file mode 100644 index dd5b333..0000000 --- a/src/main/deploy/pathplanner/b_bottom_1Cone+1Cube+Climb.path +++ /dev/null @@ -1,62 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 10000.0, - "y": 0.9834493848730912 - }, - "prevControl": null, - "nextControl": { - "x": 10000.996194698093, - "y": 1.0706051276207487 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 10000.0, - "y": 0.94 - }, - "prevControl": { - "x": 9999.0, - "y": 0.94 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "Score[2,3]", - "ClimbPoseBlue", - "Climb" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [ - { - "position": 0.6906605113636364, - "names": [ - "IntakeCube" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_bottom_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_bottom_1Cone+1Cube.path deleted file mode 100644 index 9ef8401..0000000 --- a/src/main/deploy/pathplanner/b_bottom_1Cone+1Cube.path +++ /dev/null @@ -1,94 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.85, - "y": 0.5 - }, - "prevControl": null, - "nextControl": { - "x": 2.7126239701837465, - "y": 0.8347884313457634 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.0, - "y": 0.94 - }, - "prevControl": { - "x": 5.8788191142647115, - "y": 0.94 - }, - "nextControl": { - "x": 8.121180885735289, - "y": 0.94 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "Score[2,3]" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 1.85, - "y": 0.5 - }, - "prevControl": { - "x": 3.1538625716140323, - "y": 0.7998279763920637 - }, - "nextControl": null, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 2.5, - "maxAcceleration": 1.0, - "isReversed": null, - "markers": [ - { - "position": 0.7, - "names": [ - "IntakeCube" - ] - }, - { - "position": 1.45, - "names": [ - "event" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_bottom_1Cone.path b/src/main/deploy/pathplanner/b_bottom_1Cone.path deleted file mode 100644 index 382fc45..0000000 --- a/src/main/deploy/pathplanner/b_bottom_1Cone.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.855, - "y": 0.5 - }, - "prevControl": null, - "nextControl": { - "x": 2.3060494925769115, - "y": 0.5 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.8, - "y": 0.8 - }, - "prevControl": { - "x": 4.8, - "y": 0.7999999999999998 - }, - "nextControl": null, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 1.0, - "maxAcceleration": 0.5, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path new file mode 100644 index 0000000..4c72664 --- /dev/null +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path @@ -0,0 +1,169 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.85, + "y": 0.5 + }, + "prevControl": null, + "nextControl": { + "x": 3.8274507521237267, + "y": 0.8020895069567917 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.3285851554342, + "y": 0.93 + }, + "prevControl": { + "x": 4.357546951100798, + "y": 0.93 + }, + "nextControl": { + "x": 5.853408258948316, + "y": 0.93 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.6, + "y": 0.932272484336635 + }, + "prevControl": { + "x": 6.162259163045806, + "y": 0.932272484336635 + }, + "nextControl": { + "x": 6.162259163045806, + "y": 0.932272484336635 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.33, + "y": 0.75 + }, + "prevControl": { + "x": 5.769244114941804, + "y": 0.75 + }, + "nextControl": { + "x": 4.890755885058196, + "y": 0.75 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.85, + "y": 1.1 + }, + "prevControl": { + "x": 3.1355383752434176, + "y": 0.7262412510936134 + }, + "nextControl": { + "x": 3.1355383752434176, + "y": 0.7262412510936134 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "ScoreCubeHigh" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.060105736530918, + "y": 0.9122666370332734 + }, + "prevControl": { + "x": 3.3978179327886435, + "y": 0.6569456350718256 + }, + "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.1, + "names": [ + "PickupCube" + ] + }, + { + "position": 2.997869318181817, + "names": [ + "Neutral" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+2Cube.path b/src/main/deploy/pathplanner/b_cable_1Cone+2Cube.path new file mode 100644 index 0000000..e38f49a --- /dev/null +++ b/src/main/deploy/pathplanner/b_cable_1Cone+2Cube.path @@ -0,0 +1,258 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.85, + "y": 0.5 + }, + "prevControl": null, + "nextControl": { + "x": 3.8274507521237267, + "y": 0.8020895069567917 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreLow" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.3285851554342, + "y": 0.93 + }, + "prevControl": { + "x": 4.357546951100798, + "y": 0.93 + }, + "nextControl": { + "x": 5.853408258948316, + "y": 0.93 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.6, + "y": 0.932272484336635 + }, + "prevControl": { + "x": 6.162259163045806, + "y": 0.932272484336635 + }, + "nextControl": { + "x": 6.162259163045806, + "y": 0.932272484336635 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.33, + "y": 0.75 + }, + "prevControl": { + "x": 5.769244114941804, + "y": 0.75 + }, + "nextControl": { + "x": 4.890755885058196, + "y": 0.75 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.85, + "y": 1.1 + }, + "prevControl": { + "x": 3.3442370547935543, + "y": 0.7980883374961188 + }, + "nextControl": { + "x": 3.3442370547935543, + "y": 0.7980883374961188 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "ScoreLow" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.33, + "y": 0.75 + }, + "prevControl": { + "x": 3.7416048151272685, + "y": 0.5641657253120875 + }, + "nextControl": { + "x": 5.850070908475151, + "y": 0.8108456893997114 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.757234604746988, + "y": 1.839378192645274 + }, + "prevControl": { + "x": 6.020252404417489, + "y": 1.1023959923157753 + }, + "nextControl": { + "x": 6.020252404417489, + "y": 1.1023959923157753 + }, + "holonomicAngle": 45.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.33, + "y": 0.75 + }, + "prevControl": { + "x": 5.6606822206699965, + "y": 0.850451468264047 + }, + "nextControl": { + "x": 4.8188409362392335, + "y": 0.5947250101102777 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.85, + "y": 1.0969582998193774 + }, + "prevControl": { + "x": 3.036262982248185, + "y": 0.7111380545677755 + }, + "nextControl": null, + "holonomicAngle": 180.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/b_hp_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_hp_1Cone+1Cube.path new file mode 100644 index 0000000..4882f1d --- /dev/null +++ b/src/main/deploy/pathplanner/b_hp_1Cone+1Cube.path @@ -0,0 +1 @@ +{"waypoints": [{"anchorPoint": {"x": 1.85, "y": 5.0}, "prevControl": null, "nextControl": {"x": 3.8274507521237267, "y": 4.697910493043208}, "holonomicAngle": 180.0, "isReversal": false, "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": {"names": ["ScoreConeHigh"], "executionBehavior": "sequential", "waitBehavior": "none", "waitTime": 0}}, {"anchorPoint": {"x": 5.3285851554342, "y": 4.57}, "prevControl": {"x": 4.357546951100798, "y": 4.57}, "nextControl": {"x": 5.853408258948316, "y": 4.57}, "holonomicAngle": 180.0, "isReversal": false, "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": {"names": [], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0}}, {"anchorPoint": {"x": 6.6, "y": 4.567727515663365}, "prevControl": {"x": 6.162259163045806, "y": 4.567727515663365}, "nextControl": {"x": 6.162259163045806, "y": 4.567727515663365}, "holonomicAngle": 0, "isReversal": true, "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": {"names": [], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0}}, {"anchorPoint": {"x": 5.33, "y": 4.75}, "prevControl": {"x": 5.769244114941804, "y": 4.75}, "nextControl": {"x": 4.890755885058196, "y": 4.75}, "holonomicAngle": 180.0, "isReversal": false, "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": {"names": [], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0}}, {"anchorPoint": {"x": 1.85, "y": 4.4}, "prevControl": {"x": 3.1355383752434176, "y": 4.773758748906387}, "nextControl": {"x": 3.1355383752434176, "y": 4.773758748906387}, "holonomicAngle": 180.0, "isReversal": true, "velOverride": null, "isLocked": false, "isStopPoint": true, "stopEvent": {"names": ["ScoreCubeHigh"], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0}}, {"anchorPoint": {"x": 7.060105736530918, "y": 4.587733362966727}, "prevControl": {"x": 3.3978179327886435, "y": 4.843054364928174}, "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.1, "names": ["PickupCube"]}, {"position": 2.997869318181817, "names": ["Neutral"]}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_mid_1Cone+1Cube+Climb.path b/src/main/deploy/pathplanner/b_mid_1Cone+1Cube+Climb.path new file mode 100644 index 0000000..a9174a1 --- /dev/null +++ b/src/main/deploy/pathplanner/b_mid_1Cone+1Cube+Climb.path @@ -0,0 +1,142 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.85, + "y": 2.2 + }, + "prevControl": null, + "nextControl": { + "x": 2.0685018644342845, + "y": 2.412705184785369 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.407499497290661, + "y": 2.7256082342025794 + }, + "prevControl": { + "x": 2.1337963109954, + "y": 2.6430913625716137 + }, + "nextControl": { + "x": 2.665994993099602, + "y": 2.803540244909385 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.8659547596872974, + "y": 2.7413229722292773 + }, + "prevControl": { + "x": 3.228725032102842, + "y": 2.7392933935173427 + }, + "nextControl": { + "x": 4.45150006934901, + "y": 2.743187935905513 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": 4.0, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.707654896212571, + "y": 2.1609214485790087 + }, + "prevControl": { + "x": 6.036038306451613, + "y": 2.4445985645443513 + }, + "nextControl": { + "x": 6.036038306451613, + "y": 2.4445985645443513 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.505970334857337, + "y": 2.797223368752293 + }, + "prevControl": { + "x": 5.891590290125025, + "y": 2.6871042266390086 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "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/b_mid_1Cone+Climb.path b/src/main/deploy/pathplanner/b_mid_1Cone+Climb.path index a7453d1..4ffb024 100644 --- a/src/main/deploy/pathplanner/b_mid_1Cone+Climb.path +++ b/src/main/deploy/pathplanner/b_mid_1Cone+Climb.path @@ -2,15 +2,15 @@ "waypoints": [ { "anchorPoint": { - "x": 1.855, + "x": 1.85, "y": 2.2 }, "prevControl": null, "nextControl": { - "x": 2.700994282998023, - "y": 2.2 + "x": 2.9852335746338157, + "y": 2.4664020387068555 }, - "holonomicAngle": 0, + "holonomicAngle": 180.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -26,30 +26,53 @@ }, { "anchorPoint": { - "x": 6.0, - "y": 2.2 + "x": 3.8659547596872974, + "y": 2.7413229722292773 }, "prevControl": { - "x": 5.0, - "y": 2.2 + "x": 3.228725032102842, + "y": 2.7392933935173427 + }, + "nextControl": { + "x": 4.45150006934901, + "y": 2.743187935905513 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": 4.0, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.5, + "y": 2.8069653465695823 + }, + "prevControl": { + "x": 5.098965119521481, + "y": 2.8069653465695823 }, "nextControl": null, - "holonomicAngle": 0, + "holonomicAngle": 180.0, "isReversal": false, "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [ - "Climb" - ], + "names": [], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0 } } ], - "maxVelocity": 4.5, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "isReversed": null, "markers": [] diff --git a/src/main/deploy/pathplanner/b_mid_1Cone.path b/src/main/deploy/pathplanner/b_mid_1Cone.path deleted file mode 100644 index 96e9bb9..0000000 --- a/src/main/deploy/pathplanner/b_mid_1Cone.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.855, - "y": 2.2 - }, - "prevControl": null, - "nextControl": { - "x": 2.923192605879788, - "y": 2.2 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.0, - "y": 2.2 - }, - "prevControl": { - "x": 5.0, - "y": 2.2 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 1.0, - "maxAcceleration": 0.5, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_top_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_top_1Cone+1Cube.path deleted file mode 100644 index 665ded1..0000000 --- a/src/main/deploy/pathplanner/b_top_1Cone+1Cube.path +++ /dev/null @@ -1,63 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.855, - "y": 4.9 - }, - "prevControl": null, - "nextControl": { - "x": 2.8511946980917475, - "y": 4.812844257252342 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.8, - "y": 4.602852566816245 - }, - "prevControl": { - "x": 6.505962678734585, - "y": 4.602852566816245 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "Score[8,3]" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 4.5, - "maxAcceleration": 2.0, - "isReversed": null, - "markers": [ - { - "position": 0.6, - "names": [ - "IntakeCube" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_top_1Cone.path b/src/main/deploy/pathplanner/b_top_1Cone.path deleted file mode 100644 index 3457a4f..0000000 --- a/src/main/deploy/pathplanner/b_top_1Cone.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.855, - "y": 4.9 - }, - "prevControl": null, - "nextControl": { - "x": 2.855000000000002, - "y": 4.9 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.0, - "y": 4.7 - }, - "prevControl": { - "x": 4.705962678734585, - "y": 4.7 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 4.5, - "maxAcceleration": 2.0, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_bottom_1Cone+1Cube.path b/src/main/deploy/pathplanner/r_bottom_1Cone+1Cube.path deleted file mode 100644 index 22768b9..0000000 --- a/src/main/deploy/pathplanner/r_bottom_1Cone+1Cube.path +++ /dev/null @@ -1,63 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.645, - "y": 0.5 - }, - "prevControl": null, - "nextControl": { - "x": 13.645, - "y": 0.5000000000000001 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.613431732071797, - "y": 0.9239802055992996 - }, - "prevControl": { - "x": 10.849822322607475, - "y": 0.9239802055992995 - }, - "nextControl": null, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "Score[2,3]" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 4.5, - "maxAcceleration": 2.0, - "isReversed": null, - "markers": [ - { - "position": 0.7, - "names": [ - "IntakeCube" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_bottom_1Cone.path b/src/main/deploy/pathplanner/r_bottom_1Cone.path deleted file mode 100644 index 518da07..0000000 --- a/src/main/deploy/pathplanner/r_bottom_1Cone.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.645, - "y": 0.5 - }, - "prevControl": null, - "nextControl": { - "x": 13.645, - "y": 0.5000000000000001 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 10.5, - "y": 0.7 - }, - "prevControl": { - "x": 10.794037321265414, - "y": 0.7 - }, - "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.0, - "isReversed": null, - "markers": [] -} \ 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 new file mode 100644 index 0000000..de2334b --- /dev/null +++ b/src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path @@ -0,0 +1 @@ +{"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": ["ScoreConeHigh"], "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.950000000000001, "y": 0.932272484336635}, "prevControl": {"x": 10.387740836954194, "y": 0.932272484336635}, "nextControl": {"x": 10.387740836954194, "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.414461624756584, "y": 0.7262412510936134}, "nextControl": {"x": 13.414461624756584, "y": 0.7262412510936134}, "holonomicAngle": 360.0, "isReversal": true, "velOverride": null, "isLocked": false, "isStopPoint": true, "stopEvent": {"names": ["ScoreCubeHigh"], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0}}, {"anchorPoint": {"x": 9.489894263469083, "y": 0.9122666370332734}, "prevControl": {"x": 13.152182067211356, "y": 0.6569456350718256}, "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_cable_1Cone+2Cube.path b/src/main/deploy/pathplanner/r_cable_1Cone+2Cube.path new file mode 100644 index 0000000..482ed7d --- /dev/null +++ b/src/main/deploy/pathplanner/r_cable_1Cone+2Cube.path @@ -0,0 +1,258 @@ +{ + "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.950000000000001, + "y": 0.932272484336635 + }, + "prevControl": { + "x": 10.387740836954194, + "y": 0.932272484336635 + }, + "nextControl": { + "x": 10.387740836954194, + "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.792765395253014, + "y": 1.839378192645274 + }, + "prevControl": { + "x": 10.529747595582512, + "y": 1.1023959923157758 + }, + "nextControl": { + "x": 10.529747595582512, + "y": 1.1023959923157758 + }, + "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+1Cube.path b/src/main/deploy/pathplanner/r_hp_1Cone+1Cube.path new file mode 100644 index 0000000..f4fb3df --- /dev/null +++ b/src/main/deploy/pathplanner/r_hp_1Cone+1Cube.path @@ -0,0 +1 @@ +{"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.2214148445658, "y": 4.57}, "prevControl": {"x": 12.192453048899203, "y": 4.57}, "nextControl": {"x": 10.696591741051684, "y": 4.57}, "holonomicAngle": 360.0, "isReversal": false, "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": {"names": [], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0}}, {"anchorPoint": {"x": 9.950000000000001, "y": 4.567727515663365}, "prevControl": {"x": 10.387740836954194, "y": 4.567727515663365}, "nextControl": {"x": 10.387740836954194, "y": 4.567727515663365}, "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": 9.489894263469083, "y": 4.587733362966727}, "prevControl": {"x": 13.152182067211356, "y": 4.843054364928174}, "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+1Cube+Climb.path b/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path new file mode 100644 index 0000000..3a0499a --- /dev/null +++ b/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path @@ -0,0 +1 @@ +{"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.321274967897159, "y": 2.7392933935173427}, "nextControl": {"x": 12.098499930650991, "y": 2.743187935905513}, "holonomicAngle": 180.0, "isReversal": false, "velOverride": 4.0, "isLocked": false, "isStopPoint": false, "stopEvent": {"names": [], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0}}, {"anchorPoint": {"x": 9.84234510378743, "y": 2.1609214485790087}, "prevControl": {"x": 10.513961693548389, "y": 2.4445985645443513}, "nextControl": {"x": 10.513961693548389, "y": 2.4445985645443513}, "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": [], "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+Climb.path b/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path index b86dfd0..0923836 100644 --- a/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path +++ b/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path @@ -1,56 +1 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.645, - "y": 2.2 - }, - "prevControl": null, - "nextControl": { - "x": 13.799005717001977, - "y": 2.2 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 10.5, - "y": 2.2 - }, - "prevControl": { - "x": 11.5, - "y": 2.2 - }, - "nextControl": null, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "Climb" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 4.5, - "maxAcceleration": 2.0, - "isReversed": null, - "markers": [] -} \ No newline at end of file +{"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": 11.05, "y": 2.8069653465695823}, "prevControl": {"x": 11.45103488047852, "y": 2.8069653465695823}, "nextControl": null, "holonomicAngle": 360.0, "isReversal": false, "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": {"names": [], "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/deploy/pathplanner/r_mid_1Cone.path b/src/main/deploy/pathplanner/r_mid_1Cone.path deleted file mode 100644 index e0780bf..0000000 --- a/src/main/deploy/pathplanner/r_mid_1Cone.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.645, - "y": 2.2 - }, - "prevControl": null, - "nextControl": { - "x": 13.799005717001977, - "y": 2.2 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 10.5, - "y": 2.2 - }, - "prevControl": { - "x": 11.5, - "y": 2.2 - }, - "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.0, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_top_1Cone+1Cube.path b/src/main/deploy/pathplanner/r_top_1Cone+1Cube.path deleted file mode 100644 index a2ebf98..0000000 --- a/src/main/deploy/pathplanner/r_top_1Cone+1Cube.path +++ /dev/null @@ -1,63 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.645, - "y": 4.9 - }, - "prevControl": null, - "nextControl": { - "x": 13.644999999999998, - "y": 4.9 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.8, - "y": 4.602852566816245 - }, - "prevControl": { - "x": 10.094037321265414, - "y": 4.602852566816245 - }, - "nextControl": null, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "Score[8,3]" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 4.5, - "maxAcceleration": 2.0, - "isReversed": null, - "markers": [ - { - "position": 0.65, - "names": [ - "IntakeCube" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_top_1Cone.path b/src/main/deploy/pathplanner/r_top_1Cone.path deleted file mode 100644 index 4cc12b1..0000000 --- a/src/main/deploy/pathplanner/r_top_1Cone.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.645, - "y": 4.9 - }, - "prevControl": null, - "nextControl": { - "x": 13.644999999999998, - "y": 4.9 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 12.0, - "y": 4.7 - }, - "prevControl": { - "x": 12.294037321265414, - "y": 4.7 - }, - "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.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 27be734..1a13c2a 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -39,47 +39,29 @@ public static class ConversionConstants { } public static class TrajectoryConstants { - public static final Rotation2d HEADING_0 = Rotation2d.fromDegrees(180); + public static final Rotation2d HEADING = Rotation2d.fromDegrees(180); public static final Translation2d POINT_1 = new Translation2d(12.7, 6.75); - public static final Rotation2d HEADING_1 = Rotation2d.fromDegrees(180); public static final double CONDITION_1 = 12.7; - public static final ListPOINT_2 = new ArrayList() { - { - add(POINT_2A); - add(POINT_2B); - } - }; - public static final Translation2d POINT_2A = new Translation2d(5.6, 4.6); - public static final Translation2d POINT_2B = new Translation2d(5.6, 0.8); - public static final Rotation2d HEADING_2 = Rotation2d.fromDegrees(180); - public static final double CONDITION_2 = 5.6; - - public static final ListPOINT_3 = new ArrayList() { - { - add(POINT_3A); - add(POINT_3B); - } - }; - public static final Translation2d POINT_3A = new Translation2d(2.8, 0.8); - public static final Translation2d POINT_3B = new Translation2d(2.8, 4.6); - public static final Rotation2d HEADING_3 = Rotation2d.fromDegrees(180); - public static final double CONDITION_3 = 2.25; + public static final Translation2d POINT_2A = new Translation2d(4.85, 0.8); + public static final Translation2d POINT_2B = new Translation2d(4.85, 4.7); + public static final double CONDITION_2 = 4.85; - public static final Rotation2d HEADING_4A = Rotation2d.fromDegrees(135); - public static final Rotation2d HEADING_4B = Rotation2d.fromDegrees(-135); + public static final Translation2d POINT_3A = new Translation2d(2.5, 0.8); + public static final Translation2d POINT_3B = new Translation2d(2.5, 4.7); + public static final double CONDITION_3 = 2.5; public static final Pose2d[] END_POINTS = new Pose2d[]{ - new Pose2d(1.90,0.5,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,1.05,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,1.65,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,2.15,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,2.75,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,3.3,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,3.85,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,4.45,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,4.89,Rotation2d.fromDegrees(180)) + new Pose2d(1.75,0.5,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,0.95,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,1.55,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,2.05,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,2.65,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,3.2,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,3.75,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,4.35,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,4.79,Rotation2d.fromDegrees(180)) }; } @@ -98,10 +80,13 @@ public static class AutoConstants { public static final Pose2d ClimbSetupOutsideBot = new Pose2d(5.6, 2.9, Rotation2d.fromDegrees(180)); public static final Pose2d ClimbSetupOutsideTop = new Pose2d(5.6, 3.3, Rotation2d.fromDegrees(180)); - public static final double ANGLE_THRESHOLD = 3; - public static final double VELOCITY_THRESHOLD = 8; - public static final double RAMP_THRESHOLD = 8; - public static final double DRIVE_SPEED = Units.inchesToMeters(15); + public static final double ANGLE_THRESHOLD = 9; //7, 9 + public static final double VELOCITY_THRESHOLD = 3; //6, 3 + public static final double RAMP_THRESHOLD = 10; //8, 10 + public static final double DRIVE_SPEED = Units.inchesToMeters(40); //30, 40 + public static final double kP = 0.000000001; + public static final double kI = 0.0; + public static final double kD = 0; public static final Pose2d[] STARTING_POINTS = new Pose2d[] { new Pose2d(1.85 ,0.5, Rotation2d.fromDegrees(180)), @@ -134,6 +119,9 @@ public static class AutoConstants { }; public static final double BALANCE_FF = 0.3; + + public static final double slowSpeed = 1.5; + public static final double slowAcceleration = 2; } public static class SwerveConstants { @@ -199,10 +187,9 @@ public static class SwerveConstants { public static final double rotationKD = 0; /* Turning PID Values */ - public static final double turnKP = 0.1; + public static final double turnKP = 5; public static final double turnKI = 0; public static final double turnKD = 0; - public static final double turnKF = 0.1; /* Angle Motor PID Values */ // switched 364 pid values to SDS pid values @@ -229,7 +216,7 @@ public static class SwerveConstants { // For safety, use less than theoretical and real values public static final double maxSpeed = 4.5; //meters per second - 16.3 ft/sec public static final double bumpSpeed = 2.5; - public static final double maxAcceleration = 2.3; + public static final double maxAcceleration = 2.5; public static final double maxAngularVelocity = 5; //3; //11.5; // citrus: 10 public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(maxSpeed, maxAcceleration); @@ -291,12 +278,20 @@ public static final class Mod3 { public static class VisionConstants { - public static final Camera FRONT = new Camera("Frog", true, 0, 0, 0, - new Transform2d(new Translation2d(Units.inchesToMeters(-5.75), - Units.inchesToMeters(-11.5)), Rotation2d.fromDegrees(0))); - public static final Camera BACK = new Camera("Blog", true, 0, 0, 0, - new Transform2d(new Translation2d(Units.inchesToMeters(-5.75), - Units.inchesToMeters(11.5)), Rotation2d.fromDegrees(180))); + public static final Camera FRONT_LEFT = new Camera("FRONT_LEFT", true, 0, 0, 0, + new Transform2d( + new Translation2d(Units.inchesToMeters(5.43), Units.inchesToMeters(-11.9)), + Rotation2d.fromDegrees(0))); + + public static final Camera FRONT_RIGHT = new Camera("FRONT_RIGHT", true, 0, 0, 0, + new Transform2d( + new Translation2d(Units.inchesToMeters(5.43), Units.inchesToMeters(11.7)), + Rotation2d.fromDegrees(0))); + + public static final Camera BACK_RIGHT = new Camera("BACK_RIGHT", true, 0, 0, 0, + new Transform2d( + new Translation2d(Units.inchesToMeters(6.57), Units.inchesToMeters(-11.7)), + Rotation2d.fromDegrees(180))); public static final PIDController xController = new PIDController(1, 0, 0); public static final PIDController yController = new PIDController(1, 0, 0); @@ -323,15 +318,15 @@ public static class VisionConstants { public static final Matrix SVR_VISION_MEASUREMENT_STD = VecBuilder.fill(1,1,Units.degreesToRadians(10)); public static final Pose2d[] SCORES = new Pose2d[]{ - new Pose2d(1.90,0.5,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,1.05,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,1.65,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,2.15,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,2.75,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,3.3,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,3.85,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,4.45,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,4.89,Rotation2d.fromDegrees(180)) + new Pose2d(1.90,0.55,Rotation2d.fromDegrees(180)), //0.5 + new Pose2d(1.90,1.1,Rotation2d.fromDegrees(180)), //1.05 + new Pose2d(1.90,1.7,Rotation2d.fromDegrees(180)), //1.65 + new Pose2d(1.90,2.2,Rotation2d.fromDegrees(180)), //2.15 + new Pose2d(1.90,2.8,Rotation2d.fromDegrees(180)), //2.75 + new Pose2d(1.90,3.35,Rotation2d.fromDegrees(180)), //3.3 + new Pose2d(1.90,3.90,Rotation2d.fromDegrees(180)), //3.85 + new Pose2d(1.90,4.50,Rotation2d.fromDegrees(180)), //4.45 + new Pose2d(1.90,4.94,Rotation2d.fromDegrees(180)) //4.89 }; public static final Pose2d[][] SCORES_GRID = new Pose2d[][] { @@ -519,29 +514,29 @@ public static class ManipulatorConstants{ public static final int ROLLER_MOTOR_ID = 31; public static final double ROLLER_POWER = 0.9; public static final double STALL_POWER_CONE = 0.05; - public static final double STALL_POWER_CUBE = 0; + public static final double STALL_POWER_CUBE = 0.1; - public static final double CURRENT_THRESHOLD_CONE = 25; - public static final double CURRENT_THRESHOLD_CUBE = 15; + public static final double CURRENT_THRESHOLD_CONE = 30; + public static final double CURRENT_THRESHOLD_CUBE = 25; } public static class ElevatorConstants { public static final int ELV1_ID = 11; public static final int ELV2_ID = 12; - public static final double kP = 3; + public static final double kP = 1; public static final double kI = 0; public static final double kD = 0; - public static final double kS = 0.475; + public static final double kS = 0.975; public static final double kV = 0; - public static final double kG = 0.625; + public static final double kG = 1.05; public static final double MIN_DIST = 2; //Ask Charlie public static final double MAX_DIST = 55; //Ask Charlie - public static final double GEAR_RATIO = 12.5; + public static final double GEAR_RATIO = 10; public static final double SPOOL_CIRCUMFERENCE = 3 * Math.PI; public static final double FRAME_LENGTH = 15; @@ -560,44 +555,45 @@ public static class BalanceConstants{ } public static class LedConstants{ - public static class Green{ - public static final int HUE = 60; - public static final int SATURATION = 255; - public static final int VALUE = 255; - } - - public static class Blue{ - public static final int HUE = 120; - public static final int SATURATION = 255; - public static final int VALUE = 255; - } + public static final int CANDLE_ID = 52; + + public static final int WHITE_VALUE = 0; //leds used don't have a white value + + public static final int STARTING_ID = 8; + public static final int PIVOT_COUNT = 200; + public static final int PIVOT_COUNT_FRONT = 50; //change + public static final int PIVOT_COUNT_BACK = 50; //change - public static class Red{ - public static final int HUE = 0; - public static final int SATURATION = 255; - public static final int VALUE = 255; - } + public static final double HOLDING_SPEED = 2; - public static class Yellow{ - public static final int HUE = 30; - public static final int SATURATION = 255; - public static final int VALUE = 255; - } + public static class RainbowAnimation { + public static final double BRIGHTNESS = 1; + public static final double SPEED = 1; - public static class Purple{ - public static final int HUE = 130; - public static final int SATURATION = 255; - public static final int VALUE = 50; } - public static class Off{ - public static final int HUE = 0; - public static final int SATURATION = 0; - public static final int VALUE = 0; + public enum Colors { + OFF(0,0,0,false), + CONE(255,255,0,false), + CUBE(255,0,255,false), + HOLDING(255,0,0,false), + + AUTO(0,0,0,true), + DEFAULT(0,0,225,false); + + public final int r; + public final int b; + public final int g; + public final boolean animation; + + Colors(int r, int g, int b,boolean animation) { + this.r = r; + this.g = g; + this.b = b; + this.animation = animation; + } + } - - public static final int PORT = 0; - public static final int LENGTH = 288; } diff --git a/src/main/java/frc/team3128/PositionConstants.java b/src/main/java/frc/team3128/PositionConstants.java index 9440a66..8407db5 100644 --- a/src/main/java/frc/team3128/PositionConstants.java +++ b/src/main/java/frc/team3128/PositionConstants.java @@ -3,18 +3,18 @@ public class PositionConstants { public static enum Position { HIGH_CONE(47, 0, true), - HIGH_CUBE(50, 20, false), - MID_CONE(22.5, 28, true), + HIGH_CUBE(48.5, 20, false), + MID_CONE(25.5, 28, true), MID_CUBE(25, 30, false), - LOW(3, 45, true), + LOW(3, 0, true), SHELF_CONE(53, -30, true), SHELF_CUBE(37, 22, false), CHUTE_CONE(10, 55, true), CHUTE_CUBE(10, 45, false), - GROUND_CONE(10, -18, true), - GROUND_CUBE(0, 0, false), + GROUND_CONE(8, -18, true), + GROUND_CUBE(2.5, 0, false), NEUTRAL(3, 80, false); diff --git a/src/main/java/frc/team3128/Robot.java b/src/main/java/frc/team3128/Robot.java index 1976dff..ad6c9cd 100644 --- a/src/main/java/frc/team3128/Robot.java +++ b/src/main/java/frc/team3128/Robot.java @@ -59,7 +59,7 @@ public void autonomousInit() { @Override public void autonomousPeriodic() { CommandScheduler.getInstance().run(); - if (timer.hasElapsed(14.75) && Math.abs(Swerve.getInstance().getPitch()) > 6) { + if (timer.hasElapsed(14.75)) { new RunCommand(()->Swerve.getInstance().xlock(), Swerve.getInstance()).schedule(); } } diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index 3d59f9e..d05b314 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -1,6 +1,5 @@ package frc.team3128; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotController; @@ -8,22 +7,13 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; - import static 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.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team3128.commands.CmdSwerveDrive; - -import static frc.team3128.Constants.FieldConstants.*; - -import static frc.team3128.Constants.SwerveConstants.*; - +import frc.team3128.Constants.LedConstants.Colors; import frc.team3128.PositionConstants.Position; -import frc.team3128.commands.CmdBalance; -import frc.team3128.commands.CmdBangBangBalance; import static frc.team3128.commands.CmdManager.*; import frc.team3128.common.hardware.input.NAR_ButtonBoard; import frc.team3128.common.hardware.input.NAR_Joystick; @@ -31,7 +21,8 @@ import frc.team3128.common.narwhaldashboard.NarwhalDashboard; import frc.team3128.common.utility.Log; import frc.team3128.subsystems.Elevator; -import frc.team3128.subsystems.Led; +import frc.team3128.subsystems.Leds; +import frc.team3128.subsystems.Manipulator; import frc.team3128.common.utility.NAR_Shuffleboard; import frc.team3128.subsystems.Swerve; import frc.team3128.subsystems.Vision; @@ -48,7 +39,7 @@ public class RobotContainer { private Swerve swerve; private Vision vision; - private Led led; + private Leds leds; private Elevator elevator; private NAR_Joystick leftStick; @@ -71,7 +62,7 @@ public RobotContainer() { swerve = Swerve.getInstance(); vision = Vision.getInstance(); - led = Led.getInstance(); + leds = Leds.getInstance(); elevator = Elevator.getInstance(); //TODO: Enable all PIDSubsystems so that useOutput runs here @@ -92,20 +83,38 @@ public RobotContainer() { initDashboard(); configureButtonBindings(); - if(RobotBase.isSimulation()) - DriverStation.silenceJoystickConnectionWarning(true); + DriverStation.silenceJoystickConnectionWarning(true); } private void configureButtonBindings() { controller.getButton("A").onTrue(new InstantCommand(()-> Vision.AUTO_ENABLED = !Vision.AUTO_ENABLED)); - controller.getButton("RightTrigger").onTrue(new InstantCommand(()-> swerve.throttle = 1)).onFalse(new InstantCommand(()-> swerve.throttle = 0.8)); - controller.getButton("LeftTrigger").onTrue(new InstantCommand(()-> swerve.throttle = .25)).onFalse(new InstantCommand(()-> swerve.throttle = 0.8)); + controller.getButton("RightTrigger").onTrue(score(Position.LOW, true)).onFalse(runOnce(()-> ENABLE = false)); + controller.getButton("LeftTrigger").onTrue(runOnce(()-> ENABLE = true)).onFalse(runOnce(()-> ENABLE = false)); controller.getButton("X").onTrue(new RunCommand(()-> swerve.xlock(), swerve)).onFalse(new InstantCommand(()-> swerve.stop(),swerve)); controller.getButton("B").onTrue(new InstantCommand(()-> swerve.resetEncoders())); - controller.getButton("Y").onTrue(runOnce(()-> ENABLE = true)).onFalse(runOnce(()-> ENABLE = false)); + controller.getButton("Y").onTrue(runOnce(()-> swerve.throttle = 1)).onFalse(runOnce(()-> swerve.throttle = 0.8)); controller.getButton("Start").onTrue(resetSwerve()); - controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CONE, true)); - controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CUBE, true)); + controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true)).onFalse(retract(Position.NEUTRAL).andThen(stopManip())); + controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true)).onFalse(retract(Position.NEUTRAL).andThen(stopManip())); + + controller.getUpPOVButton().onTrue(runOnce(()-> { + CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 180 : 0; + CmdSwerveDrive.enabled = true; + })); + controller.getDownPOVButton().onTrue(runOnce(()-> { + CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 0 : 180; + CmdSwerveDrive.enabled = true; + })); + + controller.getRightPOVButton().onTrue(runOnce(()-> { + CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 90 : 270; + CmdSwerveDrive.enabled = true; + })); + + controller.getLeftPOVButton().onTrue(runOnce(()-> { + CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 270 : 90; + CmdSwerveDrive.enabled = true; + })); rightStick.getButton(1).onTrue(resetSwerve()); rightStick.getButton(2).onTrue(moveElv(0.4)).onFalse(moveElv(0)); @@ -136,7 +145,7 @@ private void configureButtonBindings() { buttonPad.getButton(13).onTrue(runOnce(()-> SINGLE_STATION = !SINGLE_STATION)); - buttonPad.getButton(14).onTrue(retract(Position.NEUTRAL)); + buttonPad.getButton(14).onTrue(retract(Position.NEUTRAL).beforeStarting(stopManip())); buttonPad.getButton(16).onTrue( HPpickup(Position.CHUTE_CONE, Position.SHELF_CONE) @@ -156,23 +165,10 @@ private void configureButtonBindings() { operatorController.getButton("Back").onTrue(new InstantCommand(()-> Vision.MANUAL = !Vision.MANUAL)); - // inProtected = new Trigger( - // () -> { - // Pose2d pose = Swerve.getInstance().getPose(); - // if (DriverStation.getAlliance() == Alliance.Red) { - // return ((pose.getY() < midY + robotLength/2 && pose.getX() < outerX + robotLength/2) || - // (pose.getY() < leftY + robotLength/2 && pose.getX() < midX + robotLength/2)) || - // ((pose.getY() > 6.85 && pose.getX() > FIELD_X_LENGTH - 6.70) || (pose.getY() > 5.50 && pose.getX() > FIELD_X_LENGTH - 3.30)); - // } - // return ((pose.getY() < midY + robotLength/2 && pose.getX() > FIELD_X_LENGTH - outerX - robotLength/2) || - // (pose.getY() < leftY + robotLength/2 && pose.getX() > FIELD_X_LENGTH - midX - robotLength/2)) || - // ((pose.getY() > 6.85 && pose.getX() < 6.70) || (pose.getY() > 5.50 && pose.getX() < 3.30)); - // } - // ); - // inProtected.onTrue(new InstantCommand(()-> controller.startVibrate())).onFalse(new InstantCommand(()-> controller.stopVibrate())); } public void init() { + leds.setElevatorLeds(Colors.DEFAULT); Vision.AUTO_ENABLED = false; if (DriverStation.getAlliance() == Alliance.Red) { buttonPad.getButton(4).onTrue( @@ -220,7 +216,7 @@ public void init() { private void initDashboard() { if (DEBUG.getAsBoolean()) { SmartDashboard.putData("CommandScheduler", CommandScheduler.getInstance()); - //SmartDashboard.putData("Swerve", swerve); + // SmartDashboard.putData("Swerve", swerve); } swerve.initShuffleboard(); diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index 1ebe81f..dce93e9 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -1,18 +1,15 @@ package frc.team3128.autonomous; -import java.util.HashMap; - -import com.pathplanner.lib.auto.PIDConstants; -import com.pathplanner.lib.auto.SwerveAutoBuilder; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.team3128.Constants.SwerveConstants; +import edu.wpi.first.wpilibj2.command.CommandBase; +import static edu.wpi.first.wpilibj2.command.Commands.*; +import frc.team3128.PositionConstants.Position; import frc.team3128.common.narwhaldashboard.NarwhalDashboard; -import frc.team3128.common.utility.Log; import frc.team3128.subsystems.Swerve; +import static frc.team3128.commands.CmdManager.*; /** * Class to store information about autonomous routines. @@ -31,19 +28,43 @@ public AutoPrograms() { } private void initAutoSelector() { - String[] autoStrings = new String[] {"TestAuto1", "b_bottom_1Cone+1Cube","b_bottom_1Cone"}; + String[] autoStrings = new String[] { + //Blue Autos + //Cable + "b_cable_1Cone+1Cube","b_cable_1Cone+2Cube", //"b_cable_1Cone+2Cube+Climb", + //Mid + "b_mid_1Cone+Climb","b_mid_1Cone+1Cube+Climb", + //Hp + "b_hp_1Cone+1Cube",//"b_cable_1Cone+2Cube", + + //Red Autos + //Cable + "r_cable_1Cone+1Cube","r_cable_1Cone+2Cube", + //Mid + "r_mid_1Cone+Climb","r_mid_1Cone+1Cube+Climb", + //Hp + "r_hp_1Cone+1Cube"//"r_cable_1Cone+2Cube", + + }; NarwhalDashboard.addAutos(autoStrings); } public Command getAutonomousCommand() { - //String selectedAutoName = NarwhalDashboard.getSelectedAutoName(); - String selectedAutoName = "b_bottom_1Cone"; //uncomment and change this for testing without opening Narwhal Dashboard - + String selectedAutoName = NarwhalDashboard.getSelectedAutoName(); + // String selectedAutoName = "b_cable_1Cone+2Cube+Climb"; //uncomment and change this for testing without opening Narwhal Dashboard + SmartDashboard.putString(selectedAutoName, selectedAutoName); if (selectedAutoName == null) { - return null; + return score(Position.HIGH_CONE, true).beforeStarting(resetAuto()); } - return Trajectories.get(selectedAutoName); + return Trajectories.get(selectedAutoName,selectedAutoName.contains("Climb")).beforeStarting(resetAuto()); + } + + public CommandBase resetAuto() { + return sequence( + resetAll(), + retract(Position.NEUTRAL) + ); } // /** diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index 0f7eae5..dc695c7 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -13,10 +13,16 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.Commands; +import static edu.wpi.first.wpilibj2.command.Commands.*; import static frc.team3128.Constants.SwerveConstants.*; +import frc.team3128.Constants.AutoConstants; import frc.team3128.Constants.SwerveConstants; +import frc.team3128.PositionConstants.Position; +import static frc.team3128.commands.CmdManager.*; + +import frc.team3128.commands.CmdAutoBalance; +import frc.team3128.subsystems.Manipulator; import frc.team3128.subsystems.Swerve; /** @@ -27,54 +33,85 @@ public class Trajectories { private static HashMap> trajectories = new HashMap>(); - public static SwerveAutoBuilder builder; + private static SwerveAutoBuilder builder; private static HashMap CommandEventMap = new HashMap(); + private static Manipulator manipulator = Manipulator.getInstance(); + private static Swerve swerve = Swerve.getInstance(); public static double autoSpeed = SwerveConstants.maxSpeed; public static void initTrajectories() { - final String[] trajectoryNames = {"TestAuto1", "b_bottom_1Cone+1Cube","b_bottom_1Cone"}; - - CommandEventMap.put("ScoreConeHigh", Commands.sequence()); - CommandEventMap.put("ScoreCubeLow", Commands.sequence()); + final String[] trajectoryNames = { + //Blue Autos + //Cable + "b_cable_1Cone+1Cube","b_cable_1Cone+2Cube", "b_cable_1Cone+2Cube+Climb", + //Mid + "b_mid_1Cone+Climb","b_mid_1Cone+1Cube+Climb", + //Hp + "b_hp_1Cone+1Cube","b_cable_1Cone+2Cube", + + //Red Autos + //Cable + "r_cable_1Cone+1Cube","r_cable_1Cone+2Cube", + //Mid + "r_mid_1Cone+Climb","r_mid_1Cone+1Cube+Climb", + //Hp + "r_hp_1Cone+1Cube","r_cable_1Cone+2Cube", + }; + + CommandEventMap.put("ScoreConeHigh", score(Position.HIGH_CONE, true)); + + CommandEventMap.put("ScoreCubeHigh", score(Position.HIGH_CUBE, true)); + + CommandEventMap.put("ScoreLow", score(Position.LOW, true)); - CommandEventMap.put("IntakeCube", Commands.sequence()); - - CommandEventMap.put("RetractIntake", Commands.sequence()); + CommandEventMap.put("PickupCube", pickup(Position.GROUND_CUBE, true)); + CommandEventMap.put("Neutral", retract(Position.NEUTRAL)); for (String trajectoryName : trajectoryNames) { // Path path = Filesystem.getDeployDirectory().toPath().resolve("paths").resolve(trajectoryName + ".wpilib.json"); - trajectories.put(trajectoryName, PathPlanner.loadPathGroup(trajectoryName, new PathConstraints(maxSpeed, maxAcceleration))); + if (trajectoryName.contains("mid")) { + trajectories.put(trajectoryName, PathPlanner.loadPathGroup(trajectoryName, new PathConstraints(AutoConstants.slowSpeed, AutoConstants.slowAcceleration))); + } + else { + trajectories.put(trajectoryName, PathPlanner.loadPathGroup(trajectoryName, new PathConstraints(maxSpeed, maxAcceleration))); + } } builder = new SwerveAutoBuilder( - swerve::getPose, - swerve::resetOdometry, + Swerve.getInstance()::getPose, + Swerve.getInstance()::resetOdometry, swerveKinematics, new PIDConstants(translationKP,translationKI,translationKD), new PIDConstants(rotationKP,rotationKI,rotationKD), - swerve::setModuleStates, + Swerve.getInstance()::setModuleStates, CommandEventMap, - swerve + Swerve.getInstance() ); } - public static CommandBase get(String name) { - return builder.fullAuto(trajectories.get(name)); - } - public static CommandBase generateAuto(PathPlannerTrajectory trajectory) { return builder.fullAuto(trajectory); } + public static CommandBase get(String name, boolean balance) { + if (balance) { + return sequence( + builder.fullAuto(trajectories.get(name)), + new CmdAutoBalance(true) + ); + } + return builder.fullAuto(trajectories.get(name)); + } + public static PathPlannerTrajectory line(Pose2d start, Pose2d end) { return PathPlanner.generatePath( - new PathConstraints(maxSpeed, 4), + new PathConstraints(maxSpeed, 4), new PathPoint(start.getTranslation(), start.getRotation()), new PathPoint(end.getTranslation(), end.getRotation()) ); diff --git a/src/main/java/frc/team3128/commands/CmdAutoBalance.java b/src/main/java/frc/team3128/commands/CmdAutoBalance.java index 80775d5..a87795b 100644 --- a/src/main/java/frc/team3128/commands/CmdAutoBalance.java +++ b/src/main/java/frc/team3128/commands/CmdAutoBalance.java @@ -1,46 +1,75 @@ package frc.team3128.commands; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandBase; import static frc.team3128.Constants.AutoConstants.*; + +import frc.team3128.common.utility.NAR_Shuffleboard; +import frc.team3128.subsystems.Manipulator; import frc.team3128.subsystems.Swerve; public class CmdAutoBalance extends CommandBase{ private final Swerve swerve; private double prevAngle; + private double angleVelocity; + private int plateauCount; private boolean onRamp; + private boolean shoot; - public CmdAutoBalance(boolean intialDirection) { + public CmdAutoBalance() { swerve = Swerve.getInstance(); + addRequirements(Swerve.getInstance()); + } + + public CmdAutoBalance(boolean shoot) { + this.shoot = shoot; + swerve = Swerve.getInstance(); + addRequirements(Swerve.getInstance()); } @Override public void initialize() { + swerve.zeroAxis(); prevAngle = swerve.getPitch(); onRamp = false; + plateauCount = 0; + angleVelocity = 0; + } + + public double getAngleVelocity(double currAngle) { + if (plateauCount < 5) { + plateauCount ++; + return angleVelocity; + } + plateauCount = 0; + angleVelocity = (currAngle - prevAngle) / 0.1; + prevAngle = currAngle; + return angleVelocity; } @Override public void execute() { - final double angle = swerve.getPitch(); - final double angleVelocity = (angle - prevAngle) / 0.02; - prevAngle = angle; + final double advAngle = swerve.getPitch(); + // final double angleVelocity = (advAngle - prevAngle) / 0.02; + final double angleVelocity = getAngleVelocity(advAngle); - if (angle > RAMP_THRESHOLD) onRamp = true; + if (advAngle > RAMP_THRESHOLD) onRamp = true; - if (Math.abs(angle) < ANGLE_THRESHOLD && onRamp) { + if (Math.abs(advAngle) < ANGLE_THRESHOLD && onRamp) { swerve.xlock(); return; } - if ((angle < 0.0 && angleVelocity > VELOCITY_THRESHOLD) || (angle > 0.0 && angleVelocity < VELOCITY_THRESHOLD)) { - swerve.stop(); + NAR_Shuffleboard.addData("Drivetrain", "Angle Velocity", angleVelocity, 6, 2); + if (((advAngle < 0.0 && angleVelocity > VELOCITY_THRESHOLD) || (advAngle > 0.0 && angleVelocity < -VELOCITY_THRESHOLD)) && onRamp) { + swerve.xlock(); return; } - swerve.drive(new Translation2d(DRIVE_SPEED * (angle > 0.0 ? -1.0 : 1.0), 0), 0, false); + swerve.drive(new Translation2d(onRamp ? DRIVE_SPEED * (advAngle > 0.0 ? 1.0 : -1.0) : DRIVE_SPEED, 0), 0, false); } @Override @@ -51,7 +80,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { return false; - } } diff --git a/src/main/java/frc/team3128/commands/CmdBalance.java b/src/main/java/frc/team3128/commands/CmdBalance.java deleted file mode 100644 index 5b3ccc8..0000000 --- a/src/main/java/frc/team3128/commands/CmdBalance.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.team3128.commands; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.PIDCommand; -import static frc.team3128.Constants.FieldConstants.*; - -import frc.team3128.Constants.AutoConstants; -import frc.team3128.Constants.SwerveConstants; -import frc.team3128.subsystems.Swerve; - -public class CmdBalance extends PIDCommand { - - private boolean LEFT; - // TODO: make it go further than the center (but corresponding to the side that we want to go) - - public CmdBalance () { - super( - new PIDController(1.5,0,0), - () -> Swerve.getInstance().getPose().getX(), - //() -> DriverStation.getAlliance() == Alliance.Red ? 12.55 : 3.95, - () -> DriverStation.getAlliance() == Alliance.Red ? FIELD_X_LENGTH - (chargingStationInnerX + chargingStationOuterX) / 2.0 : (chargingStationInnerX + chargingStationOuterX) / 2.0, - output -> Swerve.getInstance().drive(new Translation2d(0,0), 0,true), - Swerve.getInstance() - ); - m_controller.setTolerance(SwerveConstants.DRIVE_TOLERANCE); - } - - @Override - public void initialize() { - super.initialize(); - LEFT = Swerve.getInstance().getPose().getX() > m_setpoint.getAsDouble(); - double ff = LEFT ? -AutoConstants.BALANCE_FF : AutoConstants.BALANCE_FF; - m_useOutput = output -> Swerve.getInstance().drive(new Translation2d(output + ff,0), 0,true); - } - - @Override - public boolean isFinished() { - Pose2d pose = Swerve.getInstance().getPose(); - return m_controller.atSetpoint() || (pose.getX() < m_setpoint.getAsDouble() && LEFT) || (pose.getX() > m_setpoint.getAsDouble() && !LEFT); - } - -} \ No newline at end of file diff --git a/src/main/java/frc/team3128/commands/CmdBangBangBalance.java b/src/main/java/frc/team3128/commands/CmdBangBangBalance.java deleted file mode 100644 index c46d92d..0000000 --- a/src/main/java/frc/team3128/commands/CmdBangBangBalance.java +++ /dev/null @@ -1,50 +0,0 @@ -package frc.team3128.commands; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.team3128.common.utility.NAR_Shuffleboard; -import frc.team3128.subsystems.Swerve; - -public class CmdBangBangBalance extends CommandBase{ - private int plateauCount = 0; - private Swerve swerve; - private double maxPitch; - private static DoubleSupplier thresh; - private double pitchOffset; - - static { - thresh = NAR_Shuffleboard.debug("Aflack","Popeyes", 85, 0, 1); - } - - public CmdBangBangBalance() { - - swerve = Swerve.getInstance(); - plateauCount = 0; - maxPitch = 0; - pitchOffset = 0; - } - - @Override - public void initialize() { - plateauCount = 0; - maxPitch = Math.abs(swerve.getPitch()); - pitchOffset = swerve.getPitch(); - } - - @Override - public void execute() { - if (Math.abs(swerve.getPitch()) > maxPitch) { - plateauCount = 0; - maxPitch = Math.abs(swerve.getPitch()); - } - else { - plateauCount++; - } - } - - @Override - public boolean isFinished() { - return plateauCount >= thresh.getAsDouble() && Math.abs(swerve.getPitch() - pitchOffset) < 9; - } -} \ No newline at end of file diff --git a/src/main/java/frc/team3128/commands/CmdCurrentCheck.java b/src/main/java/frc/team3128/commands/CmdCurrentCheck.java deleted file mode 100644 index a326bd0..0000000 --- a/src/main/java/frc/team3128/commands/CmdCurrentCheck.java +++ /dev/null @@ -1,47 +0,0 @@ -package frc.team3128.commands; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.team3128.common.hardware.motorcontroller.NAR_TalonSRX; - -public class CmdCurrentCheck extends CommandBase { - private NAR_TalonSRX motor; - private double prevCurrent; - private double threshold; - private double absoluteThreshold; - private double count; - private boolean hasObject; - - public CmdCurrentCheck(NAR_TalonSRX motor, double threshold, double absoluteThreshold) { - this.motor = motor; - this.threshold = threshold; - this.absoluteThreshold = absoluteThreshold; - } - - @Override - public void initialize() { - prevCurrent = motor.getStatorCurrent(); - hasObject = false; - count = 0; - } - - @Override - public void execute() { - count++; - if (count >= 5) { - double current = motor.getStatorCurrent(); - SmartDashboard.putNumber("Current ROC", Math.abs(current - prevCurrent) / 0.1); - if (Math.abs(current - prevCurrent) / 0.1 > threshold) { - hasObject = true; - } - prevCurrent = current; - if (Math.abs(current) > absoluteThreshold) hasObject = true; - count = 0; - } - } - - @Override - public boolean isFinished() { - return hasObject; - } -} diff --git a/src/main/java/frc/team3128/commands/CmdDriveUp.java b/src/main/java/frc/team3128/commands/CmdDriveUp.java deleted file mode 100644 index 542e99c..0000000 --- a/src/main/java/frc/team3128/commands/CmdDriveUp.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.team3128.commands; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.team3128.subsystems.Swerve; - -public class CmdDriveUp extends CommandBase{ - private double power; - private Swerve swerve; - private Pose2d pose; - private double chargeStation; - - - public CmdDriveUp() { - - swerve = Swerve.getInstance(); - pose = swerve.getPose(); - - addRequirements(swerve); - } - - @Override - public void initialize() { - pose = swerve.getPose(); - chargeStation = DriverStation.getAlliance() == Alliance.Red ? 12.6 : 3.85; - power = pose.getX() > chargeStation ? -3: 3; - } - - @Override - public void execute() { - swerve.drive(new Translation2d(power,0), 0,true); - } - @Override - public void end(boolean interrupted) { - swerve.drive(new Translation2d(Math.copySign(0.6, power),0), 0,true); - } - - @Override - public boolean isFinished() { - return (Math.abs(swerve.getPitch()) >= 10); - } -} \ No newline at end of file diff --git a/src/main/java/frc/team3128/commands/CmdInPlaceTurn.java b/src/main/java/frc/team3128/commands/CmdInPlaceTurn.java deleted file mode 100644 index 4171c90..0000000 --- a/src/main/java/frc/team3128/commands/CmdInPlaceTurn.java +++ /dev/null @@ -1,52 +0,0 @@ -package frc.team3128.commands; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.PIDCommand; -import frc.team3128.subsystems.Swerve; -import frc.team3128.subsystems.Vision; - -import static frc.team3128.Constants.SwerveConstants.*; -import static frc.team3128.Constants.VisionConstants.TX_THRESHOLD; - -public class CmdInPlaceTurn extends PIDCommand { - - private double degrees; - - private boolean llInterrupt = false; - - private String camera; - - public CmdInPlaceTurn(double degrees, String camera) { - this(degrees); - llInterrupt = true; - this.camera = camera; - } - - public CmdInPlaceTurn(double degrees) { - super( - new PIDController(turnKP,turnKF,turnKD), - () -> Swerve.getInstance().getHeading(), - 0, - output -> Swerve.getInstance().drive(new Translation2d(), -output, false), - Swerve.getInstance() - ); - - m_controller.enableContinuousInput(-180, 180); - m_controller.setTolerance(TX_THRESHOLD); - - this.degrees = degrees; - } - - @Override - public void initialize() { - super.initialize(); - m_setpoint = ()-> MathUtil.inputModulus(Swerve.getInstance().getHeading() + degrees,-180,180); - } - - @Override - public boolean isFinished() { - return m_controller.atSetpoint() || (llInterrupt && Vision.getInstance().hasValidTarget(camera)); - } -} \ No newline at end of file diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 0cd66b7..ebd6339 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -6,17 +6,19 @@ import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.team3128.RobotContainer; +import frc.team3128.Constants.LedConstants.Colors; import frc.team3128.PositionConstants.Position; import frc.team3128.common.hardware.input.NAR_XboxController; -import frc.team3128.subsystems.Led; +import frc.team3128.subsystems.Leds; import frc.team3128.subsystems.Wrist; import frc.team3128.subsystems.Manipulator; import frc.team3128.subsystems.Swerve; +import frc.team3128.subsystems.Vision; import frc.team3128.subsystems.Elevator; public class CmdManager { - private static Led led = Led.getInstance(); + private static Leds leds = Leds.getInstance(); private static Wrist wrist = Wrist.getInstance(); private static Manipulator manipulator = Manipulator.getInstance(); private static Swerve swerve = Swerve.getInstance(); @@ -28,10 +30,13 @@ public class CmdManager { private CmdManager() {} - public static CommandBase score(Position position, int xPos, boolean runImmediately) { + private static CommandBase score(Position position, int xPos, boolean runImmediately) { return sequence( runOnce(()-> ENABLE = runImmediately), waitUntil(()-> ENABLE), + + //either(none(), new CmdTrajectory(xPos), ()-> runImmediately), + //waitUntil(()-> ENABLE), extend(position), waitUntil(()-> !ENABLE), outtake(), @@ -41,6 +46,10 @@ public static CommandBase score(Position position, int xPos, boolean runImmediat ); } + public static CommandBase score(Position position, boolean runImmediately) { + return score(position, 0, runImmediately); + } + public static CommandBase score(Position position, int xPos) { return score(position, xPos, false); } @@ -51,6 +60,7 @@ public static CommandBase HPpickup(Position position1, Position position2) { public static CommandBase pickup(Position position, boolean runImmediately) { return sequence( + runOnce(()->leds.setElevatorLeds(position.cone ? Colors.CONE : Colors.CUBE)), runOnce(()-> ENABLE = runImmediately), waitUntil(()-> ENABLE), extend(position), @@ -147,4 +157,11 @@ public static CommandBase resetWrist() { public static CommandBase resetSwerve() { return runOnce(()-> swerve.zeroGyro()); } + + public static CommandBase resetAll() { + return sequence( + resetWrist(), + resetElevator() + ); + } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/commands/CmdMove.java b/src/main/java/frc/team3128/commands/CmdMove.java deleted file mode 100644 index 21b5384..0000000 --- a/src/main/java/frc/team3128/commands/CmdMove.java +++ /dev/null @@ -1,300 +0,0 @@ -package frc.team3128.commands; - -import edu.wpi.first.math.controller.PIDController; -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.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.CommandBase; -import static frc.team3128.Constants.SwerveConstants.*; - -import java.util.function.DoubleSupplier; - -import static frc.team3128.Constants.FieldConstants.*; - -import frc.team3128.Constants.SwerveConstants; -import frc.team3128.common.utility.NAR_Shuffleboard; -import frc.team3128.subsystems.Swerve; -import frc.team3128.subsystems.Vision; - -@Deprecated -public class CmdMove extends CommandBase { - - public enum Type { - SCORE( - new double[][] { - new double[] {chargingStationInnerX - robotLength/2.0 - 0.01, chargingStationOuterX + robotLength/2.0 + 0.01}, - new double[] {LOADING_X_LEFT - robotLength/2.0, LOADING_X_RIGHT} - }, - new double[] { //Rectangular Constraint - chargingStationOuterX + robotLength/2.0 - 0.1, - FIELD_X_LENGTH/2, - chargingStationLeftY + robotLength/2.0 + 0.1, - chargingStationRightY - robotLength/2.0 - 0.1 - }, - 5.6, - true - ), - LOADING( - new double[][] { - new double[] {chargingStationInnerX - robotLength/2.0,chargingStationOuterX + robotLength/2.0} - }, - new double[] { //Rectangular Constraint - 0, //chargingStationInnerX - robotLength, - chargingStationInnerX - robotLength/2.0, - chargingStationLeftY + robotLength/2.0 + 0.1, - chargingStationRightY - robotLength/2.0 - 0.1 - }, - 2.3, - false - ), - NONE(null,null, null, false); - public double[][] xConstraints; - public double[] boxConstraints; - public Double deadLine; - public boolean movingLeft; //Relative to Blue - - private Type(double[][] xConstraints, double[] boxConstraints, Double deadLine, boolean movingLeft) { - this.xConstraints = xConstraints; - this.boxConstraints = boxConstraints; - this.deadLine = deadLine; - this.movingLeft = movingLeft; - } - } - - private static PIDController xController, yController; - private static PIDController xDeadController; - private static PIDController rController; - private static DoubleSupplier xAxis, yAxis, rAxis, throttle; - private boolean xSetpoint, ySetpoint, rSetpoint, atDestination; - private boolean inXDead; - protected Pose2d[] poses; - protected int index; - private Type type; - private double maxSpeed; - - private boolean joystickOverride; - - protected Swerve swerve; - - public CmdMove(Type type, boolean joystickOverride, double maxSpeed, Pose2d... poses) { - this.poses = poses; - this.type = type; - this.maxSpeed = maxSpeed; - this.joystickOverride = joystickOverride; - atDestination = false; - - swerve = Swerve.getInstance(); - index = 0; - - addRequirements(swerve); - } - - public CmdMove(Type type, boolean joystickOverride, Pose2d... poses) { - this(type, joystickOverride, SwerveConstants.maxSpeed, poses); - } - - public static void setController(DoubleSupplier x, DoubleSupplier y, DoubleSupplier r, DoubleSupplier accel) { - xAxis = x; - yAxis = y; - rAxis = r; - throttle = accel; - } - - static { - xController = new PIDController(translationKP, translationKI, translationKD); - yController = new PIDController(translationKP, translationKI, translationKD); - xDeadController = new PIDController(translationKP, translationKI, translationKD); - rController = new PIDController(rotationKP, rotationKI, rotationKD); - rController.enableContinuousInput(-Math.PI, Math.PI); - - xController.setTolerance(DRIVE_TOLERANCE); - xDeadController.setTolerance(DRIVE_TOLERANCE); - yController.setTolerance(DRIVE_TOLERANCE); - rController.setTolerance(Math.PI/60); - - NAR_Shuffleboard.addComplex("VisionPID","XCONTROLLER",xController,0,0); - NAR_Shuffleboard.addComplex("VisionPID","YCONTROLLER",yController,1,0); - NAR_Shuffleboard.addComplex("VisionPID","RCONTROLLER",rController,2,0); - } - - @Override - public void initialize() { - - index = 0; - boolean keepSkipping = true; - for (int i = 0; i < poses.length; i++) { - poses[i] = allianceFlip(poses[i]); - if (pastX(poses[i].getX()) && !atLastPoint() && keepSkipping && type != Type.NONE) { - index += 1; - continue; - } - keepSkipping = false; - } - - xSetpoint = false; - ySetpoint = false; - rSetpoint = false; - atDestination = false; - - inXDead = false; - //Vision.AUTO_ENABLED = false; - - xController.reset(); - yController.reset(); - rController.reset(); - - xController.setSetpoint(poses[index].getX()); - yController.setSetpoint(poses[index].getY()); - rController.setSetpoint(poses[index].getRotation().getRadians()); - if (type != Type.NONE) - xDeadController.setSetpoint(xDeadLine(type.deadLine)); - } - - @Override - public void execute() { - Pose2d pose = swerve.getPose(); - Rotation2d Rotation = swerve.getGyroRotation2d(); - double xDistance = xController.calculate(pose.getX()); - //xDistance = xDistance + Math.signum(xDistance) * VisionConstants.AUTO_FF; - double yDistance = yController.calculate(pose.getY()); - //yDistance = yDistance + Math.signum(yDistance) * VisionConstants.AUTO_FF; - double rotation = rController.calculate(Rotation.getRadians()); - - xSetpoint = xController.atSetpoint(); - ySetpoint = yController.atSetpoint(); - rSetpoint = rController.atSetpoint(); - - inXDead = !canMoveX(pose); - if (xSetpoint || inXDead) xDistance = 0; - if (ySetpoint || !canMoveY(pose)) yDistance = 0; - if (rSetpoint) rotation = 0; - - //if ((xDistance == 0) && !xSetpoint) yDistance = Math.copySign(maxSpeed, yDistance); - //Try adding logic so you can't move past the value until you clear the barrier - if (inXDead) - xDistance = xDeadController.calculate(pose.getX()); - if (nearBump()) - xDistance = Math.min(xDistance, bumpSpeed); - - if (Math.abs(yDistance) > maxSpeed) - xDistance = Math.min(Math.abs(xDistance), maxSpeed) * Math.signum(xDistance); - if (Math.abs(xDistance) > maxSpeed && Math.abs(yDistance) < maxSpeed) - xDistance = (Math.sqrt(Math.pow(maxSpeed,2) - Math.pow(yDistance,2))) * Math.signum(xDistance); - if (Math.abs(xDistance) < maxSpeed && yDistance > maxSpeed) - yDistance = (Math.sqrt(Math.pow(maxSpeed,2) - Math.pow(xDistance,2))) * Math.signum(yDistance); - - if (!Vision.AUTO_ENABLED) { - xDistance = 0; - yDistance = 0; - rotation = 0; - } - - if (joystickOverride) { - int team = DriverStation.getAlliance() == Alliance.Red ? 1 : -1; - if (Math.abs(xAxis.getAsDouble()) > 0.05 || Math.abs(yAxis.getAsDouble()) > 0.05 || Math.abs(rAxis.getAsDouble()) > 0.05) { - yDistance = xAxis.getAsDouble() * throttle.getAsDouble() * maxSpeed * team; - xDistance = yAxis.getAsDouble() * throttle.getAsDouble() * maxSpeed * -team; - rotation = -rAxis.getAsDouble() * maxAngularVelocity; - } - } - - Translation2d translation = new Translation2d(xDistance, yDistance); - if (translation.getNorm() > maxSpeed) { - Rotation2d driveAngle = translation.getAngle(); - translation = new Translation2d(maxSpeed, driveAngle); - } - - swerve.drive(translation, rotation, true); - - if (xSetpoint && ySetpoint && !atLastPoint()) { - nextPoint(); - return; - } - - atDestination = (xSetpoint && ySetpoint && atLastPoint()); - } - - private boolean canMoveX(Pose2d pose) { - if (type == Type.NONE) return true; - if (type == Type.SCORE && pose.getY() >= 5.1) return false; - double[] xConstraints = new double[] {type.boxConstraints[0], type.boxConstraints[1]}; - double[] yConstraints = new double[] {type.boxConstraints[2], type.boxConstraints[3]}; - return (!inXConstraints(xConstraints, pose.getX()) || !inYConstraints(yConstraints, pose.getY())); - } - - private boolean canMoveY(Pose2d pose) { - if (type == Type.NONE) return true; - // if (pose.getY() < chargingStationRightY - robotLength/2 - 0.05 && yController.getSetpoint() < chargingStationRightY - robotLength/2 - 0.05) return true; - // if (pose.getY() > chargingStationLeftY + robotLength/2 + 0.05 && yController.getSetpoint() > chargingStationLeftY + robotLength/2 + 0.05) return true; - double[][] constraints = type.xConstraints; - for (int i = 0; i < constraints.length; i ++) { - if (inXConstraints(constraints[i], pose.getX())){ - return false; - } - } - return true; - } - - private boolean inXConstraints(double[] constraints, double xPos) { - double left = DriverStation.getAlliance() == Alliance.Red ? FIELD_X_LENGTH - constraints[1] : constraints[0]; - double right = DriverStation.getAlliance() == Alliance.Red ? FIELD_X_LENGTH - constraints[0] : constraints[1]; - return (xPos >= left && xPos <= right); - } - - private double xDeadLine(double deadline) { - if (DriverStation.getAlliance() == Alliance.Red) - deadline = FIELD_X_LENGTH - deadline; - return deadline; - } - - private boolean inYConstraints(double[] constraints, double yPos) { - double top = constraints[0]; - double bottom = constraints[1]; - return (yPos >= bottom && yPos <= top); - } - - private boolean nearBump() { - Pose2d pose = swerve.getPose(); - return inXConstraints(new double[]{cableBumpInnerX - robotLength, cableBumpOuterX + robotLength}, pose.getX()) && - inYConstraints(new double[]{chargingStationRightY, 0}, FIELD_X_LENGTH); - } - - protected void nextPoint() { - if (atLastPoint()) return; - index += 1; - setPoint(poses[index]); - } - - protected void setPoint(Pose2d pose) { - xController.setSetpoint(pose.getX()); - yController.setSetpoint(pose.getY()); - rController.setSetpoint(pose.getRotation().getRadians()); - xSetpoint = false; - ySetpoint = false; - rSetpoint = false; - } - - protected boolean pastX(double x) { - Pose2d currentPos = swerve.getPose(); - boolean movingLeft = (type.movingLeft && DriverStation.getAlliance() == Alliance.Blue) || (!type.movingLeft && DriverStation.getAlliance() == Alliance.Red); - if (movingLeft) return (currentPos.getX() < x); - return (currentPos.getX() > x); - } - - protected boolean atLastPoint() { - return index == poses.length - 1; - } - - @Override - public boolean isFinished() { - return atDestination || Vision.MANUAL; - } - - @Override - public void end(boolean interrupted) { - swerve.stop(); - } - -} diff --git a/src/main/java/frc/team3128/commands/CmdMoveLoading.java b/src/main/java/frc/team3128/commands/CmdMoveLoading.java deleted file mode 100644 index 21d0851..0000000 --- a/src/main/java/frc/team3128/commands/CmdMoveLoading.java +++ /dev/null @@ -1,65 +0,0 @@ -package frc.team3128.commands; - -import java.util.Arrays; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.team3128.Constants.FieldConstants; -import frc.team3128.Constants.SwerveConstants; -import frc.team3128.Constants.VisionConstants; -import frc.team3128.subsystems.Vision; - -//Can't be bothered with this mess - Mason -@Deprecated -public class CmdMoveLoading extends CmdMove { - - public Pose2d[][] positions; - private int currentSelectedGrid; - private double PASS_LINE; - private boolean isReversed; - - public CmdMoveLoading(boolean isReversed, double maxSpeed, Pose2d[]... positions) { - super(CmdMove.Type.LOADING, true, maxSpeed, positions[0]); - this.positions = positions; - this.isReversed = isReversed; - } - - public CmdMoveLoading(boolean isReversed, Pose2d[]... positions) { - this(isReversed, SwerveConstants.maxSpeed, positions); - } - - @Override - public void initialize() { - currentSelectedGrid = Vision.SELECTED_GRID; - var newPoses = new Pose2d[positions.length + 2]; - newPoses[0] = swerve.getPose().nearest(Arrays.asList(VisionConstants.RAMP_AVOID_LOADING)); - newPoses[1] = VisionConstants.HPWall_Loading; //REMIND ME TO FIX LATER - for (int i = 2; i < newPoses.length; i ++) { - newPoses[i] = positions[i - 2][currentSelectedGrid]; - } - - if (isReversed) { - for (int i = 0; i < newPoses.length; i++) { - var newRotation = new Rotation2d(MathUtil.inputModulus(poses[i].getRotation().getRadians() + Math.PI, -Math.PI, Math.PI)); - poses[i] = new Pose2d(poses[i].getTranslation(), newRotation); - } - } - - PASS_LINE = FieldConstants.chargingStationInnerX + SwerveConstants.robotLength/2 + 0.02; - PASS_LINE = DriverStation.getAlliance() == Alliance.Red ? FieldConstants.FIELD_X_LENGTH - PASS_LINE : PASS_LINE; - - super.initialize(); - } - - @Override - public void execute() { - if ((pastX(PASS_LINE) && index == 0) || (swerve.getPose().getY() > VisionConstants.WALL_PASS && index == 1)) - nextPoint(); - - super.execute(); - } - -} diff --git a/src/main/java/frc/team3128/commands/CmdMovePickup.java b/src/main/java/frc/team3128/commands/CmdMovePickup.java deleted file mode 100644 index e173bfb..0000000 --- a/src/main/java/frc/team3128/commands/CmdMovePickup.java +++ /dev/null @@ -1,58 +0,0 @@ -package frc.team3128.commands; - -import java.util.Arrays; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.team3128.Constants.FieldConstants; -import frc.team3128.Constants.SwerveConstants; -import frc.team3128.Constants.VisionConstants; - -@Deprecated -public class CmdMovePickup extends CmdMove { - - public Pose2d position; - private double PASS_LINE; - private boolean isReversed; - - public CmdMovePickup(boolean isReversed, double maxSpeed, Pose2d position) { - super(CmdMove.Type.LOADING, false, maxSpeed, position); - this.position = position; - this.isReversed = isReversed; - } - - public CmdMovePickup(boolean isReversed, Pose2d position) { - this(isReversed, SwerveConstants.maxSpeed, position); - } - - @Override - public void initialize() { - var newPoses = new Pose2d[2]; - newPoses[0] = swerve.getPose().nearest(Arrays.asList(VisionConstants.RAMP_AVOID_LOADING)); - newPoses[1] = position; - if (isReversed) { - for (int i = 0; i < newPoses.length; i++) { - var newRotation = new Rotation2d(MathUtil.inputModulus(poses[i].getRotation().getRadians() + Math.PI, -Math.PI, Math.PI)); - poses[i] = new Pose2d(poses[i].getTranslation(), newRotation); - } - } - - poses = newPoses; - - PASS_LINE = FieldConstants.chargingStationInnerX + SwerveConstants.robotLength/2 + 0.02; - PASS_LINE = DriverStation.getAlliance() == Alliance.Red ? FieldConstants.FIELD_X_LENGTH - PASS_LINE : PASS_LINE; - - super.initialize(); - } - - @Override - public void execute() { - if ((pastX(PASS_LINE) && index == 0)) - nextPoint(); - super.execute(); - } - -} diff --git a/src/main/java/frc/team3128/commands/CmdMoveScore.java b/src/main/java/frc/team3128/commands/CmdMoveScore.java deleted file mode 100644 index 687ce2c..0000000 --- a/src/main/java/frc/team3128/commands/CmdMoveScore.java +++ /dev/null @@ -1,84 +0,0 @@ -package frc.team3128.commands; - -import java.util.ArrayList; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.team3128.Constants.FieldConstants; -import frc.team3128.Constants.SwerveConstants; -import frc.team3128.Constants.VisionConstants; -import frc.team3128.subsystems.Vision; - -@Deprecated -public class CmdMoveScore extends CmdMove { - - private double PASS_LINE; - private boolean[] overrides; - private ArrayList avoidRamp; - private Pose2d[][] positions; - private int currSelectedGrid; - private boolean isReversed; - - public CmdMoveScore(boolean[] overrides, boolean isReversed, double maxSpeed, Pose2d[]... positions) { - super(CmdMove.Type.SCORE, true, positions[0]); - this.positions = positions; - this.overrides = overrides; - this.isReversed = isReversed; - avoidRamp = new ArrayList(); - } - - public CmdMoveScore(boolean[] overrides, boolean isReversed, Pose2d[]... positions) { - this(overrides, isReversed, SwerveConstants.maxSpeed, positions); - } - - @Override - public void initialize() { - currSelectedGrid = Vision.SELECTED_GRID; - int extraPoint = 0; - var newPoses = new Pose2d[positions.length]; - Pose2d currentPose = swerve.getPose(); - if (overrides[currSelectedGrid]) { - avoidRamp.clear(); - extraPoint = 1; - newPoses = new Pose2d[positions.length + 1]; - for (Pose2d pose : VisionConstants.RAMP_AVOID_SCORE) { - avoidRamp.add(FieldConstants.allianceFlip(pose)); - } - newPoses[0] = currentPose.nearest(VisionConstants.RAMP_AVOID_SCORE); - } - for (int i = extraPoint; i < positions.length + extraPoint; i++) { - newPoses[i] = positions[i - extraPoint][currSelectedGrid]; - } - poses = newPoses; - if (isReversed) { - for (int i = 0; i < newPoses.length; i++) { - var newRotation = new Rotation2d(MathUtil.inputModulus(poses[i].getRotation().getRadians() + Math.PI, -Math.PI, Math.PI)); - poses[i] = new Pose2d(poses[i].getTranslation(), newRotation); - } - for (int i = 0; i < avoidRamp.size(); i++) { - var newRotation = new Rotation2d(MathUtil.inputModulus(avoidRamp.get(i).getRotation().getRadians() + Math.PI, -Math.PI, Math.PI)); - avoidRamp.set(i, new Pose2d(avoidRamp.get(i).getTranslation(), newRotation)); - } - } - PASS_LINE = FieldConstants.chargingStationOuterX - SwerveConstants.robotLength/2 - 0.02; - PASS_LINE = DriverStation.getAlliance() == Alliance.Red ? FieldConstants.FIELD_X_LENGTH - PASS_LINE : PASS_LINE; - super.initialize(); - } - - @Override - public void execute() { - if (overrides[currSelectedGrid]) { - if (pastX(PASS_LINE)) { - nextPoint(); - } - else if (!atLastPoint()) { - setPoint(swerve.getPose().nearest(avoidRamp)); - } - } - super.execute(); - } - -} diff --git a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java index 5a84f29..5a3fc96 100644 --- a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java +++ b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java @@ -2,13 +2,16 @@ import java.util.function.DoubleSupplier; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.team3128.common.utility.NAR_Shuffleboard; import frc.team3128.subsystems.Swerve; import static frc.team3128.Constants.SwerveConstants.*; @@ -23,6 +26,10 @@ public class CmdSwerveDrive extends CommandBase { private final DoubleSupplier zAxis; private final SlewRateLimiter accelLimiter; + + private final PIDController rController; + public static boolean enabled = false; + public static double rSetpoint; public CmdSwerveDrive(DoubleSupplier xAxis, DoubleSupplier yAxis, DoubleSupplier zAxis, boolean fieldRelative) { this.swerve = Swerve.getInstance(); @@ -33,6 +40,10 @@ public CmdSwerveDrive(DoubleSupplier xAxis, DoubleSupplier yAxis, DoubleSupplier this.zAxis = zAxis; accelLimiter = new SlewRateLimiter(maxAcceleration); + rController = new PIDController(turnKP, 0, 0); + rController.enableContinuousInput(0, 360); + rController.setTolerance(0.5); + swerve.fieldRelative = fieldRelative; } @@ -50,6 +61,16 @@ public void execute() { rotation = -zAxis.getAsDouble() * maxAngularVelocity * swerve.throttle; + if (Math.abs(rotation) > 0.25) { + enabled = false; + } + if (enabled) { + rotation = Units.degreesToRadians(rController.calculate(swerve.getGyroRotation2d().getDegrees(), rSetpoint)); + if (rController.atSetpoint()) { + rotation = 0; + } + } + Rotation2d driveAngle = translation.getAngle(); double slowedDist = accelLimiter.calculate(translation.getNorm()); // translation = new Translation2d(slowedDist, driveAngle); diff --git a/src/main/java/frc/team3128/commands/CmdTargetPursuit.java b/src/main/java/frc/team3128/commands/CmdTargetPursuit.java deleted file mode 100644 index 23f4914..0000000 --- a/src/main/java/frc/team3128/commands/CmdTargetPursuit.java +++ /dev/null @@ -1,89 +0,0 @@ -package frc.team3128.commands; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.CommandBase; -import static frc.team3128.Constants.VisionConstants.*; - -import static frc.team3128.Constants.SwerveConstants.*; - -import frc.team3128.subsystems.Swerve; -import frc.team3128.subsystems.Vision; - -public class CmdTargetPursuit extends CommandBase { - - private Swerve drive; - private Vision vision; - private String camera; - private Rotation2d camAngle; - - private PIDController distance, rotation; - private boolean atDistance, atRotation; - - private double dist; - - /** - * Aligns robot to ball and pursuits autonomously - * @Requirements Drivetrain - */ - public CmdTargetPursuit(String camera, double distance) { - drive = Swerve.getInstance(); - vision = Vision.getInstance(); - camAngle = vision.camSpecs(camera).offset.getRotation(); - - this.camera = camera; - this.dist = distance; - - initControllers(); - addRequirements(drive); - } - - public void initControllers() { - distance = new PIDController(distanceKP, distanceKI, distanceKD); - distance.setSetpoint(dist); - distance.setTolerance(DRIVE_TOLERANCE); - - rotation = new PIDController(alignKP, alignKI, alignKD); - rotation.enableContinuousInput(-180, 180); - rotation.setTolerance(TX_THRESHOLD); - rotation.setSetpoint(0); - } - - @Override - public void initialize() { - atDistance = false; - atRotation = false; - distance.reset(); - rotation.reset(); - } - - @Override - public void execute() { - if (vision.hasValidTarget(camera)) { - Double dist = !atDistance ? distance.calculate(vision.calculateDistance(camera)) : 0; - - Double spin = !atRotation ? rotation.calculate(vision.getTX(camera)) : 0; - drive.drive(new Translation2d(-dist,0).rotateBy(camAngle), -spin,false); - atDistance = distance.atSetpoint(); - atRotation = rotation.atSetpoint(); - } - else { - distance.reset(); - rotation.reset(); - drive.drive(new Translation2d(0,0), Units.degreesToRadians(120),false); - } - } - - @Override - public boolean isFinished() { - return atDistance && atRotation; - } - - @Override - public void end(boolean interrupted) { - drive.stop(); - } - -} \ No newline at end of file diff --git a/src/main/java/frc/team3128/commands/CmdTrajectory.java b/src/main/java/frc/team3128/commands/CmdTrajectory.java index e103dc3..534740d 100644 --- a/src/main/java/frc/team3128/commands/CmdTrajectory.java +++ b/src/main/java/frc/team3128/commands/CmdTrajectory.java @@ -3,16 +3,15 @@ import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPoint; -import com.pathplanner.lib.commands.PPSwerveControllerCommand; - 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.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.CommandBase; -import static frc.team3128.Constants.SwerveConstants.*; -import static frc.team3128.Constants.VisionConstants.*; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; + import static frc.team3128.Constants.FieldConstants.*; import java.util.ArrayList; @@ -24,79 +23,90 @@ import frc.team3128.autonomous.Trajectories; import frc.team3128.subsystems.Swerve; +import frc.team3128.subsystems.Vision; public class CmdTrajectory extends CommandBase { private final Swerve swerve; - private final int index; + private final int xPos; + private Pose2d endPoint; + private int index; private CommandBase trajCommand; - public CmdTrajectory(int index) { + public CmdTrajectory(int xPos) { swerve = Swerve.getInstance(); - this.index = index; - addRequirements(swerve); + this.xPos = xPos; + addRequirements(Vision.getInstance()); } - private PathPoint generatePoint(Translation2d translation, Rotation2d heading, Rotation2d holonomicAngle) { - return new PathPoint(allianceFlip(translation), allianceFlip(heading), allianceFlip(holonomicAngle)); + private PathPoint generatePoint(Pose2d pose, Rotation2d heading, double headingLength) { + return generatePoint(pose.getTranslation(), pose.getRotation(), heading, headingLength); } - private ArrayList generatePoses() { - final ArrayList poses = new ArrayList(); - final Translation2d start = swerve.getPose().getTranslation(); - final Rotation2d holonomicAngle = allianceFlip(END_POINTS[index].getRotation()); - poses.add(new PathPoint(start, swerve.getGyroRotation2d(), allianceFlip(HEADING_0))); - if (!pastPoint(start, CONDITION_1)) poses.add(generatePoint(POINT_1, HEADING_1, holonomicAngle)); - if (!pastPoint(start, CONDITION_2)) poses.add(new PathPoint(allianceFlip(start.nearest(POINT_2)), allianceFlip(HEADING_2), holonomicAngle)); - if (!pastPoint(start, CONDITION_3)) poses.add(new PathPoint(allianceFlip(start.nearest( POINT_3)), allianceFlip(HEADING_3), holonomicAngle)); - - return poses; + private PathPoint generatePoint(Translation2d translation, Rotation2d holonomicAngle, Rotation2d heading, double headingLength) { + final PathPoint point = new PathPoint(allianceFlip(translation), allianceFlip(heading), allianceFlip(holonomicAngle)); + point.prevControlLength = headingLength; + point.nextControlLength = headingLength; + return point; } - - private ArrayList generatePoints() { - final ArrayList points = new ArrayList(); - //final ArrayList positions = generatePoses(); - - // final Rotation2d heading = Rotation2d.fromDegrees(180); - // for (int i = 0; i < positions.size(); i++) { - // final Pose2d pose = positions.get(i); - // final PathPoint point = new PathPoint(pose.getTranslation(), heading, pose.getRotation()); - // points.add(point); - // } - - // return points; - return points; - } - - private boolean pastPoint(Translation2d start, double condition) { + private boolean isPastPoint(Translation2d start, double condition) { if (DriverStation.getAlliance() == Alliance.Blue) { return start.getX() < condition; } return start.getX() > FIELD_X_LENGTH - condition; } - @Override - public void initialize() { - final PathPlannerTrajectory trajectory = PathPlanner.generatePath( - pathConstraints, generatePoints() + private ArrayList generatePoses() { + final ArrayList pathPoints = new ArrayList(); + final Translation2d start = swerve.getPose().getTranslation(); + final Rotation2d holonomicAngle = END_POINTS[index].getRotation(); + final PathPoint startPoint = new PathPoint(start, allianceFlip(HEADING), swerve.getGyroRotation2d()); + final boolean topPath = start.getY() >= (POINT_2A.getY() + POINT_2B.getY()) / 2; + final boolean skipLastPoint = (topPath && index == 8) || (!topPath && index == 0); + startPoint.nextControlLength = 0.1; + pathPoints.add(startPoint); + if (!isPastPoint(start, CONDITION_1)) pathPoints.add(generatePoint(POINT_1, holonomicAngle, HEADING, 1)); + if (!isPastPoint(start, CONDITION_2)) pathPoints.add(generatePoint(topPath ? POINT_2B : POINT_2A, holonomicAngle, HEADING, 1)); + if (!isPastPoint(start, CONDITION_3) && !skipLastPoint) pathPoints.add(generatePoint(topPath ? POINT_3B : POINT_3A, holonomicAngle, HEADING, 0.5)); + pathPoints.add(generatePoint(END_POINTS[index], HEADING, 0.1)); + + return pathPoints; + } + + private CommandBase generateAuto() { + index = Vision.SELECTED_GRID * 3 + xPos; + final PathPlannerTrajectory trajectory = PathPlanner.generatePath(pathConstraints, generatePoses()); + return Commands.sequence( + Trajectories.generateAuto(trajectory), + Commands.run(()-> swerve.drive(new Translation2d(DriverStation.getAlliance() == Alliance.Blue ? -0.5 : 0.5, 0), 0, true)).withTimeout(0.5), + Commands.runOnce(()-> swerve.stop()) ); - trajCommand = Trajectories.generateAuto(trajectory); - trajCommand.schedule(); } @Override - public void execute() { - + public void initialize() { + trajCommand = generateAuto(); + trajCommand.schedule(); + CmdSwerveDrive.enabled = false; + endPoint = allianceFlip(END_POINTS[index]); } @Override - public boolean isFinished() { - return !trajCommand.isScheduled(); + public void execute() { + // if (Vision.AUTO_ENABLED) { + // trajCommand.schedule(); + // if (trajCommand.isScheduled()) trajCommand.cancel(); + // else { + // trajCommand = generateAuto(); + // trajCommand.schedule(); + // } + // Vision.AUTO_ENABLED = false; + // } } @Override - public void end(boolean interrupted) { - swerve.stop(); + public boolean isFinished(){ + return swerve.getPose().minus(endPoint).getTranslation().getNorm() < 0.5; } } diff --git a/src/main/java/frc/team3128/common/hardware/camera/NAR_Camera.java b/src/main/java/frc/team3128/common/hardware/camera/NAR_Camera.java index e1b3c98..5312416 100644 --- a/src/main/java/frc/team3128/common/hardware/camera/NAR_Camera.java +++ b/src/main/java/frc/team3128/common/hardware/camera/NAR_Camera.java @@ -312,9 +312,7 @@ private Transform2d getTarget(PhotonTrackedTarget target) { /** * Corrects for photonvision pose error when viewing tag at an angle * @return Transform2d of the target relative to the camera - * @deprecated Use {@link #getTest()} instead. */ - @Deprecated(forRemoval = true) public Transform2d getProcessedTarget() { return getProcessedTarget(bestTarget); } @@ -323,7 +321,6 @@ public Transform2d getProcessedTarget() { * Corrects for photonvision pose error when viewing tag at an angle * @param target the selected target * @return Transform2d of the target relative to the camera - * @deprecated Use {@link #getTest(PhotonTrackedTarget target)} instead. */ private Transform2d getProcessedTarget(PhotonTrackedTarget target) { if (!hasValidTarget() || !AprilTags.containsKey(targetId(target))) return new Transform2d(); @@ -338,7 +335,9 @@ private Transform2d getProcessedTarget(PhotonTrackedTarget target) { /** * Corrects for photonvision pose error when viewing tag at an angle * @return Transform2d of the target relative to the camera + * @deprecated Use {@link #getProcessedTarget()} instead. */ + @Deprecated(forRemoval = true) public Transform2d getTest() { return getTest(bestTarget); } @@ -347,6 +346,7 @@ public Transform2d getTest() { * Corrects for photonvision pose error when viewing tag at an angle * @param target the selected target * @return Transform2d of the target relative to the camera + * @deprecated Use {@link #getProcessedTarget(PhotonTrackedTarget target)} instead. */ private Transform2d getTest(PhotonTrackedTarget target) { if (!hasValidTarget() || !AprilTags.containsKey(targetId(target))) return new Transform2d(); diff --git a/src/main/java/frc/team3128/subsystems/Elevator.java b/src/main/java/frc/team3128/subsystems/Elevator.java index 0ccd5cf..a05cb42 100644 --- a/src/main/java/frc/team3128/subsystems/Elevator.java +++ b/src/main/java/frc/team3128/subsystems/Elevator.java @@ -28,6 +28,7 @@ public Elevator() { configMotors(); initShuffleboard(kS, kV, kG); m_controller.setTolerance(ELV_TOLERANCE); + resetEncoder(); } public void startPID(Position position) { @@ -66,12 +67,12 @@ public void stop() { } public void resetEncoder() { - m_elv1.setSelectedSensorPosition(0); + m_elv2.setSelectedSensorPosition(0); } @Override public double getMeasurement() { - return m_elv1.getSelectedSensorPosition() / GEAR_RATIO * SPOOL_CIRCUMFERENCE; + return m_elv2.getSelectedSensorPosition() / GEAR_RATIO * SPOOL_CIRCUMFERENCE; } public boolean pastFramePerimiter() { diff --git a/src/main/java/frc/team3128/subsystems/Led.java b/src/main/java/frc/team3128/subsystems/Led.java deleted file mode 100644 index b05e65c..0000000 --- a/src/main/java/frc/team3128/subsystems/Led.java +++ /dev/null @@ -1,88 +0,0 @@ -package frc.team3128.subsystems; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.team3128.Constants.LedConstants; -import frc.team3128.Constants.LedConstants.*; - -public class Led extends SubsystemBase{ - - private static Led instance; - private AddressableLED led; - private AddressableLEDBuffer ledBuffer; - - public Led(){ - initLEDs(); - } - - public static synchronized Led getInstance() { - if (instance == null) { - instance = new Led(); - } - return instance; - } - - public void initLEDs() { - led = new AddressableLED(LedConstants.PORT); - - ledBuffer = new AddressableLEDBuffer(LedConstants.LENGTH); - led.setLength(ledBuffer.getLength()); - - led.setData(ledBuffer); - led.start(); - - } - - //general color methods : use variables - public void setRGB(int r, int g, int b) { - for (var i = 0; i < ledBuffer.getLength(); i++) { - // Sets the specified LED to the RGB values - ledBuffer.setRGB(i, r, g, b); - } - - led.setData(ledBuffer); - } - - public void setHSV(int h, int s, int v) { - for (var i = 0; i < ledBuffer.getLength(); i++) { - // Sets the specified LED to the HSV values - ledBuffer.setHSV(i, h, s, v); - } - - led.setData(ledBuffer); - } - - //custom color methods : use constants - public void setAllianceColor() { - if (DriverStation.getAlliance() == DriverStation.Alliance.Blue) { - setHSV(Blue.HUE, Blue.SATURATION, Blue.VALUE); - } - else if (DriverStation.getAlliance() == DriverStation.Alliance.Red) { - setHSV(Red.HUE, Red.SATURATION, Red.VALUE); - } - } - - public void setColorYellow() { - setHSV(Yellow.HUE, Yellow.SATURATION, Yellow.VALUE); - } - - public void setColorPurple() { - setHSV(Purple.HUE, Purple.SATURATION, Purple.VALUE); - } - - public void setAutoColor() { - setHSV(Green.HUE, Green.SATURATION, Green.VALUE); - } - - public void setPickupColor(boolean cone) { - if (cone) { - setColorYellow(); - } else { - setColorPurple(); - } - } - - -} diff --git a/src/main/java/frc/team3128/subsystems/Leds.java b/src/main/java/frc/team3128/subsystems/Leds.java new file mode 100644 index 0000000..f30d2e3 --- /dev/null +++ b/src/main/java/frc/team3128/subsystems/Leds.java @@ -0,0 +1,82 @@ +package frc.team3128.subsystems; + +import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.CANdle.LEDStripType; +import com.ctre.phoenix.led.CANdle.VBatOutputMode; +import com.ctre.phoenix.CANifier.LEDChannel; +import com.ctre.phoenix.led.Animation; +import com.ctre.phoenix.led.CANdleConfiguration; +import com.ctre.phoenix.led.CANdleControlFrame; +import com.ctre.phoenix.led.CANdleStatusFrame; +import com.ctre.phoenix.led.RainbowAnimation; +import com.ctre.phoenix.led.RgbFadeAnimation; +import com.ctre.phoenix.led.SingleFadeAnimation; +import com.ctre.phoenix.led.ColorFlowAnimation.Direction; +import com.ctre.phoenix.led.LarsonAnimation.BounceMode; +import com.ctre.phoenix.led.TwinkleAnimation.TwinklePercent; +import com.ctre.phoenix.led.TwinkleOffAnimation.TwinkleOffPercent; + + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team3128.Constants.LedConstants; +import frc.team3128.Constants.LedConstants.Colors;; + + +public class Leds extends SubsystemBase { + private final CANdle m_candle = new CANdle(LedConstants.CANDLE_ID); + + private static Leds instance; + + public static Leds getInstance() { + if (instance == null) { + instance = new Leds(); + } + return instance; + } + + public Leds() { + configCandle(); + } + + private void configCandle() { + CANdleConfiguration config = new CANdleConfiguration(); + config.stripType = LEDStripType.RGB; + config.brightnessScalar = 1; + m_candle.configAllSettings(config); + } + + //Set Elevator Leds + public void setElevatorLeds(Colors color) { + + switch (color) { + case AUTO : + m_candle.animate(new RainbowAnimation(LedConstants.RainbowAnimation.BRIGHTNESS,LedConstants.RainbowAnimation.SPEED,LedConstants.PIVOT_COUNT_FRONT,false,LedConstants.STARTING_ID),0); + m_candle.animate(new RainbowAnimation(LedConstants.RainbowAnimation.BRIGHTNESS,LedConstants.RainbowAnimation.SPEED,LedConstants.PIVOT_COUNT_BACK,true,LedConstants.STARTING_ID+LedConstants.PIVOT_COUNT_FRONT),1); + break; + case HOLDING: + resetAnimationSlot(1,1); + m_candle.animate(new SingleFadeAnimation(color.r, color.g, color.b,LedConstants.WHITE_VALUE,LedConstants.HOLDING_SPEED,LedConstants.PIVOT_COUNT),0); + break; + default : + resetAnimationSlot(2); + m_candle.setLEDs(color.r,color.g,color.b,LedConstants.WHITE_VALUE,LedConstants.STARTING_ID,LedConstants.PIVOT_COUNT); + break; + } + } + + public void resetAnimationSlot(int slots) { + for (int i = 0; i < slots; i++) { + m_candle.animate(null,i); + } + } + + public void resetAnimationSlot(int slots, int offset) { + for (int i = 0; i < slots; i++) { + m_candle.animate(null,i+offset); + } + } + + + + +} \ No newline at end of file diff --git a/src/main/java/frc/team3128/subsystems/Manipulator.java b/src/main/java/frc/team3128/subsystems/Manipulator.java index 3b9aeb9..a57d7cd 100644 --- a/src/main/java/frc/team3128/subsystems/Manipulator.java +++ b/src/main/java/frc/team3128/subsystems/Manipulator.java @@ -28,7 +28,6 @@ public Manipulator(){ configMotor(); initShuffleboard(); } - private void configMotor(){ m_roller = new NAR_TalonSRX(ROLLER_MOTOR_ID); m_roller.setInverted(true); diff --git a/src/main/java/frc/team3128/subsystems/Swerve.java b/src/main/java/frc/team3128/subsystems/Swerve.java index 785030a..c94fce1 100644 --- a/src/main/java/frc/team3128/subsystems/Swerve.java +++ b/src/main/java/frc/team3128/subsystems/Swerve.java @@ -15,6 +15,10 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team3128.Constants.SwerveConstants.Mod0; +import frc.team3128.Constants.SwerveConstants.Mod1; +import frc.team3128.Constants.SwerveConstants.Mod2; +import frc.team3128.Constants.SwerveConstants.Mod3; import frc.team3128.Constants.VisionConstants; import frc.team3128.commands.CmdManager; import frc.team3128.common.swerveNeo.SwerveModule; @@ -36,6 +40,12 @@ public class Swerve extends SubsystemBase { public boolean fieldRelative; public double throttle = 0.8; + public double speed = 0; + private Translation2d prevTrans = new Translation2d(); + public double acceleration = 0; + private double prevSpeed = 0; + + private double initialRoll, initialPitch; public static synchronized Swerve getInstance() { if (instance == null) { @@ -49,6 +59,8 @@ public Swerve() { gyro.configFactoryDefault(); fieldRelative = true; estimatedPose = new Pose2d(); + zeroAxis(); + prevTrans = new Translation2d(); modules = new SwerveModule[] { new SwerveModule(0, Mod0.constants), @@ -98,6 +110,8 @@ public void initShuffleboard() { NAR_Shuffleboard.addComplex("Drivetrain","Drivetrain", this,0,0); NAR_Shuffleboard.addData("Drivetrain", "ENABLE", ()-> CmdManager.ENABLE, 0, 1); NAR_Shuffleboard.addData("Drivetrain", "Single Station", ()-> CmdManager.SINGLE_STATION, 0, 3); + NAR_Shuffleboard.addData("Drivetrain", "Speed", ()-> speed, 2, 1); + NAR_Shuffleboard.addData("Drivetrain", "Acceleration", ()-> acceleration, 3, 1); } public Pose2d getPose() { @@ -157,6 +171,19 @@ public void periodic() { for (SwerveModule module : modules) { SmartDashboard.putNumber("module " + module.moduleNumber, module.getCanCoder().getDegrees()); } + updateSpeed(); + updateAcceleration(); + } + + public void updateSpeed() { + Translation2d translation = getPose().getTranslation(); + speed = translation.getDistance(prevTrans) / 0.02; + prevTrans = translation; + } + + public void updateAcceleration(){ + acceleration = speed - prevSpeed / 0.02; + prevSpeed = speed; } public void resetAll() { @@ -190,15 +217,21 @@ public double getHeading() { } public double getPitch() { - return gyro.getPitch(); + return gyro.getRoll() - initialRoll; } public double getRoll() { - return gyro.getRoll(); + return gyro.getPitch() - initialPitch; } public void zeroGyro() { gyro.reset(); + zeroAxis(); + } + + public void zeroAxis() { + initialRoll = gyro.getRoll(); + initialPitch = gyro.getPitch(); } public void zeroGyro(double reset) { diff --git a/src/main/java/frc/team3128/subsystems/Vision.java b/src/main/java/frc/team3128/subsystems/Vision.java index 3b7b86c..8934b7b 100644 --- a/src/main/java/frc/team3128/subsystems/Vision.java +++ b/src/main/java/frc/team3128/subsystems/Vision.java @@ -14,7 +14,7 @@ public class Vision extends SubsystemBase{ public static int SELECTED_GRID = 0; - public static boolean AUTO_ENABLED = false; + public static boolean AUTO_ENABLED = true; public static boolean MANUAL = false; private HashMap cameras; @@ -35,8 +35,9 @@ public Vision() { NAR_Camera.setAprilTags(APRIL_TAG_POS); NAR_Camera.multipleTargets = false; cameras = new HashMap(); - cameras.put(FRONT.hostname, new NAR_Camera(FRONT)); - cameras.put(BACK.hostname, new NAR_Camera(BACK)); + cameras.put(FRONT_LEFT.hostname, new NAR_Camera(FRONT_LEFT)); + cameras.put(FRONT_RIGHT.hostname, new NAR_Camera(FRONT_RIGHT)); + cameras.put(BACK_RIGHT.hostname, new NAR_Camera(BACK_RIGHT)); } public Pose2d targetPos(String name, Pose2d robotPos) { @@ -123,8 +124,8 @@ public void periodic(){ } public void initShuffleboard() { - NAR_Camera cam = cameras.get(FRONT.hostname); - NAR_Camera cam2 = cameras.get(BACK.hostname); + NAR_Camera cam = cameras.get(FRONT_LEFT.hostname); + NAR_Camera cam2 = cameras.get(FRONT_RIGHT.hostname); NAR_Shuffleboard.addData("VisionComp", "HasTarget", ()->cam.hasValidTarget() || cam2.hasValidTarget(), 0, 0); NAR_Shuffleboard.addData("Vision","HasTarget", ()->cam.hasValidTarget(), 0, 0); @@ -148,14 +149,14 @@ public void initShuffleboard() { } public void logCameraAll() { - NAR_Shuffleboard.addData("Vision Urgent", "HasTarget", cameras.get(FRONT.hostname).hasValidTarget(),0,0); - NAR_Shuffleboard.addData("Vision Urgent", "Distance", calculateDistance(FRONT.hostname),1,0); - NAR_Shuffleboard.addData("Vision Urgent", "EstimatedPose",cameras.get(FRONT.hostname).getPos().toString(),0,1); - NAR_Shuffleboard.addData("Vision Urgent", "GEICO",cameras.get(FRONT.hostname).targetYaw(),2,0); - NAR_Shuffleboard.addData("Vision Urgent", "TARGETPOS",cameras.get(FRONT.hostname).getTargetPos(Swerve.getInstance().getPose()).toString(),0,2); - NAR_Shuffleboard.addData("Vision Urgent", "RAWTARGET",cameras.get(FRONT.hostname).getTarget().toString(),0,3); - NAR_Shuffleboard.addData("Vision Urgent", "TARGETGUITY",cameras.get(FRONT.hostname).targetAmbiguity(),3,0); - NAR_Shuffleboard.addData("Vision Urgent", "PROCESSED TARGET", cameras.get(FRONT.hostname).getProcessedTarget().toString(),0,4); - NAR_Shuffleboard.addData("Vision Urgent", "SUNSHINE", cameras.get(FRONT.hostname).getRawTarget().toString(),0,5); + NAR_Shuffleboard.addData("Vision Urgent", "HasTarget", cameras.get(FRONT_LEFT.hostname).hasValidTarget(),0,0); + NAR_Shuffleboard.addData("Vision Urgent", "Distance", calculateDistance(FRONT_LEFT.hostname),1,0); + NAR_Shuffleboard.addData("Vision Urgent", "EstimatedPose",cameras.get(FRONT_LEFT.hostname).getPos().toString(),0,1); + NAR_Shuffleboard.addData("Vision Urgent", "GEICO",cameras.get(FRONT_LEFT.hostname).targetYaw(),2,0); + NAR_Shuffleboard.addData("Vision Urgent", "TARGETPOS",cameras.get(FRONT_LEFT.hostname).getTargetPos(Swerve.getInstance().getPose()).toString(),0,2); + NAR_Shuffleboard.addData("Vision Urgent", "RAWTARGET",cameras.get(FRONT_LEFT.hostname).getTarget().toString(),0,3); + NAR_Shuffleboard.addData("Vision Urgent", "TARGETGUITY",cameras.get(FRONT_LEFT.hostname).targetAmbiguity(),3,0); + NAR_Shuffleboard.addData("Vision Urgent", "PROCESSED TARGET", cameras.get(FRONT_LEFT.hostname).getProcessedTarget().toString(),0,4); + NAR_Shuffleboard.addData("Vision Urgent", "SUNSHINE", cameras.get(FRONT_LEFT.hostname).getRawTarget().toString(),0,5); } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/subsystems/Wrist.java b/src/main/java/frc/team3128/subsystems/Wrist.java index a8f8a2f..e7b2473 100644 --- a/src/main/java/frc/team3128/subsystems/Wrist.java +++ b/src/main/java/frc/team3128/subsystems/Wrist.java @@ -43,7 +43,7 @@ private void configMotor() { m_wrist.setInverted(false); m_wrist.setIdleMode(IdleMode.kCoast); m_wrist.setSmartCurrentLimit(40); - // resetEncoder(); + resetEncoder(); } @Override @@ -57,7 +57,7 @@ public double getMeasurement() { } public void resetEncoder() { - m_wrist.setSelectedSensorPosition(0); + m_wrist.setSelectedSensorPosition(90 * GEAR_RATIO / ROTATION_TO_DEGREES); } public void set(double power) {