From d0602b0bc2fd700b7b6d2c1f01e607c7749499ed Mon Sep 17 00:00:00 2001 From: "Team 3128: Controls" Date: Mon, 16 Oct 2023 15:50:39 -0700 Subject: [PATCH 01/12] BatB Changes --- .../pathplanner/b_cable_1Cone+1Cube.path | 20 +-- .../pathplanner/b_cable_1Cone+2Cube.path | 18 +- .../deploy/pathplanner/b_hp_1Cone+1Cube.path | 170 +++++++++++++++++- .../pathplanner/b_mid_1Cone+1Cube+Climb.path | 6 +- .../pathplanner/r_cable_1Cone+1Cube.path | 170 +++++++++++++++++- .../pathplanner/r_cable_1Cone+2Cube.path | 18 +- .../deploy/pathplanner/r_hp_1Cone+1Cube.path | 170 +++++++++++++++++- .../pathplanner/r_mid_1Cone+1Cube+Climb.path | 143 ++++++++++++++- src/main/java/frc/team3128/Constants.java | 10 +- .../java/frc/team3128/PositionConstants.java | 2 +- .../java/frc/team3128/RobotContainer.java | 2 +- .../frc/team3128/autonomous/AutoPrograms.java | 8 +- .../frc/team3128/commands/CmdAutoBalance.java | 2 +- .../frc/team3128/commands/CmdManager.java | 18 +- .../java/frc/team3128/subsystems/Swerve.java | 4 +- 15 files changed, 710 insertions(+), 51 deletions(-) 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() { From ba8d811ed123eae8c9eb80451cc6b40aa7b3fc54 Mon Sep 17 00:00:00 2001 From: "Team 3128: Controls" Date: Tue, 17 Oct 2023 18:39:26 -0700 Subject: [PATCH 02/12] Workshop 10/16 --- src/main/java/frc/team3128/commands/CmdManager.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 171117f..3fa7568 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -70,7 +70,8 @@ public static CommandBase pickup(Position position, boolean runImmediately) { extend(position), intake(position.cone) ), - retract(Position.NEUTRAL) + retract(Position.NEUTRAL), + runOnce(()-> leds.setElevatorLeds(Colors.DEFAULT)) ); } @@ -112,7 +113,8 @@ public static CommandBase intake(Boolean cone) { waitSeconds(0.2), waitUntil(()-> manipulator.hasObjectPresent()), waitSeconds(cone ? 0.15 : 0), - runOnce(()-> manipulator.stallPower(), manipulator) + runOnce(()-> manipulator.stallPower(), manipulator), + runOnce(()-> leds.setElevatorLeds(Colors.HOLDING)) ); } From 74c9796192eb323d6ef20cf57f7becea00641a6c Mon Sep 17 00:00:00 2001 From: "Team 3128: Controls" <35188310+ControlsNarwhal@users.noreply.github.com> Date: Thu, 19 Oct 2023 15:39:48 -0700 Subject: [PATCH 03/12] Workshop 10/17 --- .../pathplanner/b_cable_1Cone+1Cube.path | 49 ++++++++++++++----- .../frc/team3128/autonomous/AutoPrograms.java | 4 +- 2 files changed, 39 insertions(+), 14 deletions(-) diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path index a3ca6ca..ae7f89c 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path @@ -26,16 +26,16 @@ }, { "anchorPoint": { - "x": 5.3285851554342, - "y": 0.93 + "x": 4.765519009711006, + "y": 0.7788305624004619 }, "prevControl": { - "x": 4.357546951100798, - "y": 0.93 + "x": 3.794480805377604, + "y": 0.7788305624004619 }, "nextControl": { - "x": 5.853408258948316, - "y": 0.93 + "x": 5.290342113225122, + "y": 0.7788305624004619 }, "holonomicAngle": 180.0, "isReversal": false, @@ -51,16 +51,41 @@ }, { "anchorPoint": { - "x": 6.7, - "y": 0.8 + "x": 5.753304113243297, + "y": 0.8738098992785663 }, "prevControl": { - "x": 6.262259163045806, - "y": 0.8 + "x": 5.315913049286986, + "y": 0.8563142567203109 }, "nextControl": { - "x": 6.262259163045806, - "y": 0.8 + "x": 5.99075245543856, + "y": 0.8833078329663785 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.74108921677559, + "y": 0.9118016340298093 + }, + "prevControl": { + "x": 6.256694598697254, + "y": 0.9497933687810524 + }, + "nextControl": { + "x": 6.256694598697254, + "y": 0.9497933687810524 }, "holonomicAngle": 0, "isReversal": true, diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index d038efd..e7250e2 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -54,7 +54,7 @@ private void initAutoSelector() { public Command getAutonomousCommand() { String selectedAutoName = NarwhalDashboard.getSelectedAutoName(); - // String selectedAutoName = "r_cable_1Cone+1Cube"; + // String selectedAutoName = "b_cable_1Cone+1Cube"; // String selectedAutoName = "b_cable_1Cone+2Cube+Climb"; //uncomment and change this for testing without opening Narwhal Dashboard // SmartDashboard.putString(selectedAutoName, selectedAutoName); if (selectedAutoName == null) { @@ -67,7 +67,7 @@ public Command getAutonomousCommand() { public CommandBase resetAuto() { return sequence( resetSwerve(DriverStation.getAlliance() == Alliance.Red ? 0 : 180), - runOnce(()-> Manipulator.getInstance().set(-0.4), Manipulator.getInstance()), + runOnce(()-> Manipulator.getInstance().set(-0.35), Manipulator.getInstance()), resetAll(), retract(Position.NEUTRAL) ); From 0aa6cbb133a320539cf306a63c05a8dd10ef1182 Mon Sep 17 00:00:00 2001 From: "Team 3128: Controls" Date: Mon, 23 Oct 2023 15:35:03 -0700 Subject: [PATCH 04/12] Tidal Tumble Co-Authored-By: Mason-Lam <97353903+Mason-Lam@users.noreply.github.com> --- .../pathplanner/b_cable_1Cone+1Cube.path | 49 +++++-------------- .../deploy/pathplanner/b_hp_1Cone+1Cube.path | 49 +++++-------------- .../pathplanner/b_mid_1Cone+1Cube+Climb.path | 6 +-- .../pathplanner/r_mid_1Cone+1Cube+Climb.path | 6 +-- src/main/java/frc/team3128/Constants.java | 2 +- .../java/frc/team3128/PositionConstants.java | 4 +- .../java/frc/team3128/RobotContainer.java | 8 +-- .../frc/team3128/autonomous/AutoPrograms.java | 2 +- .../frc/team3128/commands/CmdAutoBalance.java | 2 +- .../frc/team3128/commands/CmdManager.java | 15 +++++- .../java/frc/team3128/subsystems/Swerve.java | 2 +- 11 files changed, 54 insertions(+), 91 deletions(-) diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path index ae7f89c..c0ce477 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path @@ -101,18 +101,18 @@ }, { "anchorPoint": { - "x": 5.33, - "y": 0.75 + "x": 2.7583658020383086, + "y": 0.7089983937457166 }, "prevControl": { - "x": 5.769244114941804, - "y": 0.75 + "x": 3.197609916980112, + "y": 0.7089983937457166 }, "nextControl": { - "x": 4.890755885058196, - "y": 0.75 + "x": 2.3191216870965046, + "y": 0.7089983937457166 }, - "holonomicAngle": 180.0, + "holonomicAngle": 0.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -130,35 +130,8 @@ "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": 5.399956591634031, - "y": 0.6975629357864903 - }, - "prevControl": { - "x": 3.375880532069644, - "y": 0.7547402256046932 + "x": 2.03793195032895, + "y": 0.9605784689458102 }, "nextControl": null, "holonomicAngle": 180.0, @@ -167,7 +140,9 @@ "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [], + "names": [ + "ScoreCubeHigh" + ], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0 diff --git a/src/main/deploy/pathplanner/b_hp_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_hp_1Cone+1Cube.path index 0815d6e..06e324c 100644 --- a/src/main/deploy/pathplanner/b_hp_1Cone+1Cube.path +++ b/src/main/deploy/pathplanner/b_hp_1Cone+1Cube.path @@ -76,18 +76,18 @@ }, { "anchorPoint": { - "x": 5.33, - "y": 4.75 + "x": 2.8727203816747147, + "y": 4.699973223056289 }, "prevControl": { - "x": 5.769244114941804, - "y": 4.75 + "x": 3.3052807643769393, + "y": 4.7763074082390355 }, "nextControl": { - "x": 4.890755885058196, - "y": 4.75 + "x": 2.483914810910934, + "y": 4.631360475274445 }, - "holonomicAngle": 180.0, + "holonomicAngle": 0.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -105,35 +105,8 @@ "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 + "x": 2.198028361819919, + "y": 4.5170058956380394 }, "nextControl": null, "holonomicAngle": 180.0, @@ -142,7 +115,9 @@ "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [], + "names": [ + "ScoreCubeHigh" + ], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0 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 969c95a..1c2fe60 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.1 + "y": 1.9 }, "prevControl": { "x": 6.036038306451613, - "y": 2.3836771159653427 + "y": 2.1836771159653425 }, "nextControl": { "x": 6.036038306451613, - "y": 2.3836771159653427 + "y": 2.1836771159653425 }, "holonomicAngle": 0, "isReversal": true, 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 a07a066..9990587 100644 --- a/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path +++ b/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path @@ -77,15 +77,15 @@ { "anchorPoint": { "x": 9.81, - "y": 2.1 + "y": 1.9 }, "prevControl": { "x": 10.481616589760959, - "y": 2.3836771159653427 + "y": 2.1836771159653425 }, "nextControl": { "x": 10.481616589760959, - "y": 2.3836771159653427 + "y": 2.1836771159653425 }, "holonomicAngle": 180, "isReversal": true, diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index dc8517e..45ff4e2 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -513,7 +513,7 @@ public static class WristConstants { 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_CONE = 0.15; public static final double STALL_POWER_CUBE = 0.1; diff --git a/src/main/java/frc/team3128/PositionConstants.java b/src/main/java/frc/team3128/PositionConstants.java index ff90b96..25f62db 100644 --- a/src/main/java/frc/team3128/PositionConstants.java +++ b/src/main/java/frc/team3128/PositionConstants.java @@ -3,10 +3,10 @@ public class PositionConstants { public static enum Position { HIGH_CONE(47, 0, true), - HIGH_CUBE(48.5, 20, false), + HIGH_CUBE(48.5, 35, false), MID_CONE(41, -31, true), MID_CUBE(25, 30, false), - LOW(3, 0, true), + LOW(3, 45, true), SHELF_CONE(53, -30, true), SHELF_CUBE(37, 22, false), diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index 994fdbf..a341aaf 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.team3128.commands.CmdAutoBalance; import frc.team3128.commands.CmdSwerveDrive; import frc.team3128.Constants.LedConstants.Colors; import frc.team3128.PositionConstants.Position; @@ -92,10 +93,9 @@ private void configureButtonBindings() { 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(()-> swerve.throttle = 1)).onFalse(runOnce(()-> swerve.throttle = 0.8)); controller.getButton("Start").onTrue(resetSwerve()); - 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.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true)).onFalse(retract(Position.NEUTRAL).andThen(stallPower()).andThen(setLeds(Colors.DEFAULT))); + controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true)).onFalse(retract(Position.NEUTRAL).andThen(stallPower()).andThen(setLeds(Colors.DEFAULT))); controller.getUpPOVButton().onTrue(runOnce(()-> { CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 180 : 0; @@ -131,6 +131,7 @@ private void configureButtonBindings() { rightStick.getButton(11).onTrue(intake(false)); rightStick.getButton(12).onTrue(outtake()); rightStick.getButton(13).onTrue(stopManip()); + rightStick.getButton(14).onTrue(new CmdAutoBalance()); buttonPad.getButton(5).onTrue( @@ -147,6 +148,7 @@ private void configureButtonBindings() { buttonPad.getButton(14).onTrue(retract(Position.NEUTRAL).beforeStarting(stopManip())); + buttonPad.getButton(16).onTrue( HPpickup(Position.CHUTE_CONE, Position.SHELF_CONE) ); diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index e7250e2..8c56c9e 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -67,7 +67,7 @@ public Command getAutonomousCommand() { public CommandBase resetAuto() { return sequence( resetSwerve(DriverStation.getAlliance() == Alliance.Red ? 0 : 180), - runOnce(()-> Manipulator.getInstance().set(-0.35), Manipulator.getInstance()), + 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 b81f16d..9032f7c 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 * 1.5, 0), 0, false); + swerve.drive(new Translation2d(onRamp ? DRIVE_SPEED * (advAngle > 0.0 ? 1.0 : -1.0) : DRIVE_SPEED * 2, 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 3fa7568..a6c953b 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -1,5 +1,6 @@ package frc.team3128.commands; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; import static edu.wpi.first.wpilibj2.command.Commands.*; @@ -37,11 +38,13 @@ private static CommandBase score(Position position, int xPos, boolean runImmedia runOnce(()-> ENABLE = runImmediately), waitUntil(()-> ENABLE), runOnce(()-> ENABLE = !runImmediately), + runOnce(()-> Swerve.getInstance().throttle = 0.6), //either(none(), new CmdTrajectory(xPos), ()-> runImmediately), //waitUntil(()-> ENABLE), extend(position), waitUntil(()-> !ENABLE), waitSeconds(DriverStation.isAutonomous() ? 0.5 : 0), + runOnce(()-> Swerve.getInstance().throttle = 1), outtake(), waitSeconds(0.5), stopManip(), @@ -112,9 +115,9 @@ public static CommandBase intake(Boolean cone) { runOnce(()-> manipulator.intake(cone), manipulator), waitSeconds(0.2), waitUntil(()-> manipulator.hasObjectPresent()), + runOnce(()-> leds.setElevatorLeds(Colors.HOLDING)), waitSeconds(cone ? 0.15 : 0), - runOnce(()-> manipulator.stallPower(), manipulator), - runOnce(()-> leds.setElevatorLeds(Colors.HOLDING)) + runOnce(()-> manipulator.stallPower(), manipulator) ); } @@ -126,6 +129,10 @@ public static CommandBase stopManip() { return runOnce(()-> manipulator.stopRoller(), manipulator); } + public static CommandBase stallPower() { + return runOnce(()-> manipulator.stallPower(), manipulator); + } + public static CommandBase vibrateController() { return new ScheduleCommand(new WaitCommand(0.5).deadlineWith(new StartEndCommand(() -> controller.startVibrate(), () -> RobotContainer.controller.stopVibrate()))); } @@ -168,6 +175,10 @@ public static CommandBase resetSwerve() { return runOnce(()-> swerve.zeroGyro()); } + public static CommandBase setLeds(Colors color) { + return runOnce(()-> leds.setElevatorLeds(color)); + } + public static CommandBase resetAll() { return sequence( resetWrist(), diff --git a/src/main/java/frc/team3128/subsystems/Swerve.java b/src/main/java/frc/team3128/subsystems/Swerve.java index f8dfa3a..e3fd64b 100644 --- a/src/main/java/frc/team3128/subsystems/Swerve.java +++ b/src/main/java/frc/team3128/subsystems/Swerve.java @@ -39,7 +39,7 @@ public class Swerve extends SubsystemBase { private Field2d field; public boolean fieldRelative; - public double throttle = 0.8; + public double throttle = 1; public double speed = 0; private Translation2d prevTrans = new Translation2d(); public double acceleration = 0; From 3b8b02d928477e697331e2769871fbd49dc9d9c4 Mon Sep 17 00:00:00 2001 From: Mason-Lam <97353903+Mason-Lam@users.noreply.github.com> Date: Tue, 24 Oct 2023 04:32:51 -0700 Subject: [PATCH 05/12] Post Tidal Changes Massive code cleanup because I can't sleep --- src/main/deploy/example.txt | 3 - .../pathplanner/b_mid_1Cone+1Cube+Climb.path | 4 +- .../deploy/pathplanner/b_mid_1Cone+Climb.path | 8 +- .../pathplanner/r_mid_1Cone+1Cube+Climb.path | 4 +- .../deploy/pathplanner/r_mid_1Cone+Climb.path | 82 +++++++- src/main/deploy/pose.txt | 0 src/main/java/frc/team3128/Constants.java | 196 ++---------------- src/main/java/frc/team3128/Robot.java | 7 +- .../java/frc/team3128/RobotContainer.java | 83 ++------ .../frc/team3128/autonomous/AutoPrograms.java | 66 ++---- .../frc/team3128/autonomous/Trajectories.java | 72 +++---- .../frc/team3128/commands/CmdAutoBalance.java | 18 +- .../frc/team3128/commands/CmdManager.java | 42 +++- .../frc/team3128/commands/CmdTrajectory.java | 2 +- .../common/constantsint/ConstantsInt.java | 2 - .../common/constantsint/ConstantsInt2.java | 1 - .../team3128/common/swerve/SwerveModule.java | 1 - .../common/swerveNeo/SwerveConversions.java | 7 +- .../common/swerveNeo/SwerveModule.java | 2 +- .../frc/team3128/subsystems/Elevator.java | 3 - .../java/frc/team3128/subsystems/Leds.java | 12 +- .../frc/team3128/subsystems/Manipulator.java | 20 +- .../java/frc/team3128/subsystems/Swerve.java | 6 +- .../java/frc/team3128/subsystems/Vision.java | 7 +- 24 files changed, 239 insertions(+), 409 deletions(-) delete mode 100644 src/main/deploy/example.txt delete mode 100644 src/main/deploy/pose.txt diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt deleted file mode 100644 index bb82515..0000000 --- a/src/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function -to get a proper path relative to the deploy directory. \ 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 1c2fe60..0bcea67 100644 --- a/src/main/deploy/pathplanner/b_mid_1Cone+1Cube+Climb.path +++ b/src/main/deploy/pathplanner/b_mid_1Cone+1Cube+Climb.path @@ -115,7 +115,9 @@ "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [], + "names": [ + "Balance" + ], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0 diff --git a/src/main/deploy/pathplanner/b_mid_1Cone+Climb.path b/src/main/deploy/pathplanner/b_mid_1Cone+Climb.path index 4ffb024..d81b0fc 100644 --- a/src/main/deploy/pathplanner/b_mid_1Cone+Climb.path +++ b/src/main/deploy/pathplanner/b_mid_1Cone+Climb.path @@ -51,11 +51,11 @@ }, { "anchorPoint": { - "x": 5.5, + "x": 5.7, "y": 2.8069653465695823 }, "prevControl": { - "x": 5.098965119521481, + "x": 5.2989651195214815, "y": 2.8069653465695823 }, "nextControl": null, @@ -65,7 +65,9 @@ "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [], + "names": [ + "Balance" + ], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0 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 9990587..e10c8e7 100644 --- a/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path +++ b/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path @@ -115,7 +115,9 @@ "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [], + "names": [ + "Balance" + ], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0 diff --git a/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path b/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path index 0923836..17d9064 100644 --- a/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path +++ b/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path @@ -1 +1,81 @@ -{"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 +{ + "waypoints": [ + { + "anchorPoint": { + "x": 14.700000000000001, + "y": 2.2 + }, + "prevControl": null, + "nextControl": { + "x": 13.564766425366185, + "y": 2.4664020387068555 + }, + "holonomicAngle": 360.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 12.684045240312702, + "y": 2.7413229722292773 + }, + "prevControl": { + "x": 13.321274967897159, + "y": 2.7392933935173427 + }, + "nextControl": { + "x": 12.098499930650991, + "y": 2.743187935905513 + }, + "holonomicAngle": 360.0, + "isReversal": false, + "velOverride": 4.0, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 10.85, + "y": 2.8069653465695823 + }, + "prevControl": { + "x": 11.25103488047852, + "y": 2.8069653465695823 + }, + "nextControl": null, + "holonomicAngle": 360.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "Balance" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 1.0, + "maxAcceleration": 2.0, + "isReversed": null, + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pose.txt b/src/main/deploy/pose.txt deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 45ff4e2..2bca0dd 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -1,11 +1,6 @@ package frc.team3128; -import static frc.team3128.common.hardware.motorcontroller.MotorControllerConstants.FALCON_ENCODER_RESOLUTION; -import static frc.team3128.common.hardware.motorcontroller.MotorControllerConstants.SPARKMAX_ENCODER_RESOLUTION; - -import java.util.ArrayList; import java.util.HashMap; -import java.util.List; import com.pathplanner.lib.PathConstraints; @@ -23,7 +18,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; -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.Transform2d; @@ -31,13 +25,6 @@ public class Constants { - public static class ConversionConstants { - - public static final double SPARK_VELOCITY_FACTOR = SPARKMAX_ENCODER_RESOLUTION / 60; // RPM to nu/s - public static final double FALCON_NUp100MS_TO_RPM = 10 * 60 / FALCON_ENCODER_RESOLUTION; // sensor units per 100 ms to rpm - public static final double FALCON_NUpS_TO_RPM = 60 / FALCON_ENCODER_RESOLUTION; // sensor units per second to rpm - } - public static class TrajectoryConstants { public static final Rotation2d HEADING = Rotation2d.fromDegrees(180); @@ -66,67 +53,32 @@ public static class TrajectoryConstants { } public static class AutoConstants { - public static final PathConstraints pathConstraints = new PathConstraints(SwerveConstants.maxSpeed, SwerveConstants.maxAcceleration); - public static final Pose2d PICKUP_1 = new Pose2d(5.9, 0.95, Rotation2d.fromDegrees(0)); - public static final Pose2d PICKUP_2 = new Pose2d(7.1, 2.28, Rotation2d.fromDegrees(45)); - public static final Pose2d PICKUP_3 = new Pose2d(7.1, 3.3, Rotation2d.fromDegrees(-45)); - public static final Pose2d PICKUP_4 = new Pose2d(5.9, 4.55, Rotation2d.fromDegrees(0)); + public static final double slowSpeed = 1.5; + public static final double slowAcceleration = 2; - public static final Pose2d MOBILITY_BOTTOM = new Pose2d(5.5, 0.95, Rotation2d.fromDegrees(0)); - public static final Pose2d MOBILITY_TOP = new Pose2d(5.5, 4.5, Rotation2d.fromDegrees(0)); + public static final PathConstraints fast = new PathConstraints(SwerveConstants.maxSpeed, SwerveConstants.maxAcceleration); + public static final PathConstraints slow = new PathConstraints(slowSpeed, slowAcceleration); - public static final Pose2d ClimbSetupInside = new Pose2d(2.2, 2.7, Rotation2d.fromDegrees(0)); - 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)); + /* Translation PID Values */ + public static final double translationKP = 3; + public static final double translationKI = 0; + public static final double translationKD = 0; + + /* Rotation PID Values */ + public static final double rotationKP = 2; + public static final double rotationKI = 0; + public static final double rotationKD = 0; 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; - public static final double kD = 0; - - public static final Pose2d[] STARTING_POINTS = new Pose2d[] { - new Pose2d(1.85 ,0.5, Rotation2d.fromDegrees(180)), - new Pose2d(1.85 ,1.05, Rotation2d.fromDegrees(180)), - new Pose2d(1.85 ,1.65, Rotation2d.fromDegrees(180)), - new Pose2d(1.85 ,2.15, Rotation2d.fromDegrees(180)), - new Pose2d(1.85 ,2.75, Rotation2d.fromDegrees(0)), - new Pose2d(1.85 ,3.3, Rotation2d.fromDegrees(180)), - new Pose2d(1.85 ,3.85, Rotation2d.fromDegrees(180)), - new Pose2d(1.85 ,4.45, Rotation2d.fromDegrees(180)), - new Pose2d(1.85 ,5, Rotation2d.fromDegrees(180)) - }; - 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)) - }; - - public static final Pose2d[][] SCORES_GRID = new Pose2d[][] { - new Pose2d[] {SCORES[0], SCORES[3], SCORES[6]}, - new Pose2d[] {SCORES[1], SCORES[4], SCORES[7]}, - new Pose2d[] {SCORES[2], SCORES[5], SCORES[8]} - }; - - 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 { public static final int pigeonID = 30; - public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW- /* Drivetrain Constants */ public static final double bumperLength = Units.inchesToMeters(5); @@ -161,31 +113,6 @@ public static class SwerveConstants { public static final double drivePeakCurrentDuration = 0.1; public static final boolean driveEnableCurrentLimit = true; - - public static final double TURN_TOLERANCE = 5; - - public static final double DRIVE_TOLERANCE = 0.04; - - /* Translation PID Values */ - public static final double translationKP = 3; - public static final double translationKI = 0; - public static final double translationKD = 0; - - /* Translation PID Values */ - public static final double distanceKP = 3; - public static final double distanceKI = 0; - public static final double distanceKD = 0; - - /* Rotation PID Values */ - public static final double alignKP = 0.05; - public static final double alignKI = 0; - public static final double alignKD = 0; - - /* Rotation PID Values */ - public static final double rotationKP = 2; - public static final double rotationKI = 0; - public static final double rotationKD = 0; - /* Turning PID Values */ public static final double turnKP = 5; public static final double turnKI = 0; @@ -208,16 +135,14 @@ public static class SwerveConstants { public static final double driveKS = 0.19255;//0.60094; // 0.19225; public static final double driveKV = 2.4366;//1.1559; // 2.4366 public static final double driveKA = 0.34415; //0.12348; // 0.34415 - public static final double turnTolerance = 2; /* Swerve Profiling Values */ // Theoretical: v = 4.96824, omega = 11.5 // Real: v = 4.5, omega = 10 // 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.5; - public static final double maxAngularVelocity = 5; //3; //11.5; // citrus: 10 + public static final double maxAngularVelocity = 5; //3; //11.5; // citrus: 10 - Mason look at this later wtf public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(maxSpeed, maxAcceleration); /* Motor Inverts */ @@ -227,10 +152,6 @@ public static class SwerveConstants { /* Angle Encoder Invert */ public static final boolean canCoderInvert = false; - /* Auto-Balance Constants */ - public static final double BEAM_BALANCED_GOAL_DEGREES = 0; - public static final double BEAM_BALANACED_DRIVE_KP = 0.015; // P (Proportional) constant of a PID loop - /* Module Specific Constants */ /* Front Left Module - Module 0 */ // TODO: Figure out angle offsets @@ -292,82 +213,14 @@ public static class VisionConstants { 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); - public static final PIDController rController = new PIDController(1, 0, 0); - - public static final double SCREEN_WIDTH = 320; - public static final double SCREEN_HEIGHT = 240; - - public static final double HORIZONTAL_FOV = 59.6; //degrees - public static final double VERTICAL_FOV = 45.7; //degrees - - public static final double TX_THRESHOLD = 3; // degrees - - public static final double POSE_THRESH = 5; - - public static final double AUTO_FF = 0.1; - - public static final double ANGLE_THRESHOLD = 10; // degrees - - public static final double TARGET_AREA = 6.25 * 6.25; //inches + public static final double POSE_THRESH = 100; public static final Matrix SVR_STATE_STD = VecBuilder.fill(0.1,0.1,Units.degreesToRadians(3)); 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.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[][] { - new Pose2d[] {SCORES[0], SCORES[3], SCORES[6]}, - new Pose2d[] {SCORES[1], SCORES[4], SCORES[7]}, - new Pose2d[] {SCORES[2], SCORES[5], SCORES[8]} - }; - - public static final boolean[][] RAMP_OVERRIDE = new boolean[][] { - new boolean[] {true, true, true}, //false, true, true - new boolean[] {true, true, true}, //false, true, false - new boolean[] {true, true, true} //true, true, false - }; - - public static final ArrayList RAMP_AVOID_SCORE = new ArrayList(); - - public static final Pose2d[] SCORE_SETUP = new Pose2d[]{ - new Pose2d(5.3,0.75,Rotation2d.fromDegrees(180)), - new Pose2d(5.3,2.75,Rotation2d.fromDegrees(180)), - new Pose2d(5.3,4.6,Rotation2d.fromDegrees(180)), - }; - - public static final Pose2d[] LOADING_ZONE = new Pose2d[] { - new Pose2d(15.1,6,Rotation2d.fromDegrees(0)), - new Pose2d(Units.inchesToMeters(636.96-76.925),Units.inchesToMeters(265.74+54.5-26), Rotation2d.fromDegrees(90)), - new Pose2d(15.1,7.3, Rotation2d.fromDegrees(0)) - }; - - public static final Pose2d[] RAMP_AVOID_LOADING = new Pose2d[] { - new Pose2d(10, 0.95, new Rotation2d()), - new Pose2d(10, 4.5, new Rotation2d()) - }; - - public static final Pose2d HPWall_Loading = new Pose2d(12.4, 7.5, new Rotation2d()); - - public static final double WALL_PASS = 5.8 + SwerveConstants.robotLength/2.0; - public static final HashMap APRIL_TAG_POS = new HashMap(); - public static final HashMap TestTags = new HashMap(); - static { APRIL_TAG_POS.put(1, new Pose2d( new Translation2d(Units.inchesToMeters(610.77), Units.inchesToMeters(42.19)), @@ -401,13 +254,6 @@ public static class VisionConstants { new Translation2d(Units.inchesToMeters(40.45), Units.inchesToMeters(42.19)), Rotation2d.fromDegrees(0)) ); - - TestTags.put(8, APRIL_TAG_POS.get(3)); - TestTags.put(7, APRIL_TAG_POS.get(2)); - TestTags.put(6,APRIL_TAG_POS.get(1)); - - RAMP_AVOID_SCORE.add(new Pose2d(2.1,4.87, Rotation2d.fromDegrees(180))); - RAMP_AVOID_SCORE.add(new Pose2d(2.1, 0.4, Rotation2d.fromDegrees(180))); } } @@ -545,15 +391,6 @@ public static class ElevatorConstants { public static final int CURRENT_LIMIT = 80; } - public static class BalanceConstants{ - public static final double turnKP = 6; - public static final double turnKI = 0; - public static final double turnKD = .005; - public static final double TURN_TOLERANCE = 1.5; - public static final double CHARGE_STATION_X = 5; - public static final double CHARGE_STATION_Y = 5; - } - public static class LedConstants{ public static final int CANDLE_ID = 52; @@ -579,7 +416,8 @@ public enum Colors { HOLDING(255,0,0,false), AUTO(0,0,0,true), - DEFAULT(0,0,225,false); + SHELF(0, 255, 0, false), + CHUTE(0,0,225,false); public final int r; public final int b; diff --git a/src/main/java/frc/team3128/Robot.java b/src/main/java/frc/team3128/Robot.java index ad6c9cd..46ff8c4 100644 --- a/src/main/java/frc/team3128/Robot.java +++ b/src/main/java/frc/team3128/Robot.java @@ -10,7 +10,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.team3128.Constants.LedConstants.Colors; import frc.team3128.autonomous.AutoPrograms; +import frc.team3128.subsystems.Leds; import frc.team3128.subsystems.Swerve; /** @@ -37,7 +39,6 @@ public static synchronized Robot getInstance() { @Override public void robotInit(){ LiveWindow.disableAllTelemetry(); - // Pivot.getInstance().offset = Pivot.getInstance().getAngle(); } @Override @@ -50,6 +51,7 @@ public void autonomousInit() { m_robotContainer.init(); timer = new Timer(); m_autonomousCommand = autoPrograms.getAutonomousCommand(); + Leds.getInstance().defaultColor = Colors.AUTO; if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); timer.start(); @@ -66,6 +68,7 @@ public void autonomousPeriodic() { @Override public void teleopInit() { + Leds.getInstance().defaultColor = Colors.CHUTE; m_robotContainer.init(); xlockTimer = new Timer(); xlockTimer.start(); @@ -76,7 +79,7 @@ public void teleopInit() { public void teleopPeriodic() { CommandScheduler.getInstance().run(); if (xlockTimer.hasElapsed(134.75)) { - //new RunCommand(()->Swerve.getInstance().xlock(), Swerve.getInstance()).schedule(); + 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 a341aaf..06c9597 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -1,35 +1,24 @@ package frc.team3128; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.DriverStation.MatchType; import edu.wpi.first.wpilibj2.command.CommandScheduler; 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.button.Trigger; import frc.team3128.commands.CmdAutoBalance; import frc.team3128.commands.CmdSwerveDrive; -import frc.team3128.Constants.LedConstants.Colors; import frc.team3128.PositionConstants.Position; import static frc.team3128.commands.CmdManager.*; import frc.team3128.common.hardware.input.NAR_ButtonBoard; import frc.team3128.common.hardware.input.NAR_Joystick; import frc.team3128.common.hardware.input.NAR_XboxController; import frc.team3128.common.narwhaldashboard.NarwhalDashboard; -import frc.team3128.common.utility.Log; -import frc.team3128.subsystems.Elevator; -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; -import java.util.function.BooleanSupplier; - /** * Command-based is a "declarative" paradigm, very little robot logic should * actually be handled in the {@link Robot} periodic methods (other than the @@ -39,48 +28,22 @@ public class RobotContainer { private Swerve swerve; - private Vision vision; - private Leds leds; - private Elevator elevator; - private NAR_Joystick leftStick; private NAR_Joystick rightStick; private NAR_ButtonBoard buttonPad; - private NAR_XboxController operatorController; public static NAR_XboxController controller; - private CommandScheduler commandScheduler = CommandScheduler.getInstance(); - - public static BooleanSupplier DEBUG = ()-> false; - - private Trigger inProtected; - public RobotContainer() { - NAR_Shuffleboard.addData("DEBUG", "DEBUG", ()-> DEBUG.getAsBoolean(), 0, 1); - var x = NAR_Shuffleboard.addData("DEBUG", "TOGGLE", false, 0, 0).withWidget("Toggle Button"); - DEBUG = ()-> x.getEntry().getBoolean(false); swerve = Swerve.getInstance(); - vision = Vision.getInstance(); - leds = Leds.getInstance(); - elevator = Elevator.getInstance(); - //TODO: Enable all PIDSubsystems so that useOutput runs here - // pivot.enable(); - // telescope.enable(); - - leftStick = new NAR_Joystick(0); rightStick = new NAR_Joystick(1); controller = new NAR_XboxController(2); buttonPad = new NAR_ButtonBoard(3); - operatorController = new NAR_XboxController(4); - - // commandScheduler.setDefaultCommand(swerve, new CmdSwerveDrive(rightStick::getX, rightStick::getY, rightStick::getZ, true)); - //uncomment line below to enable driving - commandScheduler.setDefaultCommand(swerve, new CmdSwerveDrive(controller::getLeftX,controller::getLeftY, controller::getRightX, true)); + CommandScheduler.getInstance().setDefaultCommand(swerve, new CmdSwerveDrive(controller::getLeftX,controller::getLeftY, controller::getRightX, true)); initDashboard(); configureButtonBindings(); @@ -88,14 +51,13 @@ public RobotContainer() { } private void configureButtonBindings() { - controller.getButton("A").onTrue(new InstantCommand(()-> Vision.AUTO_ENABLED = !Vision.AUTO_ENABLED)); 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("Start").onTrue(resetSwerve()); - controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true)).onFalse(retract(Position.NEUTRAL).andThen(stallPower()).andThen(setLeds(Colors.DEFAULT))); - controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true)).onFalse(retract(Position.NEUTRAL).andThen(stallPower()).andThen(setLeds(Colors.DEFAULT))); + controller.getButton("X").onTrue(xLock()).onFalse(stop()); + controller.getButton("B").onTrue(resetSwerve()); + controller.getButton("Start").onTrue(resetGyro()); + controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true)).onFalse(retract(Position.NEUTRAL).andThen(stallPower()).andThen(resetLeds())); + controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true)).onFalse(retract(Position.NEUTRAL).andThen(stallPower()).andThen(resetLeds())); controller.getUpPOVButton().onTrue(runOnce(()-> { CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 180 : 0; @@ -131,7 +93,8 @@ private void configureButtonBindings() { rightStick.getButton(11).onTrue(intake(false)); rightStick.getButton(12).onTrue(outtake()); rightStick.getButton(13).onTrue(stopManip()); - rightStick.getButton(14).onTrue(new CmdAutoBalance()); + rightStick.getButton(14).onTrue(new CmdAutoBalance(true)); + rightStick.getButton(15).onTrue(new CmdAutoBalance(false)); buttonPad.getButton(5).onTrue( @@ -144,7 +107,7 @@ private void configureButtonBindings() { score(Position.HIGH_CUBE, 1) ); - buttonPad.getButton(13).onTrue(runOnce(()-> SINGLE_STATION = true)); + buttonPad.getButton(13).onTrue(toggleLeds()); buttonPad.getButton(14).onTrue(retract(Position.NEUTRAL).beforeStarting(stopManip())); @@ -165,13 +128,9 @@ private void configureButtonBindings() { Vision.SELECTED_GRID = DriverStation.getAlliance() == Alliance.Red ? 2 : 0; })); - operatorController.getButton("Back").onTrue(new InstantCommand(()-> Vision.MANUAL = !Vision.MANUAL)); - } public void init() { - leds.setElevatorLeds(Colors.DEFAULT); - Vision.AUTO_ENABLED = false; if (DriverStation.getAlliance() == Alliance.Red) { buttonPad.getButton(4).onTrue( score(Position.LOW, 0) @@ -216,21 +175,11 @@ public void init() { } private void initDashboard() { - if (DEBUG.getAsBoolean()) { - SmartDashboard.putData("CommandScheduler", CommandScheduler.getInstance()); - // SmartDashboard.putData("Swerve", swerve); - } swerve.initShuffleboard(); - vision.initShuffleboard(); + Vision.getInstance().initShuffleboard(); NarwhalDashboard.startServer(); - - Log.info("NarwhalRobot", "Setting up limelight chooser..."); - - // for (NAR_Camera cam : vision.getCameras()) { - // NarwhalDashboard.addLimelight(cam); - // } } public void updateDashboard() { @@ -238,11 +187,9 @@ public void updateDashboard() { NarwhalDashboard.put("voltage", RobotController.getBatteryVoltage()); NarwhalDashboard.put("x", swerve.getPose().getX()); NarwhalDashboard.put("y", swerve.getPose().getY()); - SmartDashboard.putNumber("LeftX",controller.getLeftX()); - SmartDashboard.putNumber("LeftY",controller.getLeftY()); - SmartDashboard.putNumber("RightX",controller.getRightX()); - SmartDashboard.putNumber("RightY",controller.getRightY()); - NAR_Shuffleboard.update(); - SmartDashboard.putNumber("Pitch",swerve.getPitch()); + + if (DriverStation.getMatchType() == MatchType.None) { + NAR_Shuffleboard.update(); + } } } diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index 8c56c9e..c4e9767 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -1,17 +1,12 @@ package frc.team3128.autonomous; -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.commands.CmdAutoBalance; import frc.team3128.common.narwhaldashboard.NarwhalDashboard; -import frc.team3128.subsystems.Manipulator; -import frc.team3128.subsystems.Swerve; import static frc.team3128.commands.CmdManager.*; /** @@ -21,10 +16,7 @@ public class AutoPrograms { - public static Swerve swerve; - public AutoPrograms() { - swerve = Swerve.getInstance(); Trajectories.initTrajectories(); initAutoSelector(); @@ -34,59 +26,33 @@ private void initAutoSelector() { 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", + "cable_1Cone+1Cube","cable_1Cone+2Cube", //Mid - "r_mid_1Cone+Climb","r_mid_1Cone+1Cube+Climb", + "mid_1Cone+Climb","mid_1Cone+1Cube+Climb", //Hp - "r_hp_1Cone+1Cube"//"r_cable_1Cone+2Cube", + "hp_1Cone+1Cube", + "scuffedClimb" }; NarwhalDashboard.addAutos(autoStrings); } public Command getAutonomousCommand() { - String selectedAutoName = NarwhalDashboard.getSelectedAutoName(); - // String selectedAutoName = "b_cable_1Cone+1Cube"; - // String selectedAutoName = "b_cable_1Cone+2Cube+Climb"; //uncomment and change this for testing without opening Narwhal Dashboard - // SmartDashboard.putString(selectedAutoName, selectedAutoName); + String selectedAutoName = NarwhalDashboard.getSelectedAutoName(); + if (selectedAutoName == null) { - return score(Position.HIGH_CONE, true).beforeStarting(resetAuto()); + return score(Position.HIGH_CONE, true).beforeStarting(Trajectories.resetAuto()); } - return Trajectories.get(selectedAutoName,selectedAutoName.contains("Climb")).beforeStarting(resetAuto()); - } + if (selectedAutoName == "scuffedClimb") { + return sequence( + score(Position.HIGH_CONE, true).beforeStarting(Trajectories.resetAuto()), + new CmdAutoBalance(false) + ); + } - public CommandBase resetAuto() { - return sequence( - resetSwerve(DriverStation.getAlliance() == Alliance.Red ? 0 : 180), - runOnce(()-> Manipulator.getInstance().set(-0.4), Manipulator.getInstance()), - resetAll(), - retract(Position.NEUTRAL) - ); - } - - // /** - // * Follow trajectory and intake balls along the path - // */ - // private SequentialCommandGroup IntakePathCmd(String trajectory) { - // ParallelDeadlineGroup movement = new ParallelDeadlineGroup( - // trajectoryCmd(trajectory), - // new ScheduleCommand(new CmdExtendIntakeAndRun())); - // return new SequentialCommandGroup(new InstantCommand(intake::ejectIntake, intake), movement); - // } + selectedAutoName = (DriverStation.getAlliance() == Alliance.Red) ? "r_" : "b_" + selectedAutoName; - /** - * Flip 180 degrees rotation wise but keep same pose translation - */ - private Pose2d inverseRotation(Pose2d pose) { - return new Pose2d(pose.getTranslation(), new Rotation2d(pose.getRotation().getRadians() + Math.PI)); + return Trajectories.get(selectedAutoName); } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index dc695c7..b6d68d0 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -3,21 +3,21 @@ import java.util.HashMap; import java.util.List; -import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.PathPoint; import com.pathplanner.lib.auto.PIDConstants; import com.pathplanner.lib.auto.SwerveAutoBuilder; -import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; + import static edu.wpi.first.wpilibj2.command.Commands.*; +import static frc.team3128.Constants.AutoConstants.*; 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.*; @@ -31,18 +31,13 @@ */ public class Trajectories { - private static HashMap> trajectories = new HashMap>(); - - private static SwerveAutoBuilder builder; - - private static HashMap CommandEventMap = new HashMap(); - - private static Manipulator manipulator = Manipulator.getInstance(); + private static final HashMap> trajectories = new HashMap>(); - private static Swerve swerve = Swerve.getInstance(); + private static final HashMap CommandEventMap = new HashMap(); - public static double autoSpeed = SwerveConstants.maxSpeed; + private static final Swerve swerve = Swerve.getInstance(); + private static SwerveAutoBuilder builder; public static void initTrajectories() { final String[] trajectoryNames = { @@ -72,26 +67,28 @@ public static void initTrajectories() { CommandEventMap.put("PickupCube", pickup(Position.GROUND_CUBE, true)); CommandEventMap.put("Neutral", retract(Position.NEUTRAL)); + + CommandEventMap.put("Balance", new ScheduleCommand(new CmdAutoBalance(true))); + + for (final String trajectoryName : trajectoryNames) { - for (String trajectoryName : trajectoryNames) { - // Path path = Filesystem.getDeployDirectory().toPath().resolve("paths").resolve(trajectoryName + ".wpilib.json"); if (trajectoryName.contains("mid")) { - trajectories.put(trajectoryName, PathPlanner.loadPathGroup(trajectoryName, new PathConstraints(AutoConstants.slowSpeed, AutoConstants.slowAcceleration))); + trajectories.put(trajectoryName, PathPlanner.loadPathGroup(trajectoryName, slow)); } else { - trajectories.put(trajectoryName, PathPlanner.loadPathGroup(trajectoryName, new PathConstraints(maxSpeed, maxAcceleration))); + trajectories.put(trajectoryName, PathPlanner.loadPathGroup(trajectoryName, fast)); } } builder = new SwerveAutoBuilder( - Swerve.getInstance()::getPose, - Swerve.getInstance()::resetOdometry, + swerve::getPose, + swerve::resetOdometry, swerveKinematics, - new PIDConstants(translationKP,translationKI,translationKD), - new PIDConstants(rotationKP,rotationKI,rotationKD), - Swerve.getInstance()::setModuleStates, + new PIDConstants(translationKP, translationKI, translationKD), + new PIDConstants(rotationKP, rotationKI, rotationKD), + swerve::setModuleStates, CommandEventMap, - Swerve.getInstance() + swerve ); } @@ -99,26 +96,17 @@ 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 PathPoint(start.getTranslation(), start.getRotation()), - new PathPoint(end.getTranslation(), end.getRotation()) - ); + public static CommandBase get(String name) { + return builder.fullAuto(trajectories.get(name)).beforeStarting(resetAuto()); } - public static CommandBase lineCmd(Pose2d start, Pose2d end) { - return builder.fullAuto(line(start, end)); + public static CommandBase resetAuto() { + return sequence( + resetGyro(DriverStation.getAlliance() == Alliance.Red ? 0 : 180), + runOnce(()-> Manipulator.getInstance().set(-0.4), Manipulator.getInstance()), + resetAll(), + retract(Position.NEUTRAL) + ); } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/commands/CmdAutoBalance.java b/src/main/java/frc/team3128/commands/CmdAutoBalance.java index 9032f7c..1af2845 100644 --- a/src/main/java/frc/team3128/commands/CmdAutoBalance.java +++ b/src/main/java/frc/team3128/commands/CmdAutoBalance.java @@ -1,32 +1,25 @@ 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.Constants.SwerveConstants; 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 final int direction; private double prevAngle; private double angleVelocity; private int plateauCount; private boolean onRamp; - private boolean shoot; - public CmdAutoBalance() { - swerve = Swerve.getInstance(); - addRequirements(Swerve.getInstance()); - } - - public CmdAutoBalance(boolean shoot) { - this.shoot = shoot; + public CmdAutoBalance(boolean isForward) { swerve = Swerve.getInstance(); + this.direction = isForward ? 1 : -1; addRequirements(Swerve.getInstance()); } @@ -59,6 +52,7 @@ public void execute() { if (advAngle > RAMP_THRESHOLD) onRamp = true; if (Math.abs(advAngle) < ANGLE_THRESHOLD && onRamp) { + Manipulator.getInstance().outtake(); swerve.xlock(); return; } @@ -69,7 +63,7 @@ public void execute() { return; } - swerve.drive(new Translation2d(onRamp ? DRIVE_SPEED * (advAngle > 0.0 ? 1.0 : -1.0) : DRIVE_SPEED * 2, 0), 0, false); + swerve.drive(new Translation2d(onRamp ? DRIVE_SPEED * direction * (advAngle > 0.0 ? 1.0 : -1.0) : SwerveConstants.maxSpeed * direction, 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 a6c953b..e1b8568 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -1,10 +1,8 @@ package frc.team3128.commands; -import edu.wpi.first.wpilibj2.command.Command; 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; @@ -17,7 +15,6 @@ 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 { @@ -43,7 +40,6 @@ private static CommandBase score(Position position, int xPos, boolean runImmedia //waitUntil(()-> ENABLE), extend(position), waitUntil(()-> !ENABLE), - waitSeconds(DriverStation.isAutonomous() ? 0.5 : 0), runOnce(()-> Swerve.getInstance().throttle = 1), outtake(), waitSeconds(0.5), @@ -66,7 +62,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)), + setLeds(position.cone ? Colors.CONE : Colors.CUBE), runOnce(()-> ENABLE = runImmediately), waitUntil(()-> ENABLE), parallel( @@ -74,7 +70,7 @@ public static CommandBase pickup(Position position, boolean runImmediately) { intake(position.cone) ), retract(Position.NEUTRAL), - runOnce(()-> leds.setElevatorLeds(Colors.DEFAULT)) + resetLeds() ); } @@ -115,7 +111,7 @@ public static CommandBase intake(Boolean cone) { runOnce(()-> manipulator.intake(cone), manipulator), waitSeconds(0.2), waitUntil(()-> manipulator.hasObjectPresent()), - runOnce(()-> leds.setElevatorLeds(Colors.HOLDING)), + setLeds(Colors.HOLDING), waitSeconds(cone ? 0.15 : 0), runOnce(()-> manipulator.stallPower(), manipulator) ); @@ -159,6 +155,30 @@ public static CommandBase moveWri(double speed) { return runOnce(()-> wrist.set(speed), wrist); } + public static CommandBase xLock() { + return run(()-> swerve.xlock(), swerve); + } + + public static CommandBase stop() { + return runOnce(()-> swerve.stop(), swerve); + } + + public static CommandBase resetLeds() { + return runOnce(()-> leds.resetLeds()); + } + + public static CommandBase setLeds(Colors color) { + return runOnce(()-> leds.setElevatorLeds(color)); + } + + public static CommandBase toggleLeds() { + return sequence( + runOnce(()-> SINGLE_STATION = !SINGLE_STATION), + runOnce(()-> leds.defaultColor = (leds.defaultColor == Colors.CHUTE) ? Colors.SHELF : Colors.CHUTE), + resetLeds() + ); + } + public static CommandBase resetElevator() { return runOnce(()-> elevator.resetEncoder()); } @@ -167,16 +187,16 @@ public static CommandBase resetWrist() { return runOnce(()-> wrist.resetEncoder()); } - public static CommandBase resetSwerve(double angle) { + public static CommandBase resetGyro(double angle) { return runOnce(()-> swerve.zeroGyro(angle)); } - public static CommandBase resetSwerve() { + public static CommandBase resetGyro() { return runOnce(()-> swerve.zeroGyro()); } - public static CommandBase setLeds(Colors color) { - return runOnce(()-> leds.setElevatorLeds(color)); + public static CommandBase resetSwerve() { + return runOnce(()-> swerve.resetEncoders()); } public static CommandBase resetAll() { diff --git a/src/main/java/frc/team3128/commands/CmdTrajectory.java b/src/main/java/frc/team3128/commands/CmdTrajectory.java index 534740d..e782fed 100644 --- a/src/main/java/frc/team3128/commands/CmdTrajectory.java +++ b/src/main/java/frc/team3128/commands/CmdTrajectory.java @@ -76,7 +76,7 @@ private ArrayList generatePoses() { private CommandBase generateAuto() { index = Vision.SELECTED_GRID * 3 + xPos; - final PathPlannerTrajectory trajectory = PathPlanner.generatePath(pathConstraints, generatePoses()); + final PathPlannerTrajectory trajectory = PathPlanner.generatePath(fast, 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), diff --git a/src/main/java/frc/team3128/common/constantsint/ConstantsInt.java b/src/main/java/frc/team3128/common/constantsint/ConstantsInt.java index 63580c9..bde55cb 100644 --- a/src/main/java/frc/team3128/common/constantsint/ConstantsInt.java +++ b/src/main/java/frc/team3128/common/constantsint/ConstantsInt.java @@ -21,7 +21,6 @@ public class ConstantsInt extends Constants { //The Constants will be overridden and NarwhalDashboard will be able to change it //IMPORTANT: Omit "final" when temporarily adding constants to this class - public static class ConversionConstants extends Constants.ConversionConstants {} public static class SwerveConstants extends Constants.SwerveConstants {} public static class VisionConstants extends Constants.VisionConstants {} @@ -30,7 +29,6 @@ public static class VisionConstants extends Constants.VisionConstants {} categories = new HashMap>(); constants = new HashMap>(); primitiveNumbers = Arrays.asList(int.class, long.class, double.class, byte.class, short.class); - categories.put("ConversionConstants", ConstantsInt.ConversionConstants.class); categories.put("SwerveConstants", ConstantsInt.SwerveConstants.class); categories.put("VisionConstants", ConstantsInt.VisionConstants.class); diff --git a/src/main/java/frc/team3128/common/constantsint/ConstantsInt2.java b/src/main/java/frc/team3128/common/constantsint/ConstantsInt2.java index b3c6481..2d16cf2 100644 --- a/src/main/java/frc/team3128/common/constantsint/ConstantsInt2.java +++ b/src/main/java/frc/team3128/common/constantsint/ConstantsInt2.java @@ -33,7 +33,6 @@ public class ConstantsInt2 { categories = new Hashtable>(); //Add each class to the HashMap - categories.put("ConversionConstants", Constants.ConversionConstants.class); categories.put("SwerveConstants", Constants.SwerveConstants.class); categories.put("VisionConstants", Constants.VisionConstants.class); diff --git a/src/main/java/frc/team3128/common/swerve/SwerveModule.java b/src/main/java/frc/team3128/common/swerve/SwerveModule.java index 4aac636..c449662 100644 --- a/src/main/java/frc/team3128/common/swerve/SwerveModule.java +++ b/src/main/java/frc/team3128/common/swerve/SwerveModule.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.team3128.common.hardware.motorcontroller.NAR_TalonFX; -import frc.team3128.subsystems.Swerve; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; diff --git a/src/main/java/frc/team3128/common/swerveNeo/SwerveConversions.java b/src/main/java/frc/team3128/common/swerveNeo/SwerveConversions.java index f90ce80..7567e06 100644 --- a/src/main/java/frc/team3128/common/swerveNeo/SwerveConversions.java +++ b/src/main/java/frc/team3128/common/swerveNeo/SwerveConversions.java @@ -77,9 +77,10 @@ public static double MPSToRPM(double velocity, double circumference, double gear * @param gearRatio Gear Ratio between motor and mechanism * @return Meters traveled */ - public static double falconToMeters(double rotations, double circumference, double gearRatio){ + public static double rotationsToMeters(double rotations, double circumference, double gearRatio){ double mechRotations = rotations / gearRatio; - return mechRotations * circumference; + double meters = mechRotations * circumference; + return meters; } /** @@ -88,7 +89,7 @@ public static double falconToMeters(double rotations, double circumference, doub * @param gearRatio Gear Ratio between motor and Mechanism * @return rotations of motor */ - public static double metersToFalcon(double meters, double circumference, double gearRatio){ + public static double metersToRotations(double meters, double circumference, double gearRatio){ double mechRotations = meters / circumference; double rotations = mechRotations * gearRatio; return rotations; diff --git a/src/main/java/frc/team3128/common/swerveNeo/SwerveModule.java b/src/main/java/frc/team3128/common/swerveNeo/SwerveModule.java index 914b4c7..4443f38 100644 --- a/src/main/java/frc/team3128/common/swerveNeo/SwerveModule.java +++ b/src/main/java/frc/team3128/common/swerveNeo/SwerveModule.java @@ -158,7 +158,7 @@ public SwerveModuleState getState(){ * @return A swerve module position */ public SwerveModulePosition getPosition() { - double position = falconToMeters(driveMotor.getSelectedSensorPosition(), wheelCircumference, driveGearRatio); + double position = rotationsToMeters(driveMotor.getSelectedSensorPosition(), wheelCircumference, driveGearRatio); Rotation2d angle = getAngle(); return new SwerveModulePosition(position, angle); } diff --git a/src/main/java/frc/team3128/subsystems/Elevator.java b/src/main/java/frc/team3128/subsystems/Elevator.java index a05cb42..0796da9 100644 --- a/src/main/java/frc/team3128/subsystems/Elevator.java +++ b/src/main/java/frc/team3128/subsystems/Elevator.java @@ -75,7 +75,4 @@ public double getMeasurement() { return m_elv2.getSelectedSensorPosition() / GEAR_RATIO * SPOOL_CIRCUMFERENCE; } - public boolean pastFramePerimiter() { - return getMeasurement() >= FRAME_LENGTH; - } } diff --git a/src/main/java/frc/team3128/subsystems/Leds.java b/src/main/java/frc/team3128/subsystems/Leds.java index f30d2e3..0b4c8c3 100644 --- a/src/main/java/frc/team3128/subsystems/Leds.java +++ b/src/main/java/frc/team3128/subsystems/Leds.java @@ -25,6 +25,8 @@ public class Leds extends SubsystemBase { private final CANdle m_candle = new CANdle(LedConstants.CANDLE_ID); + public Colors defaultColor = Colors.CHUTE; + private static Leds instance; public static Leds getInstance() { @@ -36,6 +38,8 @@ public static Leds getInstance() { public Leds() { configCandle(); + + resetLeds(); } private void configCandle() { @@ -45,6 +49,10 @@ private void configCandle() { m_candle.configAllSettings(config); } + public void resetLeds() { + setElevatorLeds(defaultColor); + } + //Set Elevator Leds public void setElevatorLeds(Colors color) { @@ -64,13 +72,13 @@ public void setElevatorLeds(Colors color) { } } - public void resetAnimationSlot(int slots) { + private void resetAnimationSlot(int slots) { for (int i = 0; i < slots; i++) { m_candle.animate(null,i); } } - public void resetAnimationSlot(int slots, int offset) { + private void resetAnimationSlot(int slots, int offset) { for (int i = 0; i < slots; i++) { m_candle.animate(null,i+offset); } diff --git a/src/main/java/frc/team3128/subsystems/Manipulator.java b/src/main/java/frc/team3128/subsystems/Manipulator.java index a57d7cd..b2ef096 100644 --- a/src/main/java/frc/team3128/subsystems/Manipulator.java +++ b/src/main/java/frc/team3128/subsystems/Manipulator.java @@ -14,8 +14,6 @@ public class Manipulator extends SubsystemBase { public static boolean isCone = true; - public boolean isOuttaking = false; - public static synchronized Manipulator getInstance() { if (instance == null){ instance = new Manipulator(); @@ -35,14 +33,12 @@ private void configMotor(){ } public void intake(boolean cone) { - isOuttaking = false; isCone = cone; if (isCone) reverse(); else forward(); } public void outtake(){ - isOuttaking = true; if (!isCone) reverse(); else forward(); } @@ -51,20 +47,20 @@ public void stallPower() { set(isCone ? -STALL_POWER_CONE : STALL_POWER_CUBE); } - public void set(double power){ - m_roller.set(power); + private void forward(){ + set(ROLLER_POWER); } - public void forward(){ - m_roller.set(ROLLER_POWER); + private void reverse(){ + set(-ROLLER_POWER); } - public void reverse(){ - m_roller.set(-ROLLER_POWER); + public void stopRoller(){ + set(0); } - public void stopRoller(){ - m_roller.set(0); + public void set(double power){ + m_roller.set(power); } public boolean hasObjectPresent(){ diff --git a/src/main/java/frc/team3128/subsystems/Swerve.java b/src/main/java/frc/team3128/subsystems/Swerve.java index e3fd64b..fba952f 100644 --- a/src/main/java/frc/team3128/subsystems/Swerve.java +++ b/src/main/java/frc/team3128/subsystems/Swerve.java @@ -100,7 +100,6 @@ public void initShuffleboard() { NAR_Shuffleboard.addComplex("General","Gyro",gyro,7,2,2,2);//.withWidget("Gyro"); NAR_Shuffleboard.addData("General","Heading",this::getHeading,1,2); // // Drivetrain Tab - NAR_Shuffleboard.addComplex("Field","field",field,0,0,13,7);//.withWidget("Field"); NAR_Shuffleboard.addData("Drivetrain","Pose",() -> (getPose().toString()),2,0,4,1); NAR_Shuffleboard.addComplex("Drivetrain","Gyro",gyro,3,1,2,2);//.withWidget("Gyro"); NAR_Shuffleboard.addData("Drivetrain","Yaw",this::getYaw,4,1); @@ -119,7 +118,7 @@ public Pose2d getPose() { } public void addVisionMeasurement(Pose2d pose, double timeStamp) { - if (Math.abs(pose.getX() - getPose().getX()) > VisionConstants.POSE_THRESH && + if (Math.abs(pose.getX() - getPose().getX()) > VisionConstants.POSE_THRESH || Math.abs(pose.getY() - getPose().getY()) > VisionConstants.POSE_THRESH) return; odometry.addVisionMeasurement(pose, timeStamp); } @@ -130,9 +129,8 @@ public void resetEncoders() { } } - public void resetOdometry(Pose2d pose) { // TODO: Call this!!!! + public void resetOdometry(Pose2d pose) { zeroGyro(pose.getRotation().getDegrees()); - //System.out.println(pose.toString()); odometry.resetPosition(getGyroRotation2d(), getPositions(), pose); } diff --git a/src/main/java/frc/team3128/subsystems/Vision.java b/src/main/java/frc/team3128/subsystems/Vision.java index 8934b7b..3186788 100644 --- a/src/main/java/frc/team3128/subsystems/Vision.java +++ b/src/main/java/frc/team3128/subsystems/Vision.java @@ -12,10 +12,8 @@ import frc.team3128.common.hardware.camera.NAR_Camera; import frc.team3128.common.utility.NAR_Shuffleboard; -public class Vision extends SubsystemBase{ +public class Vision extends SubsystemBase { public static int SELECTED_GRID = 0; - public static boolean AUTO_ENABLED = true; - public static boolean MANUAL = false; private HashMap cameras; @@ -143,9 +141,6 @@ public void initShuffleboard() { NAR_Shuffleboard.addData("Vision2", "Processed Target",()->cam2.getProcessedTarget().toString(),0,2,4,1); NAR_Shuffleboard.addData("Vision2","EstimatedPose", ()-> cam2.getPos().toString(),0,3,4,1); NAR_Shuffleboard.addData("Test", "TESTING", ()->cam2.getTest().toString(),0,2,3,1); - - NAR_Shuffleboard.addData("Togglables", "AUTO_ENABLED", ()-> AUTO_ENABLED, 0, 0); - NAR_Shuffleboard.addData("Togglables", "MANUAL", ()-> MANUAL, 1, 0); } public void logCameraAll() { From b70f4d662a0423bb018edbb2e769db0e1e8a725a Mon Sep 17 00:00:00 2001 From: "Team 3128: Controls" Date: Tue, 24 Oct 2023 20:14:53 -0700 Subject: [PATCH 06/12] Workshop 10/24 --- .../java/frc/team3128/PositionConstants.java | 2 +- .../java/frc/team3128/RobotContainer.java | 3 ++- .../frc/team3128/commands/CmdSwerveDrive.java | 23 ++++++++++++++++++- 3 files changed, 25 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team3128/PositionConstants.java b/src/main/java/frc/team3128/PositionConstants.java index 25f62db..62faf18 100644 --- a/src/main/java/frc/team3128/PositionConstants.java +++ b/src/main/java/frc/team3128/PositionConstants.java @@ -8,7 +8,7 @@ public static enum Position { MID_CUBE(25, 30, false), LOW(3, 45, true), - SHELF_CONE(53, -30, true), + SHELF_CONE(26, 45, true), SHELF_CUBE(37, 22, false), CHUTE_CONE(10, 55, true), CHUTE_CUBE(10, 45, false), diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index 06c9597..1179974 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -58,6 +58,7 @@ private void configureButtonBindings() { controller.getButton("Start").onTrue(resetGyro()); controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true)).onFalse(retract(Position.NEUTRAL).andThen(stallPower()).andThen(resetLeds())); controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true)).onFalse(retract(Position.NEUTRAL).andThen(stallPower()).andThen(resetLeds())); + controller.getButton("RightStick").onTrue(runOnce(()-> CmdSwerveDrive.setTurnSetpoint())); controller.getUpPOVButton().onTrue(runOnce(()-> { CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 180 : 0; @@ -78,7 +79,7 @@ private void configureButtonBindings() { CmdSwerveDrive.enabled = true; })); - rightStick.getButton(1).onTrue(resetSwerve()); + rightStick.getButton(1).onTrue(resetGyro()); rightStick.getButton(2).onTrue(moveElv(0.4)).onFalse(moveElv(0)); rightStick.getButton(3).onTrue(moveElv(-0.4)).onFalse(moveElv(0)); rightStick.getButton(4).onTrue(moveElevator(30)); diff --git a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java index 5a3fc96..411b5a9 100644 --- a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java +++ b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java @@ -2,6 +2,7 @@ import java.util.function.DoubleSupplier; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; @@ -61,7 +62,7 @@ public void execute() { rotation = -zAxis.getAsDouble() * maxAngularVelocity * swerve.throttle; - if (Math.abs(rotation) > 0.25) { + if (Math.abs(rotation) > maxAngularVelocity * swerve.throttle / 2.0) { enabled = false; } if (enabled) { @@ -82,6 +83,26 @@ public void execute() { } + public static void setTurnSetpoint() { + final double currentRotation = MathUtil.inputModulus(Swerve.getInstance().getYaw(), 0, 360); + if (currentRotation <= 45) { + rSetpoint = 0; + } + else if (currentRotation <= 135) { + rSetpoint = 90; + } + else if (currentRotation <= 225) { + rSetpoint = 180; + } + else if (currentRotation <= 315) { + rSetpoint = 270; + } + else { + rSetpoint = 360; + } + enabled = true; + } + @Override public void end(boolean interrupted) { swerve.stop(); From 83dc96cdef2357f7c78112eda83734337ceae790 Mon Sep 17 00:00:00 2001 From: Mason-Lam <97353903+Mason-Lam@users.noreply.github.com> Date: Thu, 26 Oct 2023 01:14:43 -0700 Subject: [PATCH 07/12] Led Bugfix + Cleanup --- src/main/java/frc/team3128/Constants.java | 2 +- src/main/java/frc/team3128/Robot.java | 26 ++++++++----------- .../frc/team3128/autonomous/AutoPrograms.java | 16 +++++++----- .../frc/team3128/autonomous/Trajectories.java | 4 +++ .../frc/team3128/commands/CmdAutoBalance.java | 4 ++- 5 files changed, 29 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 2bca0dd..d4d0439 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -363,7 +363,7 @@ public static class ManipulatorConstants{ public static final double STALL_POWER_CUBE = 0.1; - public static final double CURRENT_THRESHOLD_CONE = 30; + public static final double CURRENT_THRESHOLD_CONE = 35; public static final double CURRENT_THRESHOLD_CUBE = 25; } diff --git a/src/main/java/frc/team3128/Robot.java b/src/main/java/frc/team3128/Robot.java index 46ff8c4..57aadac 100644 --- a/src/main/java/frc/team3128/Robot.java +++ b/src/main/java/frc/team3128/Robot.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import frc.team3128.Constants.LedConstants.Colors; import frc.team3128.autonomous.AutoPrograms; +import frc.team3128.commands.CmdManager; import frc.team3128.subsystems.Leds; import frc.team3128.subsystems.Swerve; @@ -23,10 +24,8 @@ public class Robot extends TimedRobot { public static Robot instance; public static RobotContainer m_robotContainer = new RobotContainer(); - private Command m_autonomousCommand; public static AutoPrograms autoPrograms = new AutoPrograms(); - public Timer timer; - public Timer xlockTimer; + public Timer xLockTimer = new Timer(); public double startTime; public static synchronized Robot getInstance() { @@ -38,6 +37,7 @@ public static synchronized Robot getInstance() { @Override public void robotInit(){ + m_robotContainer.init(); LiveWindow.disableAllTelemetry(); } @@ -48,38 +48,34 @@ public void robotPeriodic(){ @Override public void autonomousInit() { - m_robotContainer.init(); - timer = new Timer(); - m_autonomousCommand = autoPrograms.getAutonomousCommand(); - Leds.getInstance().defaultColor = Colors.AUTO; + xLockTimer.restart(); + Command m_autonomousCommand = autoPrograms.getAutonomousCommand(); if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); - timer.start(); } } @Override public void autonomousPeriodic() { CommandScheduler.getInstance().run(); - if (timer.hasElapsed(14.75)) { + if (xLockTimer.hasElapsed(14.75)) { new RunCommand(()->Swerve.getInstance().xlock(), Swerve.getInstance()).schedule(); } } @Override public void teleopInit() { - Leds.getInstance().defaultColor = Colors.CHUTE; - m_robotContainer.init(); - xlockTimer = new Timer(); - xlockTimer.start(); + Leds.getInstance().defaultColor = CmdManager.SINGLE_STATION ? Colors.CHUTE : Colors.SHELF; + Leds.getInstance().resetLeds(); + xLockTimer.restart(); CommandScheduler.getInstance().cancelAll(); } @Override public void teleopPeriodic() { CommandScheduler.getInstance().run(); - if (xlockTimer.hasElapsed(134.75)) { - new RunCommand(()-> Swerve.getInstance().xlock(), Swerve.getInstance()).schedule(); + if (xLockTimer.hasElapsed(134.75)) { + // new RunCommand(()-> Swerve.getInstance().xlock(), Swerve.getInstance()).schedule(); } } diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index c4e9767..b2946e8 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -23,7 +23,7 @@ public AutoPrograms() { } private void initAutoSelector() { - String[] autoStrings = new String[] { + final String[] autoStrings = new String[] { //Blue Autos //Cable "cable_1Cone+1Cube","cable_1Cone+2Cube", @@ -39,20 +39,24 @@ private void initAutoSelector() { public Command getAutonomousCommand() { String selectedAutoName = NarwhalDashboard.getSelectedAutoName(); + final Command autoCommand; if (selectedAutoName == null) { - return score(Position.HIGH_CONE, true).beforeStarting(Trajectories.resetAuto()); + autoCommand = score(Position.HIGH_CONE, true).beforeStarting(Trajectories.resetAuto()); } - if (selectedAutoName == "scuffedClimb") { - return sequence( + else if (selectedAutoName == "scuffedClimb") { + autoCommand = sequence( score(Position.HIGH_CONE, true).beforeStarting(Trajectories.resetAuto()), new CmdAutoBalance(false) ); } - selectedAutoName = (DriverStation.getAlliance() == Alliance.Red) ? "r_" : "b_" + selectedAutoName; + else { + selectedAutoName = (DriverStation.getAlliance() == Alliance.Red) ? "r_" : "b_" + selectedAutoName; + autoCommand = Trajectories.get(selectedAutoName); + } - return Trajectories.get(selectedAutoName); + return autoCommand.beforeStarting(Trajectories.resetAuto()); } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index b6d68d0..184ad27 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -18,10 +18,12 @@ import static frc.team3128.Constants.AutoConstants.*; import static frc.team3128.Constants.SwerveConstants.*; +import frc.team3128.Constants.LedConstants.Colors; import frc.team3128.PositionConstants.Position; import static frc.team3128.commands.CmdManager.*; import frc.team3128.commands.CmdAutoBalance; +import frc.team3128.subsystems.Leds; import frc.team3128.subsystems.Manipulator; import frc.team3128.subsystems.Swerve; @@ -102,6 +104,8 @@ public static CommandBase get(String name) { public static CommandBase resetAuto() { return sequence( + runOnce(()-> Leds.getInstance().defaultColor = Colors.AUTO), + resetLeds(), resetGyro(DriverStation.getAlliance() == Alliance.Red ? 0 : 180), runOnce(()-> Manipulator.getInstance().set(-0.4), Manipulator.getInstance()), resetAll(), diff --git a/src/main/java/frc/team3128/commands/CmdAutoBalance.java b/src/main/java/frc/team3128/commands/CmdAutoBalance.java index 1af2845..db4f2a9 100644 --- a/src/main/java/frc/team3128/commands/CmdAutoBalance.java +++ b/src/main/java/frc/team3128/commands/CmdAutoBalance.java @@ -52,8 +52,10 @@ public void execute() { if (advAngle > RAMP_THRESHOLD) onRamp = true; if (Math.abs(advAngle) < ANGLE_THRESHOLD && onRamp) { - Manipulator.getInstance().outtake(); swerve.xlock(); + if (direction == 1) { + Manipulator.getInstance().set(-1); + } return; } From a073d55d0fdac946ab6f6e4fa84f3738643d8ae1 Mon Sep 17 00:00:00 2001 From: leo-les Date: Fri, 27 Oct 2023 15:24:56 -0700 Subject: [PATCH 08/12] New BB Paths --- .../b_cable_1Cone+1.5Cube+Climb_V2.path | 306 ++++++++++++++++ .../pathplanner/b_cable_1Cone+1Cube_V2.path | 219 ++++++++++++ .../pathplanner/b_cable_1Cone+2Cube_V2.path | 331 ++++++++++++++++++ 3 files changed, 856 insertions(+) create mode 100644 src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb_V2.path create mode 100644 src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path create mode 100644 src/main/deploy/pathplanner/b_cable_1Cone+2Cube_V2.path diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb_V2.path b/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb_V2.path new file mode 100644 index 0000000..1d74621 --- /dev/null +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb_V2.path @@ -0,0 +1,306 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.85, + "y": 0.5 + }, + "prevControl": null, + "nextControl": { + "x": 2.614284574055259, + "y": 0.5526832772415535 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.7181434414952164, + "y": 0.6872733398244574 + }, + "prevControl": { + "x": 3.0950688860698854, + "y": 0.6872733398244574 + }, + "nextControl": { + "x": 4.109778196633054, + "y": 0.6872733398244574 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.303302403479807, + "y": 0.7360412168741004 + }, + "prevControl": { + "x": 4.818317779170107, + "y": 0.7360412168741004 + }, + "nextControl": { + "x": 5.828125506993927, + "y": 0.7360412168741004 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.62403625328084, + "y": 0.8761981239240252 + }, + "prevControl": { + "x": 6.158218944910113, + "y": 0.7522198586063836 + }, + "nextControl": { + "x": 6.158218944910113, + "y": 0.7522198586063836 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.316233719272994, + "y": 0.7524518104591758 + }, + "prevControl": { + "x": 5.755477834214798, + "y": 0.7524518104591756 + }, + "nextControl": { + "x": 4.8769896043311896, + "y": 0.7524518104591759 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.717041670194452, + "y": 0.7084969343549796 + }, + "prevControl": { + "x": 4.248075648196162, + "y": 0.7084969343549796 + }, + "nextControl": { + "x": 3.1253090980516522, + "y": 0.7084969343549796 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.834230493113764, + "y": 1.1 + }, + "prevControl": { + "x": 3.328467547907319, + "y": 0.7980883374961188 + }, + "nextControl": { + "x": 3.328467547907319, + "y": 0.7980883374961188 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "ScoreCubeHigh" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.7216807072503038, + "y": 0.7002046556176442 + }, + "prevControl": { + "x": 3.3272218924947463, + "y": 0.7002046556176442 + }, + "nextControl": { + "x": 4.053104693898115, + "y": 0.7002046556176442 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.3096230914684055, + "y": 0.7431737363474717 + }, + "prevControl": { + "x": 4.6687819465724045, + "y": 0.7431737363474717 + }, + "nextControl": { + "x": 5.833241223087905, + "y": 0.7431737363474717 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.783909067818136, + "y": 1.8520195686224699 + }, + "prevControl": { + "x": 6.534499982047686, + "y": 1.5165358902321173 + }, + "nextControl": { + "x": 7.279416213346316, + "y": 2.5185332176219903 + }, + "holonomicAngle": 45.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.503592828366212, + "y": 2.740801080560494 + }, + "prevControl": { + "x": 6.103620177807911, + "y": 2.740801080560494 + }, + "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.9992897727272716, + "names": [ + "PickupCube" + ] + }, + { + "position": 3.898082386363637, + "names": [ + "Neutral" + ] + }, + { + "position": 8.232244318181817, + "names": [ + "PickupCube" + ] + }, + { + "position": 9.715056818181818, + "names": [ + "Neutral" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path new file mode 100644 index 0000000..bc5cfc4 --- /dev/null +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path @@ -0,0 +1,219 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.85, + "y": 0.5 + }, + "prevControl": null, + "nextControl": { + "x": 2.614284574055259, + "y": 0.5526832772415535 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.7181434414952164, + "y": 0.6872733398244574 + }, + "prevControl": { + "x": 3.0950688860698854, + "y": 0.6872733398244574 + }, + "nextControl": { + "x": 4.109778196633054, + "y": 0.6872733398244574 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.303302403479807, + "y": 0.7360412168741004 + }, + "prevControl": { + "x": 4.818317779170107, + "y": 0.7360412168741004 + }, + "nextControl": { + "x": 5.828125506993927, + "y": 0.7360412168741004 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.694433640603393, + "y": 0.8154847264555638 + }, + "prevControl": { + "x": 6.102898428019079, + "y": 0.7729795494313204 + }, + "nextControl": { + "x": 6.102898428019079, + "y": 0.7729795494313204 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.316233719272994, + "y": 0.7524518104591758 + }, + "prevControl": { + "x": 5.755477834214798, + "y": 0.7524518104591756 + }, + "nextControl": { + "x": 4.8769896043311896, + "y": 0.7524518104591759 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.717041670194452, + "y": 0.7084969343549796 + }, + "prevControl": { + "x": 4.248075648196162, + "y": 0.7084969343549796 + }, + "nextControl": { + "x": 3.1253090980516522, + "y": 0.7084969343549796 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.834230493113764, + "y": 1.1 + }, + "prevControl": { + "x": 3.328467547907319, + "y": 0.7980883374961188 + }, + "nextControl": { + "x": 3.328467547907319, + "y": 0.7980883374961188 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "ScoreCubeHigh" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.080923415319053, + "y": 0.9977988827505433 + }, + "prevControl": { + "x": 3.665490370911298, + "y": 0.32913967911269143 + }, + "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": 2.015234375, + "names": [ + "PickupCube" + ] + }, + { + "position": 3.921875000000001, + "names": [ + "Neutral" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+2Cube_V2.path b/src/main/deploy/pathplanner/b_cable_1Cone+2Cube_V2.path new file mode 100644 index 0000000..5387101 --- /dev/null +++ b/src/main/deploy/pathplanner/b_cable_1Cone+2Cube_V2.path @@ -0,0 +1,331 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.85, + "y": 0.5 + }, + "prevControl": null, + "nextControl": { + "x": 2.614284574055259, + "y": 0.5526832772415535 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.7181434414952164, + "y": 0.6872733398244574 + }, + "prevControl": { + "x": 3.0950688860698854, + "y": 0.6872733398244574 + }, + "nextControl": { + "x": 4.109778196633054, + "y": 0.6872733398244574 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.303302403479807, + "y": 0.7360412168741004 + }, + "prevControl": { + "x": 4.818317779170107, + "y": 0.7360412168741004 + }, + "nextControl": { + "x": 5.828125506993927, + "y": 0.7360412168741004 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.778806127056699, + "y": 1.88083958633195 + }, + "prevControl": { + "x": 6.096345788177688, + "y": 1.1524527806000053 + }, + "nextControl": { + "x": 6.096345788177688, + "y": 1.1524527806000053 + }, + "holonomicAngle": 45.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.316233719272994, + "y": 0.7524518104591758 + }, + "prevControl": { + "x": 5.755477834214798, + "y": 0.7524518104591756 + }, + "nextControl": { + "x": 4.8769896043311896, + "y": 0.7524518104591759 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.717041670194452, + "y": 0.7084969343549796 + }, + "prevControl": { + "x": 4.248075648196162, + "y": 0.7084969343549796 + }, + "nextControl": { + "x": 3.1253090980516522, + "y": 0.7084969343549796 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.834230493113764, + "y": 1.1 + }, + "prevControl": { + "x": 3.328467547907319, + "y": 0.7980883374961188 + }, + "nextControl": { + "x": 3.328467547907319, + "y": 0.7980883374961188 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "ScoreCubeHigh" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.7216807072503038, + "y": 0.7002046556176442 + }, + "prevControl": { + "x": 3.3272218924947463, + "y": 0.7002046556176442 + }, + "nextControl": { + "x": 4.053104693898115, + "y": 0.7002046556176442 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.3096230914684055, + "y": 0.7431737363474717 + }, + "prevControl": { + "x": 4.6687819465724045, + "y": 0.7431737363474717 + }, + "nextControl": { + "x": 5.833241223087905, + "y": 0.7431737363474717 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.654653897849463, + "y": 0.9274014954279904 + }, + "prevControl": { + "x": 6.238880201718738, + "y": 0.8839685109925763 + }, + "nextControl": { + "x": 6.238880201718738, + "y": 0.8839685109925763 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.317799394279344, + "y": 0.745377278949002 + }, + "prevControl": { + "x": 5.663402097601588, + "y": 0.745377278949002 + }, + "nextControl": { + "x": 4.78357667766198, + "y": 0.745377278949002 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.7278274313493074, + "y": 0.6984650167216994 + }, + "prevControl": { + "x": 4.327854780791005, + "y": 0.6984650167216994 + }, + "nextControl": null, + "holonomicAngle": 90.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": 2.015234375, + "names": [ + "PickupCube" + ] + }, + { + "position": 3.921875000000001, + "names": [ + "Neutral" + ] + }, + { + "position": 7.850781250000001, + "names": [ + "PickupCube" + ] + }, + { + "position": 9.715056818181818, + "names": [ + "Neutral" + ] + } + ] +} \ No newline at end of file From 6f537582486d834c8d1900927eb53209c472d755 Mon Sep 17 00:00:00 2001 From: Mason-Lam <97353903+Mason-Lam@users.noreply.github.com> Date: Sat, 28 Oct 2023 00:22:26 -0700 Subject: [PATCH 09/12] Code Call 10/27 --- ....path => b_cable_1Cone+1.5Cube+Climb.path} | 4 +- .../pathplanner/b_cable_1Cone+1Cube.path | 126 +++++-- .../pathplanner/b_cable_1Cone+1Cube_V2.path | 219 ------------ .../pathplanner/b_cable_1Cone+2Cube.path | 201 +++++++---- .../pathplanner/b_cable_1Cone+2Cube_V2.path | 331 ------------------ .../b_mid_1Cone+0.5Cube+Climb.path | 144 ++++++++ .../pathplanner/b_mid_1Cone+1Cube+Climb.path | 46 ++- ...mb.path => r_mid_1Cone+0.5Cube+Climb.path} | 0 src/main/java/frc/team3128/Constants.java | 4 +- .../frc/team3128/autonomous/AutoPrograms.java | 8 +- .../frc/team3128/autonomous/Trajectories.java | 4 +- 11 files changed, 417 insertions(+), 670 deletions(-) rename src/main/deploy/pathplanner/{b_cable_1Cone+1.5Cube+Climb_V2.path => b_cable_1Cone+1.5Cube+Climb.path} (99%) delete mode 100644 src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path delete mode 100644 src/main/deploy/pathplanner/b_cable_1Cone+2Cube_V2.path create mode 100644 src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path rename src/main/deploy/pathplanner/{r_mid_1Cone+1Cube+Climb.path => r_mid_1Cone+0.5Cube+Climb.path} (100%) diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb_V2.path b/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path similarity index 99% rename from src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb_V2.path rename to src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path index 1d74621..4bbb930 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb_V2.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path @@ -274,8 +274,8 @@ } } ], - "maxVelocity": 4.5, - "maxAcceleration": 2.3, + "maxVelocity": 4.4, + "maxAcceleration": 3.0, "isReversed": null, "markers": [ { diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path index c0ce477..bc5cfc4 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path @@ -7,8 +7,8 @@ }, "prevControl": null, "nextControl": { - "x": 3.8274507521237267, - "y": 0.8020895069567917 + "x": 2.614284574055259, + "y": 0.5526832772415535 }, "holonomicAngle": 180.0, "isReversal": false, @@ -26,18 +26,18 @@ }, { "anchorPoint": { - "x": 4.765519009711006, - "y": 0.7788305624004619 + "x": 3.7181434414952164, + "y": 0.6872733398244574 }, "prevControl": { - "x": 3.794480805377604, - "y": 0.7788305624004619 + "x": 3.0950688860698854, + "y": 0.6872733398244574 }, "nextControl": { - "x": 5.290342113225122, - "y": 0.7788305624004619 + "x": 4.109778196633054, + "y": 0.6872733398244574 }, - "holonomicAngle": 180.0, + "holonomicAngle": 90.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -51,18 +51,18 @@ }, { "anchorPoint": { - "x": 5.753304113243297, - "y": 0.8738098992785663 + "x": 5.303302403479807, + "y": 0.7360412168741004 }, "prevControl": { - "x": 5.315913049286986, - "y": 0.8563142567203109 + "x": 4.818317779170107, + "y": 0.7360412168741004 }, "nextControl": { - "x": 5.99075245543856, - "y": 0.8833078329663785 + "x": 5.828125506993927, + "y": 0.7360412168741004 }, - "holonomicAngle": 0, + "holonomicAngle": 90.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -76,18 +76,18 @@ }, { "anchorPoint": { - "x": 6.74108921677559, - "y": 0.9118016340298093 + "x": 6.694433640603393, + "y": 0.8154847264555638 }, "prevControl": { - "x": 6.256694598697254, - "y": 0.9497933687810524 + "x": 6.102898428019079, + "y": 0.7729795494313204 }, "nextControl": { - "x": 6.256694598697254, - "y": 0.9497933687810524 + "x": 6.102898428019079, + "y": 0.7729795494313204 }, - "holonomicAngle": 0, + "holonomicAngle": 0.0, "isReversal": true, "velOverride": null, "isLocked": false, @@ -101,18 +101,18 @@ }, { "anchorPoint": { - "x": 2.7583658020383086, - "y": 0.7089983937457166 + "x": 5.316233719272994, + "y": 0.7524518104591758 }, "prevControl": { - "x": 3.197609916980112, - "y": 0.7089983937457166 + "x": 5.755477834214798, + "y": 0.7524518104591756 }, "nextControl": { - "x": 2.3191216870965046, - "y": 0.7089983937457166 + "x": 4.8769896043311896, + "y": 0.7524518104591759 }, - "holonomicAngle": 0.0, + "holonomicAngle": 90.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -126,19 +126,47 @@ }, { "anchorPoint": { - "x": 1.85, - "y": 1.1 + "x": 3.717041670194452, + "y": 0.7084969343549796 }, "prevControl": { - "x": 2.03793195032895, - "y": 0.9605784689458102 + "x": 4.248075648196162, + "y": 0.7084969343549796 }, - "nextControl": null, - "holonomicAngle": 180.0, + "nextControl": { + "x": 3.1253090980516522, + "y": 0.7084969343549796 + }, + "holonomicAngle": 90.0, "isReversal": false, "velOverride": null, "isLocked": false, "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.834230493113764, + "y": 1.1 + }, + "prevControl": { + "x": 3.328467547907319, + "y": 0.7980883374961188 + }, + "nextControl": { + "x": 3.328467547907319, + "y": 0.7980883374961188 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, "stopEvent": { "names": [ "ScoreCubeHigh" @@ -147,6 +175,28 @@ "waitBehavior": "none", "waitTime": 0 } + }, + { + "anchorPoint": { + "x": 7.080923415319053, + "y": 0.9977988827505433 + }, + "prevControl": { + "x": 3.665490370911298, + "y": 0.32913967911269143 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } } ], "maxVelocity": 4.5, @@ -154,13 +204,13 @@ "isReversed": null, "markers": [ { - "position": 1.1, + "position": 2.015234375, "names": [ "PickupCube" ] }, { - "position": 2.997869318181817, + "position": 3.921875000000001, "names": [ "Neutral" ] diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path deleted file mode 100644 index bc5cfc4..0000000 --- a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path +++ /dev/null @@ -1,219 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.85, - "y": 0.5 - }, - "prevControl": null, - "nextControl": { - "x": 2.614284574055259, - "y": 0.5526832772415535 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.7181434414952164, - "y": 0.6872733398244574 - }, - "prevControl": { - "x": 3.0950688860698854, - "y": 0.6872733398244574 - }, - "nextControl": { - "x": 4.109778196633054, - "y": 0.6872733398244574 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.303302403479807, - "y": 0.7360412168741004 - }, - "prevControl": { - "x": 4.818317779170107, - "y": 0.7360412168741004 - }, - "nextControl": { - "x": 5.828125506993927, - "y": 0.7360412168741004 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.694433640603393, - "y": 0.8154847264555638 - }, - "prevControl": { - "x": 6.102898428019079, - "y": 0.7729795494313204 - }, - "nextControl": { - "x": 6.102898428019079, - "y": 0.7729795494313204 - }, - "holonomicAngle": 0.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.316233719272994, - "y": 0.7524518104591758 - }, - "prevControl": { - "x": 5.755477834214798, - "y": 0.7524518104591756 - }, - "nextControl": { - "x": 4.8769896043311896, - "y": 0.7524518104591759 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.717041670194452, - "y": 0.7084969343549796 - }, - "prevControl": { - "x": 4.248075648196162, - "y": 0.7084969343549796 - }, - "nextControl": { - "x": 3.1253090980516522, - "y": 0.7084969343549796 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 1.834230493113764, - "y": 1.1 - }, - "prevControl": { - "x": 3.328467547907319, - "y": 0.7980883374961188 - }, - "nextControl": { - "x": 3.328467547907319, - "y": 0.7980883374961188 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [ - "ScoreCubeHigh" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.080923415319053, - "y": 0.9977988827505433 - }, - "prevControl": { - "x": 3.665490370911298, - "y": 0.32913967911269143 - }, - "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": 2.015234375, - "names": [ - "PickupCube" - ] - }, - { - "position": 3.921875000000001, - "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 index 8eb0199..f03378e 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+2Cube.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+2Cube.path @@ -3,12 +3,12 @@ { "anchorPoint": { "x": 1.85, - "y": 0.5 + "y": 1.5 }, "prevControl": null, "nextControl": { - "x": 3.8274507521237267, - "y": 0.8020895069567917 + "x": 2.614284574055259, + "y": 1.5526832772415535 }, "holonomicAngle": 180.0, "isReversal": false, @@ -17,7 +17,7 @@ "isStopPoint": false, "stopEvent": { "names": [ - "ScoreLow" + "ScoreConeHigh" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -26,18 +26,43 @@ }, { "anchorPoint": { - "x": 5.3285851554342, - "y": 0.93 + "x": 3.7181434414952164, + "y": 0.6872733398244574 }, "prevControl": { - "x": 4.357546951100798, - "y": 0.93 + "x": 3.0950688860698854, + "y": 0.6872733398244574 }, "nextControl": { - "x": 5.853408258948316, - "y": 0.93 + "x": 4.109778196633054, + "y": 0.6872733398244574 }, - "holonomicAngle": 180.0, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.303302403479807, + "y": 0.7360412168741004 + }, + "prevControl": { + "x": 4.818317779170107, + "y": 0.7360412168741004 + }, + "nextControl": { + "x": 5.828125506993927, + "y": 0.7360412168741004 + }, + "holonomicAngle": 90.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -51,18 +76,18 @@ }, { "anchorPoint": { - "x": 6.7, - "y": 0.932272484336635 + "x": 6.778806127056699, + "y": 1.88083958633195 }, "prevControl": { - "x": 6.262259163045806, - "y": 0.932272484336635 + "x": 6.096345788177688, + "y": 1.1524527806000053 }, "nextControl": { - "x": 6.262259163045806, - "y": 0.932272484336635 + "x": 6.096345788177688, + "y": 1.1524527806000053 }, - "holonomicAngle": 0, + "holonomicAngle": 45.0, "isReversal": true, "velOverride": null, "isLocked": false, @@ -76,18 +101,18 @@ }, { "anchorPoint": { - "x": 5.33, - "y": 0.75 + "x": 5.316233719272994, + "y": 0.7524518104591758 }, "prevControl": { - "x": 5.769244114941804, - "y": 0.75 + "x": 5.755477834214798, + "y": 0.7524518104591756 }, "nextControl": { - "x": 4.890755885058196, - "y": 0.75 + "x": 4.8769896043311896, + "y": 0.7524518104591759 }, - "holonomicAngle": 180.0, + "holonomicAngle": 90.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -101,15 +126,40 @@ }, { "anchorPoint": { - "x": 1.85, + "x": 3.717041670194452, + "y": 0.7084969343549796 + }, + "prevControl": { + "x": 4.248075648196162, + "y": 0.7084969343549796 + }, + "nextControl": { + "x": 3.1253090980516522, + "y": 0.7084969343549796 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.834230493113764, "y": 1.1 }, "prevControl": { - "x": 3.3442370547935543, + "x": 3.328467547907319, "y": 0.7980883374961188 }, "nextControl": { - "x": 3.3442370547935543, + "x": 3.328467547907319, "y": 0.7980883374961188 }, "holonomicAngle": 180.0, @@ -119,7 +169,7 @@ "isStopPoint": true, "stopEvent": { "names": [ - "ScoreLow" + "ScoreCubeHigh" ], "executionBehavior": "parallel", "waitBehavior": "none", @@ -128,18 +178,18 @@ }, { "anchorPoint": { - "x": 5.33, - "y": 0.75 + "x": 3.7216807072503038, + "y": 0.7002046556176442 }, "prevControl": { - "x": 3.7416048151272685, - "y": 0.5641657253120875 + "x": 3.3272218924947463, + "y": 0.7002046556176442 }, "nextControl": { - "x": 5.850070908475151, - "y": 0.8108456893997114 + "x": 4.053104693898115, + "y": 0.7002046556176442 }, - "holonomicAngle": 180.0, + "holonomicAngle": 90.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -153,18 +203,43 @@ }, { "anchorPoint": { - "x": 6.8, - "y": 1.9 + "x": 5.3096230914684055, + "y": 0.7431737363474717 }, "prevControl": { - "x": 6.063017799670501, - "y": 1.1630177996705013 + "x": 4.6687819465724045, + "y": 0.7431737363474717 }, "nextControl": { - "x": 6.063017799670501, - "y": 1.1630177996705013 + "x": 5.833241223087905, + "y": 0.7431737363474717 }, - "holonomicAngle": 45.0, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.654653897849463, + "y": 0.9274014954279904 + }, + "prevControl": { + "x": 6.238880201718738, + "y": 0.8839685109925763 + }, + "nextControl": { + "x": 6.238880201718738, + "y": 0.8839685109925763 + }, + "holonomicAngle": 0.0, "isReversal": true, "velOverride": null, "isLocked": false, @@ -178,18 +253,18 @@ }, { "anchorPoint": { - "x": 5.33, - "y": 0.75 + "x": 5.317799394279344, + "y": 0.745377278949002 }, "prevControl": { - "x": 5.6606822206699965, - "y": 0.850451468264047 + "x": 5.663402097601588, + "y": 0.745377278949002 }, "nextControl": { - "x": 4.8188409362392335, - "y": 0.5947250101102777 + "x": 4.78357667766198, + "y": 0.745377278949002 }, - "holonomicAngle": 180.0, + "holonomicAngle": 90.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -203,53 +278,51 @@ }, { "anchorPoint": { - "x": 1.85, - "y": 1.0969582998193774 + "x": 3.7278274313493074, + "y": 0.6984650167216994 }, "prevControl": { - "x": 3.036262982248185, - "y": 0.7111380545677755 + "x": 4.327854780791005, + "y": 0.6984650167216994 }, "nextControl": null, - "holonomicAngle": 180.0, + "holonomicAngle": 90.0, "isReversal": false, "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [ - "ScoreCubeHigh" - ], + "names": [], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0 } } ], - "maxVelocity": 4.5, - "maxAcceleration": 2.3, + "maxVelocity": 4.4, + "maxAcceleration": 3.0, "isReversed": null, "markers": [ { - "position": 1.0536931818181818, + "position": 2.015234375, "names": [ "PickupCube" ] }, { - "position": 2.921875000000001, + "position": 3.921875000000001, "names": [ "Neutral" ] }, { - "position": 5.5576704545454545, + "position": 7.850781250000001, "names": [ "PickupCube" ] }, { - "position": 6.715056818181819, + "position": 9.715056818181818, "names": [ "Neutral" ] diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+2Cube_V2.path b/src/main/deploy/pathplanner/b_cable_1Cone+2Cube_V2.path deleted file mode 100644 index 5387101..0000000 --- a/src/main/deploy/pathplanner/b_cable_1Cone+2Cube_V2.path +++ /dev/null @@ -1,331 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.85, - "y": 0.5 - }, - "prevControl": null, - "nextControl": { - "x": 2.614284574055259, - "y": 0.5526832772415535 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "ScoreConeHigh" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.7181434414952164, - "y": 0.6872733398244574 - }, - "prevControl": { - "x": 3.0950688860698854, - "y": 0.6872733398244574 - }, - "nextControl": { - "x": 4.109778196633054, - "y": 0.6872733398244574 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.303302403479807, - "y": 0.7360412168741004 - }, - "prevControl": { - "x": 4.818317779170107, - "y": 0.7360412168741004 - }, - "nextControl": { - "x": 5.828125506993927, - "y": 0.7360412168741004 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.778806127056699, - "y": 1.88083958633195 - }, - "prevControl": { - "x": 6.096345788177688, - "y": 1.1524527806000053 - }, - "nextControl": { - "x": 6.096345788177688, - "y": 1.1524527806000053 - }, - "holonomicAngle": 45.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.316233719272994, - "y": 0.7524518104591758 - }, - "prevControl": { - "x": 5.755477834214798, - "y": 0.7524518104591756 - }, - "nextControl": { - "x": 4.8769896043311896, - "y": 0.7524518104591759 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.717041670194452, - "y": 0.7084969343549796 - }, - "prevControl": { - "x": 4.248075648196162, - "y": 0.7084969343549796 - }, - "nextControl": { - "x": 3.1253090980516522, - "y": 0.7084969343549796 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 1.834230493113764, - "y": 1.1 - }, - "prevControl": { - "x": 3.328467547907319, - "y": 0.7980883374961188 - }, - "nextControl": { - "x": 3.328467547907319, - "y": 0.7980883374961188 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [ - "ScoreCubeHigh" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.7216807072503038, - "y": 0.7002046556176442 - }, - "prevControl": { - "x": 3.3272218924947463, - "y": 0.7002046556176442 - }, - "nextControl": { - "x": 4.053104693898115, - "y": 0.7002046556176442 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.3096230914684055, - "y": 0.7431737363474717 - }, - "prevControl": { - "x": 4.6687819465724045, - "y": 0.7431737363474717 - }, - "nextControl": { - "x": 5.833241223087905, - "y": 0.7431737363474717 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.654653897849463, - "y": 0.9274014954279904 - }, - "prevControl": { - "x": 6.238880201718738, - "y": 0.8839685109925763 - }, - "nextControl": { - "x": 6.238880201718738, - "y": 0.8839685109925763 - }, - "holonomicAngle": 0.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.317799394279344, - "y": 0.745377278949002 - }, - "prevControl": { - "x": 5.663402097601588, - "y": 0.745377278949002 - }, - "nextControl": { - "x": 4.78357667766198, - "y": 0.745377278949002 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.7278274313493074, - "y": 0.6984650167216994 - }, - "prevControl": { - "x": 4.327854780791005, - "y": 0.6984650167216994 - }, - "nextControl": null, - "holonomicAngle": 90.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": 2.015234375, - "names": [ - "PickupCube" - ] - }, - { - "position": 3.921875000000001, - "names": [ - "Neutral" - ] - }, - { - "position": 7.850781250000001, - "names": [ - "PickupCube" - ] - }, - { - "position": 9.715056818181818, - "names": [ - "Neutral" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path b/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path new file mode 100644 index 0000000..fecb67b --- /dev/null +++ b/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path @@ -0,0 +1,144 @@ +{ + "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": 1.9 + }, + "prevControl": { + "x": 6.036038306451613, + "y": 2.1836771159653425 + }, + "nextControl": { + "x": 6.036038306451613, + "y": 2.1836771159653425 + }, + "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": [ + "Balance" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.5, + "maxAcceleration": 3.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+1Cube+Climb.path b/src/main/deploy/pathplanner/b_mid_1Cone+1Cube+Climb.path index 0bcea67..3ae1e12 100644 --- a/src/main/deploy/pathplanner/b_mid_1Cone+1Cube+Climb.path +++ b/src/main/deploy/pathplanner/b_mid_1Cone+1Cube+Climb.path @@ -27,15 +27,15 @@ { "anchorPoint": { "x": 2.407499497290661, - "y": 2.7256082342025794 + "y": 2.73 }, "prevControl": { "x": 2.1337963109954, - "y": 2.6430913625716137 + "y": 2.6474831283690343 }, "nextControl": { "x": 2.665994993099602, - "y": 2.803540244909385 + "y": 2.8079320107068058 }, "holonomicAngle": 0, "isReversal": false, @@ -101,14 +101,17 @@ }, { "anchorPoint": { - "x": 5.505970334857337, - "y": 2.797223368752293 + "x": 5.5, + "y": 2.8 }, "prevControl": { - "x": 5.891590290125025, - "y": 2.6871042266390086 + "x": 5.845939452221272, + "y": 2.744509911273745 + }, + "nextControl": { + "x": 5.104026854742105, + "y": 2.863515695658558 }, - "nextControl": null, "holonomicAngle": 180.0, "isReversal": false, "velOverride": null, @@ -122,9 +125,34 @@ "waitBehavior": "none", "waitTime": 0 } + }, + { + "anchorPoint": { + "x": 1.85, + "y": 2.8 + }, + "prevControl": { + "x": 1.657861376779508, + "y": 2.8 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreCubeHigh", + "Balance2" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } } ], - "maxVelocity": 1.0, + "maxVelocity": 1.5, "maxAcceleration": 2.0, "isReversed": null, "markers": [ diff --git a/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path b/src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path similarity index 100% rename from src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path rename to src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index d4d0439..d1cc0e6 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -140,8 +140,8 @@ public static class SwerveConstants { // Theoretical: v = 4.96824, omega = 11.5 // Real: v = 4.5, omega = 10 // 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 maxAcceleration = 2.5; + public static final double maxSpeed = 4.4; //meters per second - 16.3 ft/sec + public static final double maxAcceleration = 3; public static final double maxAngularVelocity = 5; //3; //11.5; // citrus: 10 - Mason look at this later wtf public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(maxSpeed, maxAcceleration); diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index b2946e8..a945c18 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -26,9 +26,9 @@ private void initAutoSelector() { final String[] autoStrings = new String[] { //Blue Autos //Cable - "cable_1Cone+1Cube","cable_1Cone+2Cube", + "cable_1Cone+1Cube","cable_1Cone+2Cube", "cable_1Cone+1.5Cube+Climb", //Mid - "mid_1Cone+Climb","mid_1Cone+1Cube+Climb", + "mid_1Cone+Climb","mid_1Cone+0.5Cube+Climb", "mid_1Cone+1Cube+Climb", //Hp "hp_1Cone+1Cube", @@ -42,12 +42,12 @@ public Command getAutonomousCommand() { final Command autoCommand; if (selectedAutoName == null) { - autoCommand = score(Position.HIGH_CONE, true).beforeStarting(Trajectories.resetAuto()); + autoCommand = score(Position.HIGH_CONE, true); } else if (selectedAutoName == "scuffedClimb") { autoCommand = sequence( - score(Position.HIGH_CONE, true).beforeStarting(Trajectories.resetAuto()), + score(Position.HIGH_CONE, true), new CmdAutoBalance(false) ); } diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index 184ad27..81dbaec 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -71,6 +71,8 @@ public static void initTrajectories() { CommandEventMap.put("Neutral", retract(Position.NEUTRAL)); CommandEventMap.put("Balance", new ScheduleCommand(new CmdAutoBalance(true))); + + CommandEventMap.put("Balance2", new ScheduleCommand(new CmdAutoBalance(false))); for (final String trajectoryName : trajectoryNames) { @@ -99,7 +101,7 @@ public static CommandBase generateAuto(PathPlannerTrajectory trajectory) { } public static CommandBase get(String name) { - return builder.fullAuto(trajectories.get(name)).beforeStarting(resetAuto()); + return builder.fullAuto(trajectories.get(name)); } public static CommandBase resetAuto() { From 8f42d5c7f9e34a5e74d7e85ea275cf8e79b4444b Mon Sep 17 00:00:00 2001 From: "Team 3128: Controls" Date: Sat, 28 Oct 2023 23:35:53 -0700 Subject: [PATCH 10/12] Workshop 10/28 --- .../b_cable_1Cone+1.5Cube+Climb.path | 10 +- .../pathplanner/b_cable_1Cone+1Cube.path | 12 +- .../pathplanner/b_cable_1Cone+1Cube_V2.path | 225 ++++++++++++++++++ .../b_mid_1Cone+0.5Cube+Climb.path | 6 +- src/main/java/frc/team3128/Constants.java | 8 +- .../frc/team3128/autonomous/AutoPrograms.java | 2 +- .../frc/team3128/autonomous/Trajectories.java | 11 +- .../frc/team3128/commands/CmdAutoBalance.java | 4 +- .../frc/team3128/commands/CmdSwerveDrive.java | 2 +- .../common/hardware/camera/NAR_Camera.java | 2 +- .../java/frc/team3128/subsystems/Vision.java | 9 +- 11 files changed, 262 insertions(+), 29 deletions(-) create mode 100644 src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path b/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path index 4bbb930..e922079 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path @@ -151,16 +151,16 @@ }, { "anchorPoint": { - "x": 1.834230493113764, + "x": 1.82, "y": 1.1 }, "prevControl": { - "x": 3.328467547907319, - "y": 0.7980883374961188 + "x": 3.314237054793555, + "y": 0.798088337496119 }, "nextControl": { - "x": 3.328467547907319, - "y": 0.7980883374961188 + "x": 3.314237054793555, + "y": 0.798088337496119 }, "holonomicAngle": 180.0, "isReversal": true, diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path index bc5cfc4..15e341a 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path @@ -152,15 +152,15 @@ { "anchorPoint": { "x": 1.834230493113764, - "y": 1.1 + "y": 1.5 }, "prevControl": { "x": 3.328467547907319, - "y": 0.7980883374961188 + "y": 1.1980883374961189 }, "nextControl": { "x": 3.328467547907319, - "y": 0.7980883374961188 + "y": 1.1980883374961189 }, "holonomicAngle": 180.0, "isReversal": true, @@ -178,11 +178,11 @@ }, { "anchorPoint": { - "x": 7.080923415319053, + "x": 6.0, "y": 0.9977988827505433 }, "prevControl": { - "x": 3.665490370911298, + "x": 2.5845669555922446, "y": 0.32913967911269143 }, "nextControl": null, @@ -204,7 +204,7 @@ "isReversed": null, "markers": [ { - "position": 2.015234375, + "position": 1.4254545454545942, "names": [ "PickupCube" ] diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path new file mode 100644 index 0000000..bd17d85 --- /dev/null +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path @@ -0,0 +1,225 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.85, + "y": 0.5 + }, + "prevControl": null, + "nextControl": { + "x": 2.614284574055259, + "y": 0.5526832772415535 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.7075088130204783, + "y": 0.6 + }, + "prevControl": { + "x": 3.0844342575951473, + "y": 0.6 + }, + "nextControl": { + "x": 4.099143568158317, + "y": 0.6 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.694433640603393, + "y": 0.7 + }, + "prevControl": { + "x": 5.743020330548507, + "y": 0.6049491252537939 + }, + "nextControl": { + "x": 5.743020330548507, + "y": 0.6049491252537939 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.6960733550526776, + "y": 0.6 + }, + "prevControl": { + "x": 4.227107333054388, + "y": 0.6 + }, + "nextControl": { + "x": 3.104340782909878, + "y": 0.6 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.834230493113764, + "y": 1.1 + }, + "prevControl": { + "x": 2.472479352948291, + "y": 0.6632565618911479 + }, + "nextControl": { + "x": 2.472479352948291, + "y": 0.6632565618911479 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "ScoreCubeHigh" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.741815186911401, + "y": 0.7089983937457166 + }, + "prevControl": { + "x": 3.4444932798567436, + "y": 0.7089983937457166 + }, + "nextControl": { + "x": 4.04263511909015, + "y": 0.7089983937457166 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.726469715422112, + "y": 1.9897696856769331 + }, + "prevControl": { + "x": 5.6227746574520605, + "y": 0.2234883748754959 + }, + "nextControl": { + "x": 5.6227746574520605, + "y": 0.2234883748754959 + }, + "holonomicAngle": 59.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.741815186911401, + "y": 0.6289501880002328 + }, + "prevControl": { + "x": 4.222104421384305, + "y": 0.6746920198547948 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.5, + "maxAcceleration": 2.3, + "isReversed": null, + "markers": [ + { + "position": 1.1272727272727252, + "names": [ + "PickupCube" + ] + }, + { + "position": 2.5090909090909514, + "names": [ + "Neutral" + ] + }, + { + "position": 5.141818181818192, + "names": [ + "PickupCube" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path b/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path index fecb67b..0ab2eb9 100644 --- a/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path +++ b/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path @@ -77,15 +77,15 @@ { "anchorPoint": { "x": 6.707654896212571, - "y": 1.9 + "y": 1.5 }, "prevControl": { "x": 6.036038306451613, - "y": 2.1836771159653425 + "y": 1.7836771159653428 }, "nextControl": { "x": 6.036038306451613, - "y": 2.1836771159653425 + "y": 1.7836771159653428 }, "holonomicAngle": 0, "isReversal": true, diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index d1cc0e6..ec87b43 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -73,7 +73,7 @@ public static class AutoConstants { 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 DRIVE_SPEED = Units.inchesToMeters(20); //30, 40 } @@ -213,6 +213,12 @@ public static class VisionConstants { new Transform2d( new Translation2d(Units.inchesToMeters(6.57), Units.inchesToMeters(-11.7)), Rotation2d.fromDegrees(180))); + + public static final Camera BACK_LEFT = new Camera("BACK_LEFT", true, 0, 0, 0, + new Transform2d( + new Translation2d(Units.inchesToMeters(6.57), Units.inchesToMeters(18.89)), + Rotation2d.fromDegrees(180))); + public static final double POSE_THRESH = 100; public static final Matrix SVR_STATE_STD = VecBuilder.fill(0.1,0.1,Units.degreesToRadians(3)); diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index a945c18..c8ef359 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -26,7 +26,7 @@ private void initAutoSelector() { final String[] autoStrings = new String[] { //Blue Autos //Cable - "cable_1Cone+1Cube","cable_1Cone+2Cube", "cable_1Cone+1.5Cube+Climb", + "cable_1Cone+1Cube","cable_1Cone+2Cube", "cable_1Cone+1Cube_V2", "cable_1Cone+1.5Cube+Climb", //Mid "mid_1Cone+Climb","mid_1Cone+0.5Cube+Climb", "mid_1Cone+1Cube+Climb", //Hp diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index 81dbaec..c2591d8 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -45,9 +45,9 @@ public static void initTrajectories() { final String[] trajectoryNames = { //Blue Autos //Cable - "b_cable_1Cone+1Cube","b_cable_1Cone+2Cube", "b_cable_1Cone+2Cube+Climb", + "b_cable_1Cone+1Cube", "b_cable_1Cone+1Cube_V2","b_cable_1Cone+2Cube", "b_cable_1Cone+2Cube+Climb", //Mid - "b_mid_1Cone+Climb","b_mid_1Cone+1Cube+Climb", + "b_mid_1Cone+Climb","b_mid_1Cone+0.5Cube+Climb", "b_mid_1Cone+1Cube+Climb", //Hp "b_hp_1Cone+1Cube","b_cable_1Cone+2Cube", @@ -55,12 +55,12 @@ public static void initTrajectories() { //Cable "r_cable_1Cone+1Cube","r_cable_1Cone+2Cube", //Mid - "r_mid_1Cone+Climb","r_mid_1Cone+1Cube+Climb", + "r_mid_1Cone+Climb","r_mid_1Cone+0.5Cube+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("ScoreConeHigh", sequence(score(Position.HIGH_CONE, true))); CommandEventMap.put("ScoreCubeHigh", score(Position.HIGH_CUBE, true)); @@ -68,7 +68,7 @@ public static void initTrajectories() { CommandEventMap.put("PickupCube", pickup(Position.GROUND_CUBE, true)); - CommandEventMap.put("Neutral", retract(Position.NEUTRAL)); + CommandEventMap.put("Neutral", sequence(retract(Position.NEUTRAL))); CommandEventMap.put("Balance", new ScheduleCommand(new CmdAutoBalance(true))); @@ -110,6 +110,7 @@ public static CommandBase resetAuto() { resetLeds(), resetGyro(DriverStation.getAlliance() == Alliance.Red ? 0 : 180), runOnce(()-> Manipulator.getInstance().set(-0.4), Manipulator.getInstance()), + runOnce(()-> Manipulator.getInstance().isCone = true), 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 db4f2a9..4a69090 100644 --- a/src/main/java/frc/team3128/commands/CmdAutoBalance.java +++ b/src/main/java/frc/team3128/commands/CmdAutoBalance.java @@ -49,12 +49,12 @@ public void execute() { // final double angleVelocity = (advAngle - prevAngle) / 0.02; final double angleVelocity = getAngleVelocity(advAngle); - if (advAngle > RAMP_THRESHOLD) onRamp = true; + if (Math.abs(advAngle) > RAMP_THRESHOLD) onRamp = true; if (Math.abs(advAngle) < ANGLE_THRESHOLD && onRamp) { swerve.xlock(); if (direction == 1) { - Manipulator.getInstance().set(-1); + // Manipulator.getInstance().set(-1); } return; } diff --git a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java index 411b5a9..d083a39 100644 --- a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java +++ b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java @@ -62,7 +62,7 @@ public void execute() { rotation = -zAxis.getAsDouble() * maxAngularVelocity * swerve.throttle; - if (Math.abs(rotation) > maxAngularVelocity * swerve.throttle / 2.0) { + if (Math.abs(rotation) > maxAngularVelocity * swerve.throttle / 4.0) { enabled = false; } if (enabled) { 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 5312416..cf5c615 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 @@ -464,7 +464,7 @@ private Pose2d getPos(PhotonTrackedTarget target) { private Pose2d getPosApril(PhotonTrackedTarget tag) { if(!hasValidTarget() || !AprilTags.containsKey(targetId(tag))) return new Pose2d(); Transform2d transform = getProcessedTarget(tag); - if (!AprilTags.containsKey(targetId(tag)) || transform.getX() > 5 || Math.abs(transform.getRotation().getDegrees()) < 150) return new Pose2d(); + if (!AprilTags.containsKey(targetId(tag)) || transform.getX() > 4 || Math.abs(transform.getRotation().getDegrees()) < 150) return new Pose2d(); Pose2d target = AprilTags.get(targetId()); if (target == null) return new Pose2d(); Translation2d coord = target.getTranslation().plus(transform.getTranslation().rotateBy(target.getRotation())); diff --git a/src/main/java/frc/team3128/subsystems/Vision.java b/src/main/java/frc/team3128/subsystems/Vision.java index 3186788..d3b157f 100644 --- a/src/main/java/frc/team3128/subsystems/Vision.java +++ b/src/main/java/frc/team3128/subsystems/Vision.java @@ -33,9 +33,10 @@ public Vision() { NAR_Camera.setAprilTags(APRIL_TAG_POS); NAR_Camera.multipleTargets = false; cameras = new HashMap(); - cameras.put(FRONT_LEFT.hostname, new NAR_Camera(FRONT_LEFT)); - cameras.put(FRONT_RIGHT.hostname, new NAR_Camera(FRONT_RIGHT)); + // 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)); + cameras.put(BACK_LEFT.hostname, new NAR_Camera(BACK_LEFT)); } public Pose2d targetPos(String name, Pose2d robotPos) { @@ -122,8 +123,8 @@ public void periodic(){ } public void initShuffleboard() { - NAR_Camera cam = cameras.get(FRONT_LEFT.hostname); - NAR_Camera cam2 = cameras.get(FRONT_RIGHT.hostname); + NAR_Camera cam = cameras.get(BACK_LEFT.hostname); + NAR_Camera cam2 = cameras.get(BACK_RIGHT.hostname); NAR_Shuffleboard.addData("VisionComp", "HasTarget", ()->cam.hasValidTarget() || cam2.hasValidTarget(), 0, 0); NAR_Shuffleboard.addData("Vision","HasTarget", ()->cam.hasValidTarget(), 0, 0); From 16e37f0337548698ee9f95d5c465f65371ad72df Mon Sep 17 00:00:00 2001 From: "Team 3128: Controls" Date: Sun, 29 Oct 2023 00:22:32 -0700 Subject: [PATCH 11/12] Organized Auto Paths Co-Authored-By: Mason-Lam <97353903+Mason-Lam@users.noreply.github.com> --- ...ube_V2.path => b_cable_1Cone+1.5Cube.path} | 10 +- .../pathplanner/b_cable_1Cone+1Cube.path | 129 +++------- .../b_mid_1Cone+0.5Cube+Climb.path | 8 +- .../pathplanner/b_mid_1Cone+1Cube+Climb.path | 49 +++- .../deploy/pathplanner/b_mid_1Cone+Climb.path | 4 +- .../pathplanner/r_cable_1Cone+1.5Cube.path | 225 ++++++++++++++++++ .../pathplanner/r_cable_1Cone+1Cube.path | 93 +++----- .../r_mid_1Cone+0.5Cube+Climb.path | 14 +- .../pathplanner/r_mid_1Cone+1Cube+Climb.path | 197 +++++++++++++++ .../deploy/pathplanner/r_mid_1Cone+Climb.path | 4 +- .../frc/team3128/autonomous/AutoPrograms.java | 2 +- .../frc/team3128/autonomous/Trajectories.java | 8 +- 12 files changed, 545 insertions(+), 198 deletions(-) rename src/main/deploy/pathplanner/{b_cable_1Cone+1Cube_V2.path => b_cable_1Cone+1.5Cube.path} (96%) create mode 100644 src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path create mode 100644 src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path b/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube.path similarity index 96% rename from src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path rename to src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube.path index bd17d85..fa0a64a 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube.path @@ -178,15 +178,15 @@ }, { "anchorPoint": { - "x": 3.741815186911401, - "y": 0.6289501880002328 + "x": 4.656651824002649, + "y": 0.5717728981820299 }, "prevControl": { - "x": 4.222104421384305, - "y": 0.6746920198547948 + "x": 5.182682890330118, + "y": 0.6518211039275137 }, "nextControl": null, - "holonomicAngle": 0.0, + "holonomicAngle": 0, "isReversal": false, "velOverride": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path index 15e341a..817d3e8 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path @@ -26,43 +26,18 @@ }, { "anchorPoint": { - "x": 3.7181434414952164, - "y": 0.6872733398244574 + "x": 3.7075088130204783, + "y": 0.6 }, "prevControl": { - "x": 3.0950688860698854, - "y": 0.6872733398244574 + "x": 3.0844342575951473, + "y": 0.6 }, "nextControl": { - "x": 4.109778196633054, - "y": 0.6872733398244574 + "x": 4.099143568158317, + "y": 0.6 }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.303302403479807, - "y": 0.7360412168741004 - }, - "prevControl": { - "x": 4.818317779170107, - "y": 0.7360412168741004 - }, - "nextControl": { - "x": 5.828125506993927, - "y": 0.7360412168741004 - }, - "holonomicAngle": 90.0, + "holonomicAngle": 0.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -77,15 +52,15 @@ { "anchorPoint": { "x": 6.694433640603393, - "y": 0.8154847264555638 + "y": 0.7 }, "prevControl": { - "x": 6.102898428019079, - "y": 0.7729795494313204 + "x": 5.743020330548507, + "y": 0.6049491252537939 }, "nextControl": { - "x": 6.102898428019079, - "y": 0.7729795494313204 + "x": 5.743020330548507, + "y": 0.6049491252537939 }, "holonomicAngle": 0.0, "isReversal": true, @@ -101,43 +76,18 @@ }, { "anchorPoint": { - "x": 5.316233719272994, - "y": 0.7524518104591758 - }, - "prevControl": { - "x": 5.755477834214798, - "y": 0.7524518104591756 - }, - "nextControl": { - "x": 4.8769896043311896, - "y": 0.7524518104591759 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.717041670194452, - "y": 0.7084969343549796 + "x": 3.6960733550526776, + "y": 0.6 }, "prevControl": { - "x": 4.248075648196162, - "y": 0.7084969343549796 + "x": 4.227107333054388, + "y": 0.6 }, "nextControl": { - "x": 3.1253090980516522, - "y": 0.7084969343549796 + "x": 3.104340782909878, + "y": 0.6 }, - "holonomicAngle": 90.0, + "holonomicAngle": 0.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -152,38 +102,11 @@ { "anchorPoint": { "x": 1.834230493113764, - "y": 1.5 + "y": 1.1 }, "prevControl": { - "x": 3.328467547907319, - "y": 1.1980883374961189 - }, - "nextControl": { - "x": 3.328467547907319, - "y": 1.1980883374961189 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [ - "ScoreCubeHigh" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.0, - "y": 0.9977988827505433 - }, - "prevControl": { - "x": 2.5845669555922446, - "y": 0.32913967911269143 + "x": 2.472479352948291, + "y": 0.6632565618911479 }, "nextControl": null, "holonomicAngle": 180.0, @@ -192,7 +115,9 @@ "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [], + "names": [ + "ScoreCubeHigh" + ], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0 @@ -204,13 +129,13 @@ "isReversed": null, "markers": [ { - "position": 1.4254545454545942, + "position": 1.1272727272727252, "names": [ "PickupCube" ] }, { - "position": 3.921875000000001, + "position": 2.5090909090909514, "names": [ "Neutral" ] diff --git a/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path b/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path index 0ab2eb9..96bb318 100644 --- a/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path +++ b/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path @@ -55,12 +55,12 @@ "y": 2.7413229722292773 }, "prevControl": { - "x": 3.228725032102842, - "y": 2.7392933935173427 + "x": 3.228721800003708, + "y": 2.7413229722292773 }, "nextControl": { - "x": 4.45150006934901, - "y": 2.743187935905513 + "x": 4.451503039299102, + "y": 2.7413229722292773 }, "holonomicAngle": 0.0, "isReversal": false, 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 3ae1e12..06f8183 100644 --- a/src/main/deploy/pathplanner/b_mid_1Cone+1Cube+Climb.path +++ b/src/main/deploy/pathplanner/b_mid_1Cone+1Cube+Climb.path @@ -55,12 +55,12 @@ "y": 2.7413229722292773 }, "prevControl": { - "x": 3.228725032102842, - "y": 2.7392933935173427 + "x": 3.228721800003708, + "y": 2.7413229722292773 }, "nextControl": { - "x": 4.45150006934901, - "y": 2.743187935905513 + "x": 4.451503039299102, + "y": 2.7413229722292773 }, "holonomicAngle": 0.0, "isReversal": false, @@ -77,15 +77,15 @@ { "anchorPoint": { "x": 6.707654896212571, - "y": 1.9 + "y": 1.5 }, "prevControl": { "x": 6.036038306451613, - "y": 2.1836771159653425 + "y": 1.7836771159653426 }, "nextControl": { "x": 6.036038306451613, - "y": 2.1836771159653425 + "y": 1.7836771159653426 }, "holonomicAngle": 0, "isReversal": true, @@ -102,17 +102,17 @@ { "anchorPoint": { "x": 5.5, - "y": 2.8 + "y": 2.27 }, "prevControl": { "x": 5.845939452221272, - "y": 2.744509911273745 + "y": 2.2145099112737454 }, "nextControl": { "x": 5.104026854742105, - "y": 2.863515695658558 + "y": 2.333515695658558 }, - "holonomicAngle": 180.0, + "holonomicAngle": 0.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -126,13 +126,38 @@ "waitTime": 0 } }, + { + "anchorPoint": { + "x": 2.41, + "y": 2.27 + }, + "prevControl": { + "x": 2.8289009120559596, + "y": 2.27 + }, + "nextControl": { + "x": 1.9407280395724524, + "y": 2.27 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, { "anchorPoint": { "x": 1.85, "y": 2.8 }, "prevControl": { - "x": 1.657861376779508, + "x": 2.042138623220492, "y": 2.8 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/b_mid_1Cone+Climb.path b/src/main/deploy/pathplanner/b_mid_1Cone+Climb.path index d81b0fc..e03bf6b 100644 --- a/src/main/deploy/pathplanner/b_mid_1Cone+Climb.path +++ b/src/main/deploy/pathplanner/b_mid_1Cone+Climb.path @@ -51,11 +51,11 @@ }, { "anchorPoint": { - "x": 5.7, + "x": 5.9, "y": 2.8069653465695823 }, "prevControl": { - "x": 5.2989651195214815, + "x": 5.498965119521482, "y": 2.8069653465695823 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path b/src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path new file mode 100644 index 0000000..6c87510 --- /dev/null +++ b/src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path @@ -0,0 +1,225 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 14.69, + "y": 0.5 + }, + "prevControl": null, + "nextControl": { + "x": 13.925675879064304, + "y": 0.5521063911206201 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 12.83, + "y": 0.6 + }, + "prevControl": { + "x": 13.453074555425331, + "y": 0.6 + }, + "nextControl": { + "x": 12.438365244862162, + "y": 0.6 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.85, + "y": 0.7 + }, + "prevControl": { + "x": 10.801405343067072, + "y": 0.6048694133149678 + }, + "nextControl": { + "x": 10.801405343067072, + "y": 0.6048694133149678 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 12.84, + "y": 0.6 + }, + "prevControl": { + "x": 12.30896602199829, + "y": 0.6 + }, + "nextControl": { + "x": 13.4317325721428, + "y": 0.6 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 14.69, + "y": 1.1 + }, + "prevControl": { + "x": 14.051726678100353, + "y": 0.6632923124532808 + }, + "nextControl": { + "x": 14.051726678100353, + "y": 0.6632923124532808 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "ScoreCubeHigh" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 12.8, + "y": 0.7089983937457166 + }, + "prevControl": { + "x": 13.097321907054658, + "y": 0.7089983937457166 + }, + "nextControl": { + "x": 12.499180067821252, + "y": 0.7089983937457166 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.81, + "y": 1.9897696856769331 + }, + "prevControl": { + "x": 10.882701100548172, + "y": 0.20449525333979146 + }, + "nextControl": { + "x": 10.882701100548172, + "y": 0.20449525333979146 + }, + "holonomicAngle": 121.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 11.88, + "y": 0.5717728981820299 + }, + "prevControl": { + "x": 11.353965383041718, + "y": 0.49174802863363587 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.5, + "maxAcceleration": 2.3, + "isReversed": null, + "markers": [ + { + "position": 1.1272727272727252, + "names": [ + "PickupCube" + ] + }, + { + "position": 2.5090909090909514, + "names": [ + "Neutral" + ] + }, + { + "position": 5.141818181818192, + "names": [ + "PickupCube" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path b/src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path index 482ca41..d5ac369 100644 --- a/src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path +++ b/src/main/deploy/pathplanner/r_cable_1Cone+1Cube.path @@ -2,15 +2,15 @@ "waypoints": [ { "anchorPoint": { - "x": 14.700000000000001, + "x": 14.69, "y": 0.5 }, "prevControl": null, "nextControl": { - "x": 12.722549247876273, - "y": 0.8020895069567917 + "x": 13.925675879064304, + "y": 0.5521063911206201 }, - "holonomicAngle": 360.0, + "holonomicAngle": 0.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -26,18 +26,18 @@ }, { "anchorPoint": { - "x": 11.2214148445658, - "y": 0.93 + "x": 12.83, + "y": 0.6 }, "prevControl": { - "x": 12.192453048899203, - "y": 0.93 + "x": 13.453074555425331, + "y": 0.6 }, "nextControl": { - "x": 10.696591741051684, - "y": 0.93 + "x": 12.438365244862162, + "y": 0.6 }, - "holonomicAngle": 360.0, + "holonomicAngle": 180.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -51,18 +51,18 @@ }, { "anchorPoint": { - "x": 9.9, - "y": 0.8 + "x": 9.85, + "y": 0.7 }, "prevControl": { - "x": 10.337740836954193, - "y": 0.8 + "x": 10.801405343067072, + "y": 0.6048694133149678 }, "nextControl": { - "x": 10.337740836954193, - "y": 0.8 + "x": 10.801405343067072, + "y": 0.6048694133149678 }, - "holonomicAngle": 180, + "holonomicAngle": 180.0, "isReversal": true, "velOverride": null, "isLocked": false, @@ -76,18 +76,18 @@ }, { "anchorPoint": { - "x": 11.22, - "y": 0.75 + "x": 12.84, + "y": 0.6 }, "prevControl": { - "x": 10.780755885058197, - "y": 0.75 + "x": 12.30896602199829, + "y": 0.6 }, "nextControl": { - "x": 11.659244114941805, - "y": 0.75 + "x": 13.4317325721428, + "y": 0.6 }, - "holonomicAngle": 360.0, + "holonomicAngle": 180.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -101,48 +101,23 @@ }, { "anchorPoint": { - "x": 14.700000000000001, + "x": 14.69, "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 + "x": 14.051726678100353, + "y": 0.6632923124532808 }, "nextControl": null, - "holonomicAngle": 360.0, + "holonomicAngle": 0.0, "isReversal": false, "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [], + "names": [ + "ScoreCubeHigh" + ], "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0 @@ -154,13 +129,13 @@ "isReversed": null, "markers": [ { - "position": 1.1, + "position": 1.1272727272727252, "names": [ "PickupCube" ] }, { - "position": 2.997869318181817, + "position": 2.5090909090909514, "names": [ "Neutral" ] diff --git a/src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path b/src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path index e10c8e7..178ec17 100644 --- a/src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path +++ b/src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path @@ -55,12 +55,12 @@ "y": 2.7413229722292773 }, "prevControl": { - "x": 13.321274967897159, - "y": 2.7392933935173427 + "x": 13.321278199996293, + "y": 2.7413229722292773 }, "nextControl": { - "x": 12.098499930650991, - "y": 2.743187935905513 + "x": 12.0984969607009, + "y": 2.7413229722292773 }, "holonomicAngle": 180.0, "isReversal": false, @@ -77,15 +77,15 @@ { "anchorPoint": { "x": 9.81, - "y": 1.9 + "y": 1.5 }, "prevControl": { "x": 10.481616589760959, - "y": 2.1836771159653425 + "y": 1.7836771159653426 }, "nextControl": { "x": 10.481616589760959, - "y": 2.1836771159653425 + "y": 1.7836771159653426 }, "holonomicAngle": 180, "isReversal": true, 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..f785c96 --- /dev/null +++ b/src/main/deploy/pathplanner/r_mid_1Cone+1Cube+Climb.path @@ -0,0 +1,197 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 14.69, + "y": 2.2 + }, + "prevControl": null, + "nextControl": { + "x": 14.471498788558998, + "y": 2.4127058555720837 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 14.13, + "y": 2.73 + }, + "prevControl": { + "x": 14.403699108564984, + "y": 2.647469604007534 + }, + "nextControl": { + "x": 13.871508355351597, + "y": 2.807944783618144 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 12.67, + "y": 2.7413229722292773 + }, + "prevControl": { + "x": 13.307232959683589, + "y": 2.7413229722292773 + }, + "nextControl": { + "x": 12.084451720388195, + "y": 2.7413229722292773 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": 4.0, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.83, + "y": 1.5 + }, + "prevControl": { + "x": 10.501607686442625, + "y": 1.2163018060055075 + }, + "nextControl": { + "x": 10.501607686442625, + "y": 1.2163018060055075 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 11.04, + "y": 2.27 + }, + "prevControl": { + "x": 10.694057790033689, + "y": 2.2145271064967047 + }, + "nextControl": { + "x": 11.435976301859341, + "y": 2.3334960134671374 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "Balance" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 14.13, + "y": 2.27 + }, + "prevControl": { + "x": 13.711099087944042, + "y": 2.27 + }, + "nextControl": { + "x": 14.599271960427549, + "y": 2.27 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 14.69, + "y": 2.8 + }, + "prevControl": { + "x": 14.497861376779507, + "y": 2.8 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreCubeHigh", + "Balance2" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 1.5, + "maxAcceleration": 2.0, + "isReversed": null, + "markers": [ + { + "position": 2.4062500000000004, + "names": [ + "PickupCube" + ] + }, + { + "position": 3.614488636363634, + "names": [ + "Neutral" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path b/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path index 17d9064..a571804 100644 --- a/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path +++ b/src/main/deploy/pathplanner/r_mid_1Cone+Climb.path @@ -51,11 +51,11 @@ }, { "anchorPoint": { - "x": 10.85, + "x": 10.65, "y": 2.8069653465695823 }, "prevControl": { - "x": 11.25103488047852, + "x": 11.05103488047852, "y": 2.8069653465695823 }, "nextControl": null, diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index c8ef359..545ed33 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -26,7 +26,7 @@ private void initAutoSelector() { final String[] autoStrings = new String[] { //Blue Autos //Cable - "cable_1Cone+1Cube","cable_1Cone+2Cube", "cable_1Cone+1Cube_V2", "cable_1Cone+1.5Cube+Climb", + "cable_1Cone+1Cube","cable_1Cone+1.5Cube", "cable_1Cone+2Cube", "cable_1Cone+1.5Cube+Climb", //Mid "mid_1Cone+Climb","mid_1Cone+0.5Cube+Climb", "mid_1Cone+1Cube+Climb", //Hp diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index c2591d8..af9751b 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -45,19 +45,19 @@ public static void initTrajectories() { final String[] trajectoryNames = { //Blue Autos //Cable - "b_cable_1Cone+1Cube", "b_cable_1Cone+1Cube_V2","b_cable_1Cone+2Cube", "b_cable_1Cone+2Cube+Climb", + "b_cable_1Cone+1Cube", "b_cable_1Cone+1.5Cube","b_cable_1Cone+2Cube", "b_cable_1Cone+1.5Cube+Climb", //Mid "b_mid_1Cone+Climb","b_mid_1Cone+0.5Cube+Climb", "b_mid_1Cone+1Cube+Climb", //Hp - "b_hp_1Cone+1Cube","b_cable_1Cone+2Cube", + "b_hp_1Cone+1Cube", //Red Autos //Cable - "r_cable_1Cone+1Cube","r_cable_1Cone+2Cube", + "r_cable_1Cone+1Cube", "r_cable_1Cone+1.5Cube","r_cable_1Cone+2Cube", "r_cable_1Cone+1.5Cube+Climb", //Mid "r_mid_1Cone+Climb","r_mid_1Cone+0.5Cube+Climb","r_mid_1Cone+1Cube+Climb", //Hp - "r_hp_1Cone+1Cube","r_cable_1Cone+2Cube", + "r_hp_1Cone+1Cube" }; CommandEventMap.put("ScoreConeHigh", sequence(score(Position.HIGH_CONE, true))); From db52733915273e9eb8fbce6b8916944014361ea1 Mon Sep 17 00:00:00 2001 From: "Team 3128: Controls" Date: Fri, 3 Nov 2023 00:36:57 -0700 Subject: [PATCH 12/12] SDA 10/29 --- src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path | 6 +++--- src/main/java/frc/team3128/Constants.java | 8 ++++---- src/main/java/frc/team3128/PositionConstants.java | 4 ++-- src/main/java/frc/team3128/Robot.java | 8 ++++++++ src/main/java/frc/team3128/autonomous/AutoPrograms.java | 2 +- src/main/java/frc/team3128/commands/CmdAutoBalance.java | 2 +- src/main/java/frc/team3128/commands/CmdManager.java | 2 +- src/main/java/frc/team3128/commands/CmdSwerveDrive.java | 4 +++- .../java/frc/team3128/common/swerveNeo/SwerveModule.java | 4 ++++ src/main/java/frc/team3128/subsystems/Swerve.java | 6 ++++++ 10 files changed, 33 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path b/src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path index 6c87510..222674c 100644 --- a/src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path +++ b/src/main/deploy/pathplanner/r_cable_1Cone+1.5Cube.path @@ -37,7 +37,7 @@ "x": 12.438365244862162, "y": 0.6 }, - "holonomicAngle": 180.0, + "holonomicAngle": 180.01, "isReversal": false, "velOverride": null, "isLocked": false, @@ -87,7 +87,7 @@ "x": 13.4317325721428, "y": 0.6 }, - "holonomicAngle": 180.0, + "holonomicAngle": 179.99, "isReversal": false, "velOverride": null, "isLocked": false, @@ -139,7 +139,7 @@ "x": 12.499180067821252, "y": 0.7089983937457166 }, - "holonomicAngle": 180.0, + "holonomicAngle": 180.01, "isReversal": false, "velOverride": null, "isLocked": false, diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index ec87b43..0774c4d 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -142,7 +142,7 @@ public static class SwerveConstants { // For safety, use less than theoretical and real values public static final double maxSpeed = 4.4; //meters per second - 16.3 ft/sec public static final double maxAcceleration = 3; - public static final double maxAngularVelocity = 5; //3; //11.5; // citrus: 10 - Mason look at this later wtf + public static final double maxAngularVelocity = 8; //3; //11.5; // citrus: 10 - Mason look at this later wtf public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(maxSpeed, maxAcceleration); /* Motor Inverts */ @@ -381,9 +381,9 @@ public static class ElevatorConstants { public static final double kI = 0; public static final double kD = 0; - public static final double kS = 1.05; + public static final double kS = 0.2375; //0.975; //1.05; public static final double kV = 0; - public static final double kG = 0.975; + public static final double kG = 0.4125;//0.975; public static final double MIN_DIST = 2; //Ask Charlie public static final double MAX_DIST = 55; //Ask Charlie @@ -422,7 +422,7 @@ public enum Colors { HOLDING(255,0,0,false), AUTO(0,0,0,true), - SHELF(0, 255, 0, false), + SHELF(255, 105, 180, false), CHUTE(0,0,225,false); public final int r; diff --git a/src/main/java/frc/team3128/PositionConstants.java b/src/main/java/frc/team3128/PositionConstants.java index 62faf18..570c757 100644 --- a/src/main/java/frc/team3128/PositionConstants.java +++ b/src/main/java/frc/team3128/PositionConstants.java @@ -8,12 +8,12 @@ public static enum Position { MID_CUBE(25, 30, false), LOW(3, 45, true), - SHELF_CONE(26, 45, true), + SHELF_CONE(25, 45, true), SHELF_CUBE(37, 22, false), CHUTE_CONE(10, 55, true), CHUTE_CUBE(10, 45, false), - GROUND_CONE(8, -18, true), + GROUND_CONE(10.5, -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 57aadac..90f7db9 100644 --- a/src/main/java/frc/team3128/Robot.java +++ b/src/main/java/frc/team3128/Robot.java @@ -9,7 +9,9 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team3128.Constants.LedConstants.Colors; import frc.team3128.autonomous.AutoPrograms; import frc.team3128.commands.CmdManager; @@ -37,6 +39,7 @@ public static synchronized Robot getInstance() { @Override public void robotInit(){ + new Trigger(this::isEnabled).negate().debounce(2).onTrue(new InstantCommand(()-> Swerve.getInstance().setBrakeMode(false)).ignoringDisable(true)); m_robotContainer.init(); LiveWindow.disableAllTelemetry(); } @@ -88,6 +91,11 @@ public void simulationInit() { public void simulationPeriodic() { CommandScheduler.getInstance().run(); } + + @Override + public void disabledInit() { + Swerve.getInstance().setBrakeMode(true); + } @Override public void disabledPeriodic() { diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index 545ed33..6c563ab 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -53,7 +53,7 @@ else if (selectedAutoName == "scuffedClimb") { } else { - selectedAutoName = (DriverStation.getAlliance() == Alliance.Red) ? "r_" : "b_" + selectedAutoName; + selectedAutoName = ((DriverStation.getAlliance() == Alliance.Red) ? "r_" : "b_") + selectedAutoName; autoCommand = Trajectories.get(selectedAutoName); } diff --git a/src/main/java/frc/team3128/commands/CmdAutoBalance.java b/src/main/java/frc/team3128/commands/CmdAutoBalance.java index 4a69090..c909c5a 100644 --- a/src/main/java/frc/team3128/commands/CmdAutoBalance.java +++ b/src/main/java/frc/team3128/commands/CmdAutoBalance.java @@ -65,7 +65,7 @@ public void execute() { return; } - swerve.drive(new Translation2d(onRamp ? DRIVE_SPEED * direction * (advAngle > 0.0 ? 1.0 : -1.0) : SwerveConstants.maxSpeed * direction, 0), 0, false); + swerve.drive(new Translation2d(onRamp ? DRIVE_SPEED * (advAngle > 0.0 ? 1.0 : -1.0) : SwerveConstants.maxSpeed * direction, 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 e1b8568..2ca9da1 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -35,7 +35,7 @@ private static CommandBase score(Position position, int xPos, boolean runImmedia runOnce(()-> ENABLE = runImmediately), waitUntil(()-> ENABLE), runOnce(()-> ENABLE = !runImmediately), - runOnce(()-> Swerve.getInstance().throttle = 0.6), + runOnce(()-> Swerve.getInstance().throttle = 0.4), //either(none(), new CmdTrajectory(xPos), ()-> runImmediately), //waitUntil(()-> ENABLE), extend(position), diff --git a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java index d083a39..58d180f 100644 --- a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java +++ b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java @@ -59,8 +59,10 @@ public void execute() { else { translation = translation.rotateBy(Rotation2d.fromDegrees(-90)); } + + final double zValue = -zAxis.getAsDouble(); - rotation = -zAxis.getAsDouble() * maxAngularVelocity * swerve.throttle; + rotation = Math.copySign(Math.pow(zValue, 3/2), zValue) * maxAngularVelocity * swerve.throttle; if (Math.abs(rotation) > maxAngularVelocity * swerve.throttle / 4.0) { enabled = false; diff --git a/src/main/java/frc/team3128/common/swerveNeo/SwerveModule.java b/src/main/java/frc/team3128/common/swerveNeo/SwerveModule.java index 4443f38..dda2454 100644 --- a/src/main/java/frc/team3128/common/swerveNeo/SwerveModule.java +++ b/src/main/java/frc/team3128/common/swerveNeo/SwerveModule.java @@ -171,6 +171,10 @@ public void stop() { angleMotor.set(0); } + public void setBrakeMode(boolean isBrake) { + driveMotor.setIdleMode(isBrake ? IdleMode.kBrake : IdleMode.kCoast); + } + /** * Configures the CANCoder */ diff --git a/src/main/java/frc/team3128/subsystems/Swerve.java b/src/main/java/frc/team3128/subsystems/Swerve.java index fba952f..c850b14 100644 --- a/src/main/java/frc/team3128/subsystems/Swerve.java +++ b/src/main/java/frc/team3128/subsystems/Swerve.java @@ -95,6 +95,12 @@ public void stop() { } } + public void setBrakeMode(boolean isBrake) { + for (final SwerveModule module : modules) { + module.setBrakeMode(isBrake); + } + } + public void initShuffleboard() { // General Tab NAR_Shuffleboard.addComplex("General","Gyro",gyro,7,2,2,2);//.withWidget("Gyro");