diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path index 4c72664..a3ca6ca 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path @@ -51,16 +51,16 @@ }, { "anchorPoint": { - "x": 6.6, - "y": 0.932272484336635 + "x": 6.7, + "y": 0.8 }, "prevControl": { - "x": 6.162259163045806, - "y": 0.932272484336635 + "x": 6.262259163045806, + "y": 0.8 }, "nextControl": { - "x": 6.162259163045806, - "y": 0.932272484336635 + "x": 6.262259163045806, + "y": 0.8 }, "holonomicAngle": 0, "isReversal": true, @@ -128,12 +128,12 @@ }, { "anchorPoint": { - "x": 7.060105736530918, - "y": 0.9122666370332734 + "x": 5.399956591634031, + "y": 0.6975629357864903 }, "prevControl": { - "x": 3.3978179327886435, - "y": 0.6569456350718256 + "x": 3.375880532069644, + "y": 0.7547402256046932 }, "nextControl": null, "holonomicAngle": 180.0, diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+2Cube.path b/src/main/deploy/pathplanner/b_cable_1Cone+2Cube.path index e38f49a..8eb0199 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+2Cube.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+2Cube.path @@ -51,15 +51,15 @@ }, { "anchorPoint": { - "x": 6.6, + "x": 6.7, "y": 0.932272484336635 }, "prevControl": { - "x": 6.162259163045806, + "x": 6.262259163045806, "y": 0.932272484336635 }, "nextControl": { - "x": 6.162259163045806, + "x": 6.262259163045806, "y": 0.932272484336635 }, "holonomicAngle": 0, @@ -153,16 +153,16 @@ }, { "anchorPoint": { - "x": 6.757234604746988, - "y": 1.839378192645274 + "x": 6.8, + "y": 1.9 }, "prevControl": { - "x": 6.020252404417489, - "y": 1.1023959923157753 + "x": 6.063017799670501, + "y": 1.1630177996705013 }, "nextControl": { - "x": 6.020252404417489, - "y": 1.1023959923157753 + "x": 6.063017799670501, + "y": 1.1630177996705013 }, "holonomicAngle": 45.0, "isReversal": true, diff --git a/src/main/deploy/pathplanner/b_hp_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_hp_1Cone+1Cube.path index 4882f1d..0815d6e 100644 --- a/src/main/deploy/pathplanner/b_hp_1Cone+1Cube.path +++ b/src/main/deploy/pathplanner/b_hp_1Cone+1Cube.path @@ -1 +1,169 @@ -{"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 +{ + "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.22842472218468, + "y": 4.688537765092648 + }, + "prevControl": { + "x": 4.257386517851278, + "y": 4.688537765092648 + }, + "nextControl": { + "x": 5.753247825698796, + "y": 4.688537765092648 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.7, + "y": 4.5 + }, + "prevControl": { + "x": 6.220390443348648, + "y": 4.517891127756518 + }, + "nextControl": { + "x": 6.220390443348648, + "y": 4.517891127756518 + }, + "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": 5.5, + "y": 4.7457150549108515 + }, + "prevControl": { + "x": 4.345018745672299, + "y": 4.8715050925108985 + }, + "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 index a9174a1..969c95a 100644 --- a/src/main/deploy/pathplanner/b_mid_1Cone+1Cube+Climb.path +++ b/src/main/deploy/pathplanner/b_mid_1Cone+1Cube+Climb.path @@ -77,15 +77,15 @@ { "anchorPoint": { "x": 6.707654896212571, - "y": 2.1609214485790087 + "y": 2.1 }, "prevControl": { "x": 6.036038306451613, - "y": 2.4445985645443513 + "y": 2.3836771159653427 }, "nextControl": { "x": 6.036038306451613, - "y": 2.4445985645443513 + "y": 2.3836771159653427 }, "holonomicAngle": 0, "isReversal": true, diff --git a/src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path b/src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path index de2334b..482ca41 100644 --- a/src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path +++ b/src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path @@ -1 +1,169 @@ -{"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 +{ + "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.9, + "y": 0.8 + }, + "prevControl": { + "x": 10.337740836954193, + "y": 0.8 + }, + "nextControl": { + "x": 10.337740836954193, + "y": 0.8 + }, + "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": 10.88897641418678, + "y": 0.7890465994912005 + }, + "prevControl": { + "x": 12.76439152022384, + "y": 0.6861274778184357 + }, + "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 index 482ed7d..d145b75 100644 --- a/src/main/deploy/pathplanner/r_cable_1Cone+2Cube.path +++ b/src/main/deploy/pathplanner/r_cable_1Cone+2Cube.path @@ -51,15 +51,15 @@ }, { "anchorPoint": { - "x": 9.950000000000001, + "x": 9.9, "y": 0.932272484336635 }, "prevControl": { - "x": 10.387740836954194, + "x": 10.337740836954193, "y": 0.932272484336635 }, "nextControl": { - "x": 10.387740836954194, + "x": 10.337740836954193, "y": 0.932272484336635 }, "holonomicAngle": 180, @@ -153,16 +153,16 @@ }, { "anchorPoint": { - "x": 9.792765395253014, - "y": 1.839378192645274 + "x": 9.75, + "y": 1.9 }, "prevControl": { - "x": 10.529747595582512, - "y": 1.1023959923157758 + "x": 10.486982200329498, + "y": 1.1630177996705018 }, "nextControl": { - "x": 10.529747595582512, - "y": 1.1023959923157758 + "x": 10.486982200329498, + "y": 1.1630177996705018 }, "holonomicAngle": 135.0, "isReversal": true, diff --git a/src/main/deploy/pathplanner/r_hp_1Cone+1Cube.path b/src/main/deploy/pathplanner/r_hp_1Cone+1Cube.path index f4fb3df..0874e2e 100644 --- a/src/main/deploy/pathplanner/r_hp_1Cone+1Cube.path +++ b/src/main/deploy/pathplanner/r_hp_1Cone+1Cube.path @@ -1 +1,169 @@ -{"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 +{ + "waypoints": [ + { + "anchorPoint": { + "x": 14.700000000000001, + "y": 5.0 + }, + "prevControl": null, + "nextControl": { + "x": 12.722549247876273, + "y": 4.697910493043208 + }, + "holonomicAngle": 360.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 11.197733779205077, + "y": 4.688537765092648 + }, + "prevControl": { + "x": 12.16877198353848, + "y": 4.688537765092648 + }, + "nextControl": { + "x": 10.67291067569096, + "y": 4.688537765092648 + }, + "holonomicAngle": 360.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.9, + "y": 4.5 + }, + "prevControl": { + "x": 10.337740836954193, + "y": 4.5 + }, + "nextControl": { + "x": 10.337740836954193, + "y": 4.5 + }, + "holonomicAngle": 180, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 11.22, + "y": 4.75 + }, + "prevControl": { + "x": 10.780755885058197, + "y": 4.75 + }, + "nextControl": { + "x": 11.659244114941805, + "y": 4.75 + }, + "holonomicAngle": 360.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 14.700000000000001, + "y": 4.4 + }, + "prevControl": { + "x": 13.414461624756584, + "y": 4.773758748906387 + }, + "nextControl": { + "x": 13.414461624756584, + "y": 4.773758748906387 + }, + "holonomicAngle": 360.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "ScoreCubeHigh" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 12.432763239278263, + "y": 4.802892344729054 + }, + "prevControl": { + "x": 13.473389913969557, + "y": 4.585618643419883 + }, + "nextControl": null, + "holonomicAngle": 360.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.5, + "maxAcceleration": 2.3, + "isReversed": null, + "markers": [ + { + "position": 1.1, + "names": [ + "PickupCube" + ] + }, + { + "position": 2.997869318181817, + "names": [ + "Neutral" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path b/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path index 3a0499a..a07a066 100644 --- a/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path +++ b/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path @@ -1 +1,142 @@ -{"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 +{ + "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.81, + "y": 2.1 + }, + "prevControl": { + "x": 10.481616589760959, + "y": 2.3836771159653427 + }, + "nextControl": { + "x": 10.481616589760959, + "y": 2.3836771159653427 + }, + "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/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 1a13c2a..dc8517e 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -80,9 +80,9 @@ 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 = 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 ANGLE_THRESHOLD = 8; //7, 9 + public static final double VELOCITY_THRESHOLD = 4; //6, 3 + public static final double RAMP_THRESHOLD = 9; //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; @@ -529,9 +529,9 @@ public static class ElevatorConstants { public static final double kI = 0; public static final double kD = 0; - public static final double kS = 0.975; + public static final double kS = 1.05; public static final double kV = 0; - public static final double kG = 1.05; + public static final double kG = 0.975; public static final double MIN_DIST = 2; //Ask Charlie public static final double MAX_DIST = 55; //Ask Charlie diff --git a/src/main/java/frc/team3128/PositionConstants.java b/src/main/java/frc/team3128/PositionConstants.java index 8407db5..ff90b96 100644 --- a/src/main/java/frc/team3128/PositionConstants.java +++ b/src/main/java/frc/team3128/PositionConstants.java @@ -4,7 +4,7 @@ public class PositionConstants { public static enum Position { HIGH_CONE(47, 0, true), HIGH_CUBE(48.5, 20, false), - MID_CONE(25.5, 28, true), + MID_CONE(41, -31, true), MID_CUBE(25, 30, false), LOW(3, 0, true), diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index d05b314..994fdbf 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -143,7 +143,7 @@ private void configureButtonBindings() { score(Position.HIGH_CUBE, 1) ); - buttonPad.getButton(13).onTrue(runOnce(()-> SINGLE_STATION = !SINGLE_STATION)); + buttonPad.getButton(13).onTrue(runOnce(()-> SINGLE_STATION = true)); buttonPad.getButton(14).onTrue(retract(Position.NEUTRAL).beforeStarting(stopManip())); diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index dce93e9..d038efd 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -2,12 +2,15 @@ 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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; 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.subsystems.Manipulator; import frc.team3128.subsystems.Swerve; import static frc.team3128.commands.CmdManager.*; @@ -51,8 +54,9 @@ private void initAutoSelector() { public Command getAutonomousCommand() { String selectedAutoName = NarwhalDashboard.getSelectedAutoName(); + // String selectedAutoName = "r_cable_1Cone+1Cube"; // String selectedAutoName = "b_cable_1Cone+2Cube+Climb"; //uncomment and change this for testing without opening Narwhal Dashboard - SmartDashboard.putString(selectedAutoName, selectedAutoName); + // SmartDashboard.putString(selectedAutoName, selectedAutoName); if (selectedAutoName == null) { return score(Position.HIGH_CONE, true).beforeStarting(resetAuto()); } @@ -62,6 +66,8 @@ public Command getAutonomousCommand() { public CommandBase resetAuto() { return sequence( + resetSwerve(DriverStation.getAlliance() == Alliance.Red ? 0 : 180), + runOnce(()-> Manipulator.getInstance().set(-0.4), Manipulator.getInstance()), resetAll(), retract(Position.NEUTRAL) ); diff --git a/src/main/java/frc/team3128/commands/CmdAutoBalance.java b/src/main/java/frc/team3128/commands/CmdAutoBalance.java index a87795b..b81f16d 100644 --- a/src/main/java/frc/team3128/commands/CmdAutoBalance.java +++ b/src/main/java/frc/team3128/commands/CmdAutoBalance.java @@ -69,7 +69,7 @@ public void execute() { return; } - swerve.drive(new Translation2d(onRamp ? DRIVE_SPEED * (advAngle > 0.0 ? 1.0 : -1.0) : DRIVE_SPEED, 0), 0, false); + swerve.drive(new Translation2d(onRamp ? DRIVE_SPEED * (advAngle > 0.0 ? 1.0 : -1.0) : DRIVE_SPEED * 1.5, 0), 0, false); } @Override diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index ebd6339..171117f 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -2,6 +2,8 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import static edu.wpi.first.wpilibj2.command.Commands.*; + +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -34,11 +36,12 @@ private static CommandBase score(Position position, int xPos, boolean runImmedia return sequence( runOnce(()-> ENABLE = runImmediately), waitUntil(()-> ENABLE), - + runOnce(()-> ENABLE = !runImmediately), //either(none(), new CmdTrajectory(xPos), ()-> runImmediately), //waitUntil(()-> ENABLE), extend(position), waitUntil(()-> !ENABLE), + waitSeconds(DriverStation.isAutonomous() ? 0.5 : 0), outtake(), waitSeconds(0.5), stopManip(), @@ -63,9 +66,10 @@ public static CommandBase pickup(Position position, boolean runImmediately) { runOnce(()->leds.setElevatorLeds(position.cone ? Colors.CONE : Colors.CUBE)), runOnce(()-> ENABLE = runImmediately), waitUntil(()-> ENABLE), - extend(position), - waitSeconds(0.2), - intake(position.cone), + parallel( + extend(position), + intake(position.cone) + ), retract(Position.NEUTRAL) ); } @@ -105,7 +109,7 @@ public static CommandBase moveWrist(Position position) { public static CommandBase intake(Boolean cone) { return sequence( runOnce(()-> manipulator.intake(cone), manipulator), - waitSeconds(0.4), + waitSeconds(0.2), waitUntil(()-> manipulator.hasObjectPresent()), waitSeconds(cone ? 0.15 : 0), runOnce(()-> manipulator.stallPower(), manipulator) @@ -154,6 +158,10 @@ public static CommandBase resetWrist() { return runOnce(()-> wrist.resetEncoder()); } + public static CommandBase resetSwerve(double angle) { + return runOnce(()-> swerve.zeroGyro(angle)); + } + public static CommandBase resetSwerve() { return runOnce(()-> swerve.zeroGyro()); } diff --git a/src/main/java/frc/team3128/subsystems/Swerve.java b/src/main/java/frc/team3128/subsystems/Swerve.java index c94fce1..f8dfa3a 100644 --- a/src/main/java/frc/team3128/subsystems/Swerve.java +++ b/src/main/java/frc/team3128/subsystems/Swerve.java @@ -217,11 +217,11 @@ public double getHeading() { } public double getPitch() { - return gyro.getRoll() - initialRoll; + return gyro.getPitch() - initialPitch; } public double getRoll() { - return gyro.getPitch() - initialPitch; + return gyro.getRoll() - initialRoll; } public void zeroGyro() {