diff --git a/.gitignore b/.gitignore index 911e0098..0f84bd58 100644 --- a/.gitignore +++ b/.gitignore @@ -162,8 +162,7 @@ bin/ *-window.json # Logs -*.wpilog *.log #ctre simulation -/ctre_sim \ No newline at end of file +/ctre_sim diff --git a/logs/FRC_20240928_015141.wpilog b/logs/FRC_20240928_015141.wpilog new file mode 100644 index 00000000..4f9eef65 Binary files /dev/null and b/logs/FRC_20240928_015141.wpilog differ diff --git a/logs/FRC_20240928_015311.wpilog b/logs/FRC_20240928_015311.wpilog new file mode 100644 index 00000000..96f9547f Binary files /dev/null and b/logs/FRC_20240928_015311.wpilog differ diff --git a/logs/FRC_20240928_015548.wpilog b/logs/FRC_20240928_015548.wpilog new file mode 100644 index 00000000..5309293e Binary files /dev/null and b/logs/FRC_20240928_015548.wpilog differ diff --git a/logs/FRC_20240928_020420.wpilog b/logs/FRC_20240928_020420.wpilog new file mode 100644 index 00000000..bee54997 Binary files /dev/null and b/logs/FRC_20240928_020420.wpilog differ diff --git a/logs/FRC_20240928_020932.wpilog b/logs/FRC_20240928_020932.wpilog new file mode 100644 index 00000000..f9b9d9d4 Binary files /dev/null and b/logs/FRC_20240928_020932.wpilog differ diff --git a/logs/FRC_20240928_021120.wpilog b/logs/FRC_20240928_021120.wpilog new file mode 100644 index 00000000..a987ea48 Binary files /dev/null and b/logs/FRC_20240928_021120.wpilog differ diff --git a/logs/FRC_20240928_022930__P5.wpilog b/logs/FRC_20240928_022930__P5.wpilog new file mode 100644 index 00000000..bacd6581 Binary files /dev/null and b/logs/FRC_20240928_022930__P5.wpilog differ diff --git a/logs/FRC_20240928_022955__P5.wpilog b/logs/FRC_20240928_022955__P5.wpilog new file mode 100644 index 00000000..72ab2187 Binary files /dev/null and b/logs/FRC_20240928_022955__P5.wpilog differ diff --git a/logs/FRC_20240928_025507.wpilog b/logs/FRC_20240928_025507.wpilog new file mode 100644 index 00000000..7a14e2d1 Binary files /dev/null and b/logs/FRC_20240928_025507.wpilog differ diff --git a/logs/FRC_20240928_030325.wpilog b/logs/FRC_20240928_030325.wpilog new file mode 100644 index 00000000..ad163b38 Binary files /dev/null and b/logs/FRC_20240928_030325.wpilog differ diff --git a/logs/FRC_20240928_032610.wpilog b/logs/FRC_20240928_032610.wpilog new file mode 100644 index 00000000..2e297877 Binary files /dev/null and b/logs/FRC_20240928_032610.wpilog differ diff --git a/logs/FRC_20240928_033507.wpilog b/logs/FRC_20240928_033507.wpilog new file mode 100644 index 00000000..17861c3a Binary files /dev/null and b/logs/FRC_20240928_033507.wpilog differ diff --git a/logs/FRC_20240928_035115.wpilog b/logs/FRC_20240928_035115.wpilog new file mode 100644 index 00000000..a30b626a Binary files /dev/null and b/logs/FRC_20240928_035115.wpilog differ diff --git a/logs/FRC_20240928_041159.wpilog b/logs/FRC_20240928_041159.wpilog new file mode 100644 index 00000000..da08985f Binary files /dev/null and b/logs/FRC_20240928_041159.wpilog differ diff --git a/logs/FRC_20240928_172839__Q6.wpilog b/logs/FRC_20240928_172839__Q6.wpilog new file mode 100644 index 00000000..a3506111 Binary files /dev/null and b/logs/FRC_20240928_172839__Q6.wpilog differ diff --git a/logs/FRC_20240928_183325__Q13.wpilog b/logs/FRC_20240928_183325__Q13.wpilog new file mode 100644 index 00000000..f91216d3 Binary files /dev/null and b/logs/FRC_20240928_183325__Q13.wpilog differ diff --git a/logs/FRC_20240928_195717__Q17.wpilog b/logs/FRC_20240928_195717__Q17.wpilog new file mode 100644 index 00000000..76e7f669 Binary files /dev/null and b/logs/FRC_20240928_195717__Q17.wpilog differ diff --git a/logs/FRC_20240928_205012__Q23.wpilog b/logs/FRC_20240928_205012__Q23.wpilog new file mode 100644 index 00000000..16e4ff87 Binary files /dev/null and b/logs/FRC_20240928_205012__Q23.wpilog differ diff --git a/logs/FRC_20240928_224635__Q36.wpilog b/logs/FRC_20240928_224635__Q36.wpilog new file mode 100644 index 00000000..6e6c3da6 Binary files /dev/null and b/logs/FRC_20240928_224635__Q36.wpilog differ diff --git a/logs/FRC_20240928_232742__Q41.wpilog b/logs/FRC_20240928_232742__Q41.wpilog new file mode 100644 index 00000000..d19353e3 Binary files /dev/null and b/logs/FRC_20240928_232742__Q41.wpilog differ diff --git a/logs/FRC_20240929_001527__Q47.wpilog b/logs/FRC_20240929_001527__Q47.wpilog new file mode 100644 index 00000000..99236401 Binary files /dev/null and b/logs/FRC_20240929_001527__Q47.wpilog differ diff --git a/logs/FRC_20240929_012233__Q55.wpilog b/logs/FRC_20240929_012233__Q55.wpilog new file mode 100644 index 00000000..ce00555d Binary files /dev/null and b/logs/FRC_20240929_012233__Q55.wpilog differ diff --git a/logs/FRC_20240929_170658__Q65.wpilog b/logs/FRC_20240929_170658__Q65.wpilog new file mode 100644 index 00000000..aa125f0c Binary files /dev/null and b/logs/FRC_20240929_170658__Q65.wpilog differ diff --git a/logs/FRC_20240929_173844__Q69.wpilog b/logs/FRC_20240929_173844__Q69.wpilog new file mode 100644 index 00000000..d166a22f Binary files /dev/null and b/logs/FRC_20240929_173844__Q69.wpilog differ diff --git a/src/main/deploy/pathplanner/autos/ADEF BLUE.auto b/src/main/deploy/pathplanner/autos/ADEF BLUE.auto index 0c9edd0e..85bfab1d 100644 --- a/src/main/deploy/pathplanner/autos/ADEF BLUE.auto +++ b/src/main/deploy/pathplanner/autos/ADEF BLUE.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3334750274411264, - "y": 7.002978151231599 + "x": 1.3901542060896621, + "y": 6.569495408336055 }, - "rotation": 41.18642861701727 + "rotation": -0.16089256168708133 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/ADEF RED.auto b/src/main/deploy/pathplanner/autos/ADEF RED.auto index 4ba3fb38..2d07efe2 100644 --- a/src/main/deploy/pathplanner/autos/ADEF RED.auto +++ b/src/main/deploy/pathplanner/autos/ADEF RED.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3334750274411264, - "y": 7.002978151231599 + "x": 1.3824623752298102, + "y": 6.555650112788321 }, - "rotation": 0 + "rotation": 41.05050977995274 }, "command": { "type": "sequential", @@ -46,12 +46,6 @@ "data": { "pathName": "Red E Shoot to F" } - }, - { - "type": "path", - "data": { - "pathName": "Red F to Shoot" - } } ] } diff --git a/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path b/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path index 3e8ae4ee..b6940e23 100644 --- a/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path +++ b/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path @@ -16,32 +16,32 @@ }, { "anchor": { - "x": 2.6938714628917357, - "y": 5.631284339205584 + "x": 3.35, + "y": 5.75 }, "prevControl": { - "x": 2.6854239726791893, - "y": 5.63537084593235 + "x": 3.3415525097874537, + "y": 5.754086506726766 }, "nextControl": { - "x": 2.702318953104282, - "y": 5.627197832478817 + "x": 3.3584474902125465, + "y": 5.745913493273234 }, "isLocked": false, "linkedName": "Blue BCA B" }, { "anchor": { - "x": 2.8299207212253714, - "y": 4.577599659291581 + "x": 2.9299999999999997, + "y": 4.73 }, "prevControl": { - "x": 2.8213830994377913, - "y": 4.581607916922298 + "x": 2.9214623782124196, + "y": 4.734008257630718 }, "nextControl": { - "x": 2.8384583430129515, - "y": 4.573591401660863 + "x": 2.93853762178758, + "y": 4.725991742369283 }, "isLocked": false, "linkedName": "Blue BCA C" diff --git a/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path b/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path new file mode 100644 index 00000000..9aff4c04 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7023114046228107, + "y": 6.739121939782341 + }, + "prevControl": null, + "nextControl": { + "x": 1.702311404622812, + "y": 6.739121939782341 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.496814123442796, + "y": 7.4598028796628215 + }, + "prevControl": { + "x": 1.496814123442796, + "y": 7.4598028796628215 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Misc", + "previewStartingState": { + "rotation": 60.94539590092292, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue B to Center.path b/src/main/deploy/pathplanner/paths/Blue B to Center.path index 6bcc666e..304af5e8 100644 --- a/src/main/deploy/pathplanner/paths/Blue B to Center.path +++ b/src/main/deploy/pathplanner/paths/Blue B to Center.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.6938714628917357, - "y": 5.631284339205584 + "x": 3.35, + "y": 5.75 }, "prevControl": null, "nextControl": { - "x": 2.113811268173132, - "y": 5.556096692550529 + "x": 2.769939805281396, + "y": 5.6748123533449455 }, "isLocked": false, "linkedName": "Blue BCA B" diff --git a/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path b/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path index eed01821..2c95fa67 100644 --- a/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path +++ b/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.8299207212253714, - "y": 4.577599659291581 + "x": 2.9299999999999997, + "y": 4.73 }, "prevControl": null, "nextControl": { - "x": 2.454095504946963, - "y": 6.165533422800476 + "x": 2.5541747837215913, + "y": 6.317933763508896 }, "isLocked": false, "linkedName": "Blue BCA C" diff --git a/src/main/deploy/pathplanner/paths/Blue Center to B.path b/src/main/deploy/pathplanner/paths/Blue Center to B.path index a6f83ffd..e2497fd3 100644 --- a/src/main/deploy/pathplanner/paths/Blue Center to B.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to B.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.5588456716347956, + "x": 1.56, "y": 5.5301848373414 }, "prevControl": null, "nextControl": { - "x": 2.401533816274478, + "x": 2.4026881446396824, "y": 5.599459389022944 }, "isLocked": false, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.6938714628917357, - "y": 5.631284339205584 + "x": 3.35, + "y": 5.75 }, "prevControl": { - "x": 1.848971916879846, - "y": 5.5467703476329575 + "x": 2.5051004539881103, + "y": 5.665486008427374 }, "nextControl": null, "isLocked": false, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/Blue Center to C.path b/src/main/deploy/pathplanner/paths/Blue Center to C.path index e6ae60d1..1b56b1fb 100644 --- a/src/main/deploy/pathplanner/paths/Blue Center to C.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to C.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.8299207212253714, - "y": 4.577599659291581 + "x": 2.9299999999999997, + "y": 4.73 }, "prevControl": { - "x": 2.111455644972302, - "y": 5.35043636493523 + "x": 2.2115349237469304, + "y": 5.50283670564365 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue D Shoot to E.path b/src/main/deploy/pathplanner/paths/Blue D Shoot to E.path index bc6114c7..899ce777 100644 --- a/src/main/deploy/pathplanner/paths/Blue D Shoot to E.path +++ b/src/main/deploy/pathplanner/paths/Blue D Shoot to E.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 1.6981417438618351, + "y": 3.7772118849081013 }, "prevControl": null, "nextControl": { - "x": 6.579303673860324, - "y": 6.340999957855571 + "x": 3.433370711901616, + "y": 3.776683071536486 }, "isLocked": false, "linkedName": "Blue Midline Shoot Pose" diff --git a/src/main/deploy/pathplanner/paths/Blue D to Shoot.path b/src/main/deploy/pathplanner/paths/Blue D to Shoot.path index 1af267be..5d3f95ac 100644 --- a/src/main/deploy/pathplanner/paths/Blue D to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue D to Shoot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 1.6981417438618351, + "y": 3.7772118849081013 }, "prevControl": { - "x": 7.70048416956953, - "y": 7.366224863588121 + "x": 4.554551207610822, + "y": 4.801907977269035 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue E Shoot to F.path b/src/main/deploy/pathplanner/paths/Blue E Shoot to F.path index 98be21fe..60c8de9a 100644 --- a/src/main/deploy/pathplanner/paths/Blue E Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/Blue E Shoot to F.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 1.6981417438618351, + "y": 3.7772118849081013 }, "prevControl": null, "nextControl": { - "x": 7.865711885269925, - "y": 7.084464938160646 + "x": 4.719778923311217, + "y": 4.520148051841561 }, "isLocked": false, "linkedName": "Blue Midline Shoot Pose" }, { "anchor": { - "x": 9.32365277676984, - "y": 4.877773358597312 + "x": 8.282659617774593, + "y": 4.07671724848658 }, "prevControl": { - "x": 6.422771590392083, - "y": 4.690301445191947 + "x": 5.381778431396835, + "y": 3.889245335081215 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue E to Shoot.path b/src/main/deploy/pathplanner/paths/Blue E to Shoot.path index 31864c75..e5c2110d 100644 --- a/src/main/deploy/pathplanner/paths/Blue E to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue E to Shoot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 1.6981417438618351, + "y": 3.7772118849081013 }, "prevControl": { - "x": 5.895884501962457, - "y": 6.343980542313763 + "x": 2.7499515400037486, + "y": 3.779663655994678 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue F to Shoot.path b/src/main/deploy/pathplanner/paths/Blue F to Shoot.path index e4d79aa6..48d4a4e7 100644 --- a/src/main/deploy/pathplanner/paths/Blue F to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue F to Shoot.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 9.32365277676984, - "y": 4.877773358597312 + "x": 8.282659617774593, + "y": 4.07671724848658 }, "prevControl": null, "nextControl": { - "x": 6.497337601260025, - "y": 6.27989797646262 + "x": 4.738621898896219, + "y": 3.88245927489006 }, "isLocked": false, "linkedName": "Blue F" }, { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 4.24738092686751, + "y": 5.167803455956615 }, "prevControl": { - "x": 6.271245847923244, - "y": 6.380372517069439 + "x": 5.436537977800659, + "y": 4.006336996118933 }, "nextControl": null, "isLocked": false, - "linkedName": "Blue Midline Shoot Pose" + "linkedName": "Blue F Shoot" } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 11.738504411842449, + "rotation": -2.200416275819262, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Blue G Shoot to F.path b/src/main/deploy/pathplanner/paths/Blue G Shoot to F.path index c2a68084..5f789a4e 100644 --- a/src/main/deploy/pathplanner/paths/Blue G Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/Blue G Shoot to F.path @@ -3,32 +3,38 @@ "waypoints": [ { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 1.6981417438618351, + "y": 3.7772118849081013 }, "prevControl": null, "nextControl": { - "x": 6.712180051964271, - "y": 6.800875295388985 + "x": 1.0549632495276386, + "y": 1.204497907571314 }, "isLocked": false, "linkedName": "Blue Midline Shoot Pose" }, { "anchor": { - "x": 9.32365277676984, - "y": 4.877773358597312 + "x": 8.282659617774593, + "y": 4.07671724848658 }, "prevControl": { - "x": 8.60893746806095, - "y": 4.491979967032851 + "x": 8.281950694955519, + "y": 0.5730135676795581 }, "nextControl": null, "isLocked": false, "linkedName": "Blue F" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 26.187378361039833, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -39,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -1.0333876477886335, + "rotation": 61.3046916104419, "rotateFast": false }, "reversed": false, "folder": "Blue To Notes", "previewStartingState": { - "rotation": 5.880944330975369, + "rotation": -29.559383670629433, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Blue G to Shoot.path b/src/main/deploy/pathplanner/paths/Blue G to Shoot.path index 09286e45..7c923986 100644 --- a/src/main/deploy/pathplanner/paths/Blue G to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue G to Shoot.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 9.17904635660462, - "y": 3.0775484197917797 + "x": 8.304629409668046, + "y": 2.4540293807150113 }, "prevControl": null, "nextControl": { - "x": 8.083681568218793, - "y": 4.805277852743206 + "x": 4.57490373670206, + "y": 0.058470772212200116 }, "isLocked": false, "linkedName": "Blue G" }, { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 1.6981417438618351, + "y": 3.7772118849081013 }, "prevControl": { - "x": 6.4047471872845545, - "y": 6.780924609096242 + "x": 1.5695060449949956, + "y": 2.970315228379746 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 7.509278390683297, + "rotation": -44.999999999999986, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Blue H Shoot to G.path b/src/main/deploy/pathplanner/paths/Blue H Shoot to G.path index 6e6dc490..d41636f8 100644 --- a/src/main/deploy/pathplanner/paths/Blue H Shoot to G.path +++ b/src/main/deploy/pathplanner/paths/Blue H Shoot to G.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 1.6981417438618351, + "y": 3.7772118849081013 }, "prevControl": null, "nextControl": { - "x": 7.574338143467954, - "y": 7.5179462272987045 + "x": 1.8964467582174, + "y": 2.4450348539245748 }, "isLocked": false, "linkedName": "Blue Midline Shoot Pose" }, { "anchor": { - "x": 9.17904635660462, - "y": 3.0775484197917797 + "x": 4.703539435568899, + "y": 1.5436283864020728 }, "prevControl": { - "x": 7.344448548644122, - "y": 5.242269993468559 + "x": 3.166637287239808, + "y": 2.001625667856766 + }, + "nextControl": { + "x": 6.003480742048733, + "y": 1.1562455038511537 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.304629409668046, + "y": 2.4540293807150113 + }, + "prevControl": { + "x": 6.30563859418317, + "y": 1.6722640852689115 }, "nextControl": null, "isLocked": false, @@ -39,13 +55,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -53.224027189600015, + "rotation": 31.29378192563241, "rotateFast": false }, "reversed": false, "folder": "Blue To Notes", "previewStartingState": { - "rotation": 4.704435901357846, + "rotation": -32.58720422762288, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Blue H to Shoot.path b/src/main/deploy/pathplanner/paths/Blue H to Shoot.path index 2328680d..9b3c7cf5 100644 --- a/src/main/deploy/pathplanner/paths/Blue H to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue H to Shoot.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 7.1779684844711955, - "y": 2.4336941028792762 + "x": 4.060360941234703, + "y": 1.3448277608805934 }, "isLocked": false, "linkedName": "Blue H" }, { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 1.6981417438618351, + "y": 3.7772118849081013 }, "prevControl": { - "x": 6.985047751717528, - "y": 7.12657625836085 + "x": 1.1635114251592336, + "y": 3.05860258682641 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 7.957397728998733, + "rotation": -48.990913098429836, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path b/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path index 2786ac73..feaea618 100644 --- a/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path +++ b/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path @@ -32,32 +32,32 @@ }, { "anchor": { - "x": 9.32365277676984, - "y": 4.877773358597312 + "x": 8.282659617774593, + "y": 4.07671724848658 }, "prevControl": { - "x": 9.414244371346676, - "y": 4.832506665139351 + "x": 8.373251212351429, + "y": 4.031450555028619 }, "nextControl": { - "x": 9.233061182193005, - "y": 4.923040052055273 + "x": 8.192068023197757, + "y": 4.121983941944541 }, "isLocked": false, "linkedName": "Blue F" }, { "anchor": { - "x": 9.17904635660462, - "y": 3.0775484197917797 + "x": 8.304629409668046, + "y": 2.4540293807150113 }, "prevControl": { - "x": 9.268939459250493, - "y": 3.0326950007263043 + "x": 8.39452251231392, + "y": 2.409175961649536 }, "nextControl": { - "x": 9.089153253958745, - "y": 3.122401838857255 + "x": 8.214736307022172, + "y": 2.4988827997804868 }, "isLocked": false, "linkedName": "Blue G" @@ -96,16 +96,16 @@ }, { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 1.6981417438618351, + "y": 3.7772118849081013 }, "prevControl": { - "x": 4.901691893320543, - "y": 6.315771935289686 + "x": 1.7557589313618351, + "y": 3.7514550489706013 }, "nextControl": { - "x": 4.786457518320543, - "y": 6.367285607164686 + "x": 1.6405245563618351, + "y": 3.8029687208456013 }, "isLocked": false, "linkedName": "Blue Midline Shoot Pose" diff --git a/src/main/deploy/pathplanner/paths/Blue Source Side Mobility.path b/src/main/deploy/pathplanner/paths/Blue Source Side Mobility.path new file mode 100644 index 00000000..20c43361 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Source Side Mobility.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7319122330552369, + "y": 4.351321779566637 + }, + "prevControl": null, + "nextControl": { + "x": 1.5903362575955933, + "y": 2.8614134151345225 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.563724819757333, + "y": 1.4405736503780713 + }, + "prevControl": { + "x": 2.563724819757333, + "y": 1.4405736503780713 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Misc", + "previewStartingState": { + "rotation": -58.91249467652, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Source to H.path b/src/main/deploy/pathplanner/paths/Blue Source to H.path index 7ab4d6bf..54a50d03 100644 --- a/src/main/deploy/pathplanner/paths/Blue Source to H.path +++ b/src/main/deploy/pathplanner/paths/Blue Source to H.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 4.366652379138092, - "y": 1.9121879705812976 + "x": 3.9200310879254237, + "y": 0.7367317298737166 }, "isLocked": false, "linkedName": "Blue HGF Starting Pose" diff --git a/src/main/deploy/pathplanner/paths/Red A to D.path b/src/main/deploy/pathplanner/paths/Red A to D.path index f1b8c633..f4cb4581 100644 --- a/src/main/deploy/pathplanner/paths/Red A to D.path +++ b/src/main/deploy/pathplanner/paths/Red A to D.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 8.876949699583925, - "y": 7.932247466987493 + "x": 4.387797265623021, + "y": 7.226987445427883 }, "isLocked": false, "linkedName": "Red ADEF A" }, { "anchor": { - "x": 9.583540511947096, - "y": 8.029356831593127 + "x": 8.281950694955519, + "y": 7.472564688719121 }, "prevControl": { - "x": 3.4529590208733993, - "y": 7.195273922727905 + "x": 6.726628154111008, + "y": 7.226987445427883 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path b/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path new file mode 100644 index 00000000..9a5d83d1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7023114046228107, + "y": 6.739121939782341 + }, + "prevControl": null, + "nextControl": { + "x": 1.702311404622812, + "y": 6.739121939782341 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.4851032868970147, + "y": 7.436381206571257 + }, + "prevControl": { + "x": 1.4851032868970147, + "y": 7.436381206571257 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Misc", + "previewStartingState": { + "rotation": 60.94539590092292, + "velocity": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Amp to A.path b/src/main/deploy/pathplanner/paths/Red Amp to A.path index bee56d65..93875fdf 100644 --- a/src/main/deploy/pathplanner/paths/Red Amp to A.path +++ b/src/main/deploy/pathplanner/paths/Red Amp to A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3824623752298102, - "y": 6.555650112788321 + "x": 1.2654580294915574, + "y": 6.817692039942485 }, "prevControl": null, "nextControl": { - "x": 2.759059729428983, - "y": 7.0723488507988925 + "x": 2.446567628177991, + "y": 7.0398809743488435 }, "isLocked": false, "linkedName": "Red ADEF Starting Pose" @@ -20,8 +20,8 @@ "y": 7.146094279167726 }, "prevControl": { - "x": 1.6141307059399834, - "y": 6.624203555326754 + "x": 2.189296230444312, + "y": 6.993104356579084 }, "nextControl": null, "isLocked": false, @@ -39,11 +39,14 @@ }, "goalEndState": { "velocity": 0, - "rotation": 34.32633732715843, + "rotation": 1.0605910278742103, "rotateFast": false }, "reversed": false, "folder": "Red To Notes", - "previewStartingState": null, + "previewStartingState": { + "rotation": 32.32312883475656, + "velocity": 0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Center to B.path b/src/main/deploy/pathplanner/paths/Red Center to B.path index 5709f4d3..85c17027 100644 --- a/src/main/deploy/pathplanner/paths/Red Center to B.path +++ b/src/main/deploy/pathplanner/paths/Red Center to B.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/Red D Shoot to E.path b/src/main/deploy/pathplanner/paths/Red D Shoot to E.path index c4804438..6264615c 100644 --- a/src/main/deploy/pathplanner/paths/Red D Shoot to E.path +++ b/src/main/deploy/pathplanner/paths/Red D Shoot to E.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 4.457962192277661, + "y": 6.455173252226847 }, "prevControl": null, "nextControl": { - "x": 6.678528291952418, - "y": 6.478106842932436 + "x": 6.292415778409535, + "y": 6.591751323932097 }, "isLocked": false, "linkedName": "Red Midline Shoot Pose" diff --git a/src/main/deploy/pathplanner/paths/Red D to Shoot.path b/src/main/deploy/pathplanner/paths/Red D to Shoot.path index 205b4b09..d0421b39 100644 --- a/src/main/deploy/pathplanner/paths/Red D to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Red D to Shoot.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 9.583540511947096, - "y": 8.029356831593127 + "x": 8.281950694955519, + "y": 7.472564688719121 }, "prevControl": null, "nextControl": { - "x": 5.792092859297326, - "y": 6.64574068173434 + "x": 6.1653087408738925, + "y": 6.946327738809324 }, "isLocked": false, "linkedName": "Red D" }, { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 4.457962192277661, + "y": 6.455173252226847 }, "prevControl": { - "x": 6.9995180085226245, - "y": 7.030476446555571 + "x": 6.613405494979742, + "y": 7.144120927555232 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Red E Shoot to F.path b/src/main/deploy/pathplanner/paths/Red E Shoot to F.path index cc97361d..c5678b47 100644 --- a/src/main/deploy/pathplanner/paths/Red E Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/Red E Shoot to F.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 4.457962192277661, + "y": 6.455173252226847 }, "prevControl": null, "nextControl": { - "x": 5.985494331479735, - "y": 6.6556919879092735 + "x": 5.599381817936853, + "y": 6.769336468908934 }, "isLocked": false, "linkedName": "Red Midline Shoot Pose" }, { "anchor": { - "x": 9.32365277676984, - "y": 4.877773358597312 + "x": 8.285159462804046, + "y": 4.107340350097367 }, "prevControl": { - "x": 7.457228766578071, - "y": 5.583772188191697 + "x": 7.942820216124693, + "y": 6.3148433989167545 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -26.70800563120147, + "rotation": -76.9119463322888, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Red E to Shoot.path b/src/main/deploy/pathplanner/paths/Red E to Shoot.path index f791ec24..0f57fae0 100644 --- a/src/main/deploy/pathplanner/paths/Red E to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Red E to Shoot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 4.457962192277661, + "y": 6.455173252226847 }, "prevControl": { - "x": 6.214855112931953, - "y": 6.6096371506359075 + "x": 5.82874259938907, + "y": 6.723281631635568 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Red F to Shoot.path b/src/main/deploy/pathplanner/paths/Red F to Shoot.path index 8271d6a1..93943f82 100644 --- a/src/main/deploy/pathplanner/paths/Red F to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Red F to Shoot.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 9.32365277676984, - "y": 4.877773358597312 + "x": 8.285159462804046, + "y": 4.107340350097367 }, "prevControl": null, "nextControl": { - "x": 6.064864411164835, - "y": 6.154521133447029 + "x": 5.974337717193953, + "y": 4.201266592348 }, "isLocked": false, "linkedName": "Red F" }, { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 4.20100142052136, + "y": 4.993335426485668 }, "prevControl": { - "x": 6.927118650554267, - "y": 6.5047398072846745 + "x": 4.908506900347135, + "y": 3.754696069577325 }, "nextControl": null, "isLocked": false, - "linkedName": "Red Midline Shoot Pose" + "linkedName": "Red F to Shoot" } ], "rotationTargets": [], @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 13.775816001890945, + "rotation": -3.3898658008179314, "rotateFast": false }, "reversed": false, "folder": "Red To Shoots", "previewStartingState": { - "rotation": -35.112332977419605, + "rotation": 2.5273065948629965, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Red G Shoot to F.path b/src/main/deploy/pathplanner/paths/Red G Shoot to F.path index 9fa76c14..ccad7a45 100644 --- a/src/main/deploy/pathplanner/paths/Red G Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/Red G Shoot to F.path @@ -3,32 +3,38 @@ "waypoints": [ { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 4.457962192277661, + "y": 6.455173252226847 }, "prevControl": null, "nextControl": { - "x": 6.737563093801783, - "y": 6.744051894911826 + "x": 5.5803445365587185, + "y": 3.6850084419939515 }, "isLocked": false, "linkedName": "Red Midline Shoot Pose" }, { "anchor": { - "x": 9.32365277676984, - "y": 4.877773358597312 + "x": 8.285159462804046, + "y": 4.107340350097367 }, "prevControl": { - "x": 8.698835741235472, - "y": 4.399629922771751 + "x": 5.386156485668626, + "y": 4.755617469754285 }, "nextControl": null, "isLocked": false, "linkedName": "Red F" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 40.376097780771914, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/Red G to Shoot.path b/src/main/deploy/pathplanner/paths/Red G to Shoot.path index 025c558d..645bb842 100644 --- a/src/main/deploy/pathplanner/paths/Red G to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Red G to Shoot.path @@ -3,32 +3,38 @@ "waypoints": [ { "anchor": { - "x": 9.17904635660462, - "y": 3.0775484197917797 + "x": 8.294678103493114, + "y": 2.4289828564776177 }, "prevControl": null, "nextControl": { - "x": 7.379638674827945, - "y": 5.380722948945898 + "x": 3.4927161716302604, + "y": 0.23777254027722292 }, "isLocked": false, "linkedName": "Red G" }, { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 4.457962192277661, + "y": 6.455173252226847 }, "prevControl": { - "x": 6.933704780728014, - "y": 6.492384803966036 + "x": 3.499848510797321, + "y": 5.82059720628904 }, "nextControl": null, "isLocked": false, "linkedName": "Red Midline Shoot Pose" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.1, + "rotationDegrees": 22.226404773936004, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -39,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 10.292841181604183, + "rotation": -26.32471794101153, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Red H Shoot to G.path b/src/main/deploy/pathplanner/paths/Red H Shoot to G.path index 343945d2..3561dc5e 100644 --- a/src/main/deploy/pathplanner/paths/Red H Shoot to G.path +++ b/src/main/deploy/pathplanner/paths/Red H Shoot to G.path @@ -3,32 +3,38 @@ "waypoints": [ { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 4.457962192277661, + "y": 6.455173252226847 }, "prevControl": null, "nextControl": { - "x": 6.3702300963009675, - "y": 6.766887017777012 + "x": 4.181825464408966, + "y": 4.857147317149678 }, "isLocked": false, "linkedName": "Red Midline Shoot Pose" }, { "anchor": { - "x": 9.17904635660462, - "y": 3.0775484197917797 + "x": 8.294678103493114, + "y": 2.4289828564776177 }, "prevControl": { - "x": 7.9253260003915855, - "y": 4.7072550832229645 + "x": 7.393194957330086, + "y": 3.0755626183617046 }, "nextControl": null, "isLocked": false, "linkedName": "Red G" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -14.569105316429313, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -39,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -45.01939380931683, + "rotation": 40.60084159575439, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Red H to Shoot.path b/src/main/deploy/pathplanner/paths/Red H to Shoot.path index a5748f47..12aa3dbe 100644 --- a/src/main/deploy/pathplanner/paths/Red H to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Red H to Shoot.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 9.08448491097131, - "y": 1.3743847716490074 + "x": 8.310109839155691, + "y": 0.8097082386501073 }, "prevControl": null, "nextControl": { - "x": 7.278971838449629, - "y": 4.65908499246314 + "x": 2.2594611570989525, + "y": 0.7835083476434771 }, "isLocked": false, "linkedName": "Red H" }, { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 4.457962192277661, + "y": 6.455173252226847 }, "prevControl": { - "x": 7.026968229903722, - "y": 6.600647323318456 + "x": 4.808786825550858, + "y": 5.601499977928731 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 9.740747376516406, + "rotation": -28.8131676274699, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Red Midline Waypoints.path b/src/main/deploy/pathplanner/paths/Red Midline Waypoints.path index 617d74ee..d8db779d 100644 --- a/src/main/deploy/pathplanner/paths/Red Midline Waypoints.path +++ b/src/main/deploy/pathplanner/paths/Red Midline Waypoints.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 9.583540511947096, - "y": 8.029356831593127 + "x": 8.281950694955519, + "y": 7.472564688719121 }, "prevControl": null, "nextControl": { - "x": 10.583540511947096, - "y": 7.529356831593127 + "x": 9.281950694955519, + "y": 6.972564688719121 }, "isLocked": false, "linkedName": "Red D" @@ -32,80 +32,80 @@ }, { "anchor": { - "x": 9.32365277676984, - "y": 4.877773358597312 + "x": 8.285159462804046, + "y": 4.107340350097367 }, "prevControl": { - "x": 9.414462650075102, - "y": 4.832377516891699 + "x": 8.375969336109307, + "y": 4.061944508391754 }, "nextControl": { - "x": 9.23284290346458, - "y": 4.923169200302925 + "x": 8.194349589498785, + "y": 4.15273619180298 }, "isLocked": false, "linkedName": "Red F" }, { "anchor": { - "x": 9.17904635660462, - "y": 3.0775484197917797 + "x": 8.294678103493114, + "y": 2.4289828564776177 }, "prevControl": { - "x": 9.269637951181455, - "y": 3.0322817263338186 + "x": 8.38526969806995, + "y": 2.3837161630196566 }, "nextControl": { - "x": 9.088454762027784, - "y": 3.122815113249741 + "x": 8.204086508916278, + "y": 2.4742495499355788 }, "isLocked": false, "linkedName": "Red G" }, { "anchor": { - "x": 9.08448491097131, - "y": 1.3743847716490074 + "x": 8.310109839155691, + "y": 0.8097082386501073 }, "prevControl": { - "x": 9.174378013617185, - "y": 1.3295313525835315 + "x": 8.400002941801565, + "y": 0.7648548195846314 }, "nextControl": { - "x": 8.994591808325437, - "y": 1.4192381907144824 + "x": 8.220216736509817, + "y": 0.8545616577155823 }, "isLocked": false, "linkedName": "Red H" }, { "anchor": { - "x": 1.3824623752298102, - "y": 6.555650112788321 + "x": 1.2654580294915574, + "y": 6.817692039942485 }, "prevControl": { - "x": 1.4701203036966073, - "y": 6.512119171778799 + "x": 1.353115957958355, + "y": 6.7741610989329635 }, "nextControl": { - "x": 1.2948044467630135, - "y": 6.599181053797842 + "x": 1.177800101024761, + "y": 6.8612229809520064 }, "isLocked": false, "linkedName": "Red ADEF Starting Pose" }, { "anchor": { - "x": 4.844074705820543, - "y": 6.341528771227186 + "x": 4.457962192277661, + "y": 6.455173252226847 }, "prevControl": { - "x": 4.924580076914293, - "y": 6.302229759996718 + "x": 4.538467563371411, + "y": 6.415874240996378 }, "nextControl": { - "x": 4.763569334726793, - "y": 6.380827782457655 + "x": 4.377456821183911, + "y": 6.4944722634573155 }, "isLocked": false, "linkedName": "Red Midline Shoot Pose" diff --git a/src/main/deploy/pathplanner/paths/Red Source Side Mobility.path b/src/main/deploy/pathplanner/paths/Red Source Side Mobility.path new file mode 100644 index 00000000..7b9b8f2d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Source Side Mobility.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7319122330552369, + "y": 4.351321779566637 + }, + "prevControl": null, + "nextControl": { + "x": 1.5903362575955933, + "y": 2.8614134151345225 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.563724819757333, + "y": 1.4405736503780713 + }, + "prevControl": { + "x": 2.563724819757333, + "y": 1.4405736503780713 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Misc", + "previewStartingState": { + "rotation": -58.91249467652, + "velocity": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Source to H.path b/src/main/deploy/pathplanner/paths/Red Source to H.path index aabb5e95..e1d250f4 100644 --- a/src/main/deploy/pathplanner/paths/Red Source to H.path +++ b/src/main/deploy/pathplanner/paths/Red Source to H.path @@ -16,28 +16,12 @@ }, { "anchor": { - "x": 4.368911854453174, - "y": 2.305913562719982 + "x": 8.310109839155691, + "y": 0.8097082386501073 }, "prevControl": { - "x": 3.0634548695792625, - "y": 3.070445327974576 - }, - "nextControl": { - "x": 5.75651814157051, - "y": 1.4932716323765978 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 9.08448491097131, - "y": 1.3743847716490074 - }, - "prevControl": { - "x": 7.065187014488613, - "y": 1.1927133415278737 + "x": 6.290811942672994, + "y": 0.6280368085289736 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index cf302b42..ddbcd264 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -1,9 +1,6 @@ package com.stuypulse.robot; -import java.util.concurrent.atomic.AtomicBoolean; - import com.ctre.phoenix6.Utils; -import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.arm.ArmToAmp; import com.stuypulse.robot.commands.arm.ArmToClimbing; @@ -16,10 +13,10 @@ import com.stuypulse.robot.commands.arm.ArmToSpeaker; import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.auton.CenterMobilityWithWait; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.Mobility; import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF; -import com.stuypulse.robot.commands.auton.BCA.AltFourPieceBCA; import com.stuypulse.robot.commands.auton.BCA.FourPieceBCA; import com.stuypulse.robot.commands.auton.HGF.FourPieceHGF; import com.stuypulse.robot.commands.auton.SideAutons.OnePieceAmpSide; @@ -28,7 +25,6 @@ import com.stuypulse.robot.commands.intake.IntakeSetAcquire; import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.commands.leds.LEDDefaultMode; -import com.stuypulse.robot.commands.leds.LEDReset; import com.stuypulse.robot.commands.leds.LEDSet; import com.stuypulse.robot.commands.shooter.ShooterFeederDeacquire; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; @@ -36,29 +32,25 @@ import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveRobotRelative; -import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedAmp; +import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedFerry; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedManualFerry; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeaker; -import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; +import com.stuypulse.robot.commands.vision.VisionChangeWhiteList; +import com.stuypulse.robot.commands.vision.VisionReloadWhiteList; import com.stuypulse.robot.constants.LEDInstructions; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.input.gamepads.AutoGamepad; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.swerve.Telemetry; import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.robot.util.PathUtil.AutonConfig; -import com.stuypulse.robot.util.ShooterLobFerryInterpolation; -import com.stuypulse.robot.util.ShooterSpeeds; -import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.leds.LEDController; -import com.stuypulse.robot.subsystems.leds.instructions.LEDInstruction; -import com.stuypulse.robot.subsystems.leds.instructions.LEDPulseColor; -import com.stuypulse.robot.subsystems.leds.instructions.LEDRainbow; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -88,7 +80,7 @@ public class RobotContainer { public final LEDController leds = LEDController.getInstance(); - // private final Telemetry logger = new Telemetry(); + private final Telemetry logger = new Telemetry(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); @@ -103,12 +95,13 @@ public RobotContainer() { if (Utils.isSimulation()) { swerve.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0))); } - // swerve.registerTelemetry(logger::telemeterize); + swerve.registerTelemetry(logger::telemeterize); LiveWindow.disableAllTelemetry(); - new Trigger(() -> Intake.getInstance().getState() == Intake.State.ACQUIRING && Intake.getInstance().hasNote()) - .onTrue(new BuzzController(driver, 1)); + new Trigger(() -> Intake.getInstance().getState() == Intake.State.ACQUIRING && Intake.getInstance().hasNote() + || ((driver.getLeftTriggerPressed() || driver.getRightTriggerPressed()) && (Intake.getInstance().hasNote() || Shooter.getInstance().hasNote()))) + .onTrue(new BuzzController(driver, 1, 1)); } /****************/ @@ -149,6 +142,7 @@ private void configureDriverBindings() { // intake robot relative driver.getLeftTriggerButton() + .whileTrue(new SwerveDriveDriveRobotRelative(driver)) .onTrue(new ArmToFeed()) .onTrue(new IntakeSetAcquire()) .whileTrue(new LEDSet(LEDInstructions.ROBOT_RELATIVE_INTAKING)) @@ -165,6 +159,10 @@ private void configureDriverBindings() { // speaker align and score // score amp driver.getRightBumper() + .onTrue(new ConditionalCommand( + new VisionChangeWhiteList(7, 8), + new VisionChangeWhiteList(3, 4), + () -> Robot.isBlue())) .whileTrue(new ConditionalCommand( new SwerveDriveDrive(driver) .alongWith(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) @@ -179,7 +177,8 @@ private void configureDriverBindings() { .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)), () -> Arm.getInstance().getState() == Arm.State.AMP)) .onFalse(new ShooterFeederStop()) - .onFalse(new ArmToFeed().onlyIf(() -> arm.getState() == Arm.State.SPEAKER)); + .onFalse(new ArmToFeed().onlyIf(() -> arm.getState() == Arm.State.SPEAKER)) + .onFalse(new VisionReloadWhiteList()); // lob ferry align and shoot driver.getLeftStickButton() @@ -197,7 +196,7 @@ private void configureDriverBindings() { // low ferry align and shoot - driver.getRightStickButton() + driver.getLeftMenuButton() .whileTrue(new SwerveDriveDriveAlignedFerry(driver) .alongWith(new ArmToLowFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) @@ -253,7 +252,13 @@ private void configureDriverBindings() { .onFalse(new ArmToFeed()); // human player attention button - driver.getRightButton().whileTrue(new LEDSet(LEDInstructions.ATTENTION)); + // driver.getRightButton().whileTrue(new LEDSet(LEDInstructions.ATTENTION)); + + // driver.getRightButton() + // .onTrue(new IntakeDeacquire()) + // .onTrue(new ShooterFeederAcquire()) + // .onFalse(new IntakeStop()) + // .onFalse(new ShooterFeederStop()); } private void configureOperatorBindings() { @@ -272,6 +277,11 @@ public void configureAutons() { AutonConfig MOBILITY_BLUE = new AutonConfig("Mobility", Mobility::new, "Mobility"); AutonConfig MOBILITY_RED = new AutonConfig("Mobility", Mobility::new, "Mobility"); + AutonConfig CENTER_MOBILITY_BLUE = new AutonConfig("Center Mobility", CenterMobilityWithWait::new, + "Mobility"); + AutonConfig CENTER_MOBILITY_RED = new AutonConfig("Center Mobility", CenterMobilityWithWait::new, + "Mobility"); + // BCA AutonConfig BCA_BLUE = new AutonConfig("4 BCA", FourPieceBCA::new, "Blue Center to B", "Blue B to Center", "Blue Center to C", "Blue C to Shoot Before A", "Blue Center to A", "Blue A to Center"); @@ -286,9 +296,29 @@ public void configureAutons() { // ADEF AutonConfig ADEF_BLUE = new AutonConfig("5 ADEF", FivePieceADEF::new, - "Blue Amp to A", "Blue A to D", "Blue D to Shoot", "Blue D Shoot to E", "Blue E to Shoot", "Blue E Shoot to F", "Blue F to Shoot"); + "Blue Amp to A", "Blue A to D", "Blue D to Shoot", "Blue D Shoot to E", "Blue E to Shoot", "Blue E Shoot to F"); AutonConfig ADEF_RED = new AutonConfig("5 ADEF", FivePieceADEF::new, - "Red Amp to A", "Red A to D", "Red D to Shoot", "Red D Shoot to E", "Red E to Shoot", "Red E Shoot to F", "Red F to Shoot"); + "Red Amp to A", "Red A to D", "Red D to Shoot", "Red D Shoot to E", "Red E to Shoot", "Red E Shoot to F"); + + AutonConfig AMP_SIDE_ONE_PIECE_BLUE = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new, + "Blue Amp Side Mobility"); + AutonConfig AMP_SIDE_ONE_PIECE_RED = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new, + "Red Amp Side Mobility"); + + AutonConfig SOURCE_SIDE_ONE_PIECE_BLUE = new AutonConfig("Source Side One Piece", OnePieceSourceSide::new, + "Blue Source Side Mobility"); + + AutonConfig SOURCE_SIDE_ONE_PIECE_RED = new AutonConfig("Source Side One Piece", OnePieceSourceSide::new, + "Red Source Side Mobility"); + + AMP_SIDE_ONE_PIECE_BLUE.registerBlue(autonChooser); + AMP_SIDE_ONE_PIECE_RED.registerRed(autonChooser); + + CENTER_MOBILITY_BLUE.registerBlue(autonChooser); + CENTER_MOBILITY_RED.registerRed(autonChooser); + + SOURCE_SIDE_ONE_PIECE_BLUE.registerBlue(autonChooser); + SOURCE_SIDE_ONE_PIECE_RED.registerRed(autonChooser); MOBILITY_BLUE.registerBlue(autonChooser); MOBILITY_RED.registerRed(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java index 0207f46b..87661416 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java @@ -40,9 +40,9 @@ public FivePieceADEF(PathPlannerPath... paths) { // Drive to F + Shoot F new IntakeSetAcquire(), - SwerveDrive.getInstance().followPathCommand(paths[5]), - new FollowPathThenShoot(paths[6], true), - new ArmToFeed() + SwerveDrive.getInstance().followPathCommand(paths[5]) + // new FollowPathThenShoot(paths[6], true), + // new ArmToFeed() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CenterMobilityWithWait.java b/src/main/java/com/stuypulse/robot/commands/auton/CenterMobilityWithWait.java new file mode 100644 index 00000000..a5e5132e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/CenterMobilityWithWait.java @@ -0,0 +1,22 @@ +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.intake.IntakeSetAcquire; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class CenterMobilityWithWait extends SequentialCommandGroup { + + public CenterMobilityWithWait(PathPlannerPath... paths) { + addCommands( + ShootRoutine.fromSubwoofer(), + new ArmToFeed(), + new WaitCommand(10), + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[0]) + ); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathThenShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathThenShoot.java index 1e1cb728..1d09b11c 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathThenShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathThenShoot.java @@ -24,7 +24,7 @@ public FollowPathThenShoot(PathPlannerPath path, boolean isLastShot) { SwerveDrive.getInstance().followPathCommand(path), new WaitCommand(totalPathTime > 1.5 ? totalPathTime - 1.0 : 0) // wait for handoff - .andThen(new WaitUntilCommand(() -> Shooter.getInstance().hasNote()).onlyIf(() -> Intake.getInstance().hasNote()).alongWith(new WaitCommand(0.75))) + .andThen(new WaitUntilCommand(() -> Shooter.getInstance().hasNote()).withTimeout(3.0).onlyIf(() -> Intake.getInstance().hasNote())) .andThen(new ArmToSpeaker().onlyIf(() -> Shooter.getInstance().hasNote())) ), isLastShot ? ShootRoutine.fromAnywhereLastShot() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java index f7751eaf..1327ea4d 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java @@ -28,16 +28,13 @@ public static Command fromSubwoofer() { new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(1.0)), new ShooterFeederShoot(), - new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()).alongWith(new WaitCommand(0.5)), new ShooterFeederStop() ); } public static Command fromAnywhere() { return new SequentialCommandGroup( - new WaitUntilCommand(() -> Shooter.getInstance().hasNote()) - .onlyIf(() -> Intake.getInstance().hasNote()) - .withTimeout(2.0), new ArmToSpeaker(), new ParallelCommandGroup( new SwerveDriveAlignToSpeaker(), @@ -45,7 +42,7 @@ public static Command fromAnywhere() { new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET) ), new ShooterFeederShoot(), - new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()).alongWith(new WaitCommand(1.25)), new ShooterFeederStop() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java index 5f8102bb..b78cc6fb 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java @@ -3,9 +3,14 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.arm.ArmToFeed; import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.commands.intake.IntakeSetAcquire; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class OnePieceAmpSide extends SequentialCommandGroup { @@ -15,8 +20,13 @@ public OnePieceAmpSide(PathPlannerPath... paths) { ShootRoutine.fromSubwoofer(), new ArmToFeed(), + new WaitCommand(10), + + new IntakeSetAcquire(), // Mobility SwerveDrive.getInstance().followPathCommand(paths[0]) + // new WaitUntilCommand(() -> Shooter.getInstance().hasNote()).andThen(ShootRoutine.fromAnywhere()) + // .onlyIf(() -> Intake.getInstance().hasNote() || Shooter.getInstance().hasNote()) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceSourceSide.java b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceSourceSide.java index 405651c3..56695c90 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceSourceSide.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceSourceSide.java @@ -6,6 +6,7 @@ import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; public class OnePieceSourceSide extends SequentialCommandGroup { @@ -15,6 +16,8 @@ public OnePieceSourceSide(PathPlannerPath... paths) { ShootRoutine.fromSubwoofer(), new ArmToFeed(), + new WaitCommand(10), + // Mobility SwerveDrive.getInstance().followPathCommand(paths[0]) ); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java index d8df3682..c70a8e78 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java @@ -64,8 +64,8 @@ public SwerveDriveDriveRobotRelative(Gamepad driver) { @Override public void execute() { Vector2D velocity = Robot.isBlue() ? speed.get() : speed.get().mul(-1); - swerve.setControl(drive.withVelocityX(velocity.y) - .withVelocityY(-velocity.x) + swerve.setControl(drive.withVelocityX(Robot.isBlue() ? velocity.y : -velocity.y) + .withVelocityY(Robot.isBlue() ? -velocity.x : velocity.x) .withRotationalRate(turn.get()) ); } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 6f1725bb..9d50e04d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -36,7 +36,7 @@ public interface Settings { public interface Arm { double LENGTH = Units.inchesToMeters(16.5); - SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", SAFE_MODE_ENABLED ? 200 : 900); + SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", SAFE_MODE_ENABLED ? 200 : 750); SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", SAFE_MODE_ENABLED ? 200 : 700); SmartNumber MAX_ANGLE = new SmartNumber("Arm/Max Angle (deg)", 85); @@ -154,7 +154,7 @@ public interface Swerve { double MAX_LINEAR_VELOCITY = SAFE_MODE_ENABLED ? 1.0 : 4.9; double MAX_LINEAR_ACCEL = SAFE_MODE_ENABLED ? 10 : 15; - double MAX_ANGULAR_VELOCITY = SAFE_MODE_ENABLED ? 3.0 : 10.0; + double MAX_ANGULAR_VELOCITY = SAFE_MODE_ENABLED ? 3.0 : 6.75; double MAX_ANGULAR_ACCEL = SAFE_MODE_ENABLED ? 25.0 : 200.0; String CAN_BUS_NAME = "swerve"; diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index ad11229a..c3956a74 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -62,6 +62,7 @@ public State getState() { @Override public void periodic() { - // SmartDashboard.putString("Arm/State", state.toString()); + SmartDashboard.putString("Arm/State", state.toString()); + SmartDashboard.putString("States/Arm", state.toString()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 8358e7de..ee6f3cd9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -124,7 +124,7 @@ private double getSpeakerAngle() { Pose3d speakerPose = new Pose3d( Field.getAllianceSpeakerPose().getX(), Field.getAllianceSpeakerPose().getY(), - Field.SPEAKER_MAX_HEIGHT, + Field.SPEAKER_MAX_HEIGHT + 0.1, new Rotation3d() ); @@ -148,6 +148,9 @@ private double getSpeakerAngle() { double angleBetweenPivotToSpeakerAndArm = Units.radiansToDegrees(Math.acos(Settings.Arm.LENGTH / pivotToSpeaker.getNorm())); + if (speakerPose.toPose2d().minus(robotPose).getTranslation().getNorm() < 2.0) { + return -(angleBetweenPivotToSpeakerAndArm - angleFromPivotToSpeaker) + 8; + } return -(angleBetweenPivotToSpeakerAndArm - angleFromPivotToSpeaker); } catch (Exception exception) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index e423cbd8..8f0eb833 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -78,6 +78,7 @@ && getState() != Intake.State.DEACQUIRING setState(State.STOP); } - // SmartDashboard.putString("Intake/State", state.name()); + SmartDashboard.putString("Intake/State", state.name()); + SmartDashboard.putString("States/Intake", state.name()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index d48779cc..eab64aee 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -6,6 +6,7 @@ import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.util.ShooterSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class Shooter extends SubsystemBase { @@ -62,7 +63,8 @@ public FlywheelState getFlywheelState() { @Override public void periodic() { - // SmartDashboard.putString("Shooter/Feeder State", getFeederState().toString()); + SmartDashboard.putString("Shooter/Feeder State", getFeederState().toString()); + SmartDashboard.putString("States/Feeder", getFeederState().toString()); // feeder automatically pushes note further into shooter when its sticking too far out if (Arm.getInstance().getState() == Arm.State.AMP && !hasNote() && feederState != FeederState.DEACQUIRING) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index be7ec9db..af4ed8ed 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -199,7 +199,7 @@ private ShooterSpeeds getLobFerrySpeeds() { double distanceToFerryInInches = Units.metersToInches(SwerveDrive.getInstance().getPose().getTranslation().getDistance(ferryZone)); double targetRPM = ShooterLowFerryInterpolation.getRPM(distanceToFerryInInches); - return new ShooterSpeeds(targetRPM, 500); + return new ShooterSpeeds(targetRPM + 500, 500); } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/Telemetry.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/Telemetry.java index f26655d3..102e1aae 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/Telemetry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/Telemetry.java @@ -41,32 +41,32 @@ public Telemetry() {} private double lastTime = Utils.getCurrentTimeSeconds(); /* Mechanisms to represent the swerve module states */ - // private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { - // new Mechanism2d(1, 1), - // new Mechanism2d(1, 1), - // new Mechanism2d(1, 1), - // new Mechanism2d(1, 1), - // }; + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + }; /* A direction and length changing ligament for speed representation */ - // private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { - // m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - // m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - // m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - // m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - // }; + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + }; /* A direction changing and length constant ligament for module direction */ - // private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { - // m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) - // .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - // m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) - // .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - // m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) - // .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - // m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) - // .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - // }; + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; /* Accept the swerve drive state and telemeterize it to smartdashboard */ public void telemeterize(SwerveDriveState state) { @@ -94,12 +94,12 @@ public void telemeterize(SwerveDriveState state) { odomPeriod.set(state.OdometryPeriod); /* Telemeterize the module's states */ - // for (int i = 0; i < 4; ++i) { - // m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - // m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - // m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * Settings.Swerve.MAX_LINEAR_VELOCITY)); + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * Settings.Swerve.MAX_LINEAR_VELOCITY)); - // SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - // } + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java index bfd6de97..b190c278 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java @@ -13,6 +13,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; import java.util.Optional; import org.photonvision.EstimatedRobotPose; @@ -29,6 +31,8 @@ public class PhotonVision extends AprilTagVision { private final PhotonPoseEstimator[] poseEstimators; private final ArrayList outputs; + private int[] whitelist; + private final FieldObject2d robot; protected PhotonVision() { @@ -52,6 +56,8 @@ protected PhotonVision() { ); } + whitelist = new int[]{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + outputs = new ArrayList(); robot = SwerveDrive.getInstance().getField().getObject("Vision Pose"); @@ -74,7 +80,7 @@ public ArrayList getOutputs() { */ @Override public void setTagWhitelist(int... ids) { - + whitelist = ids; } @Override @@ -94,6 +100,13 @@ private int[] getIDs(PhotonPipelineResult pipelineResult) { return ids.stream().mapToInt(i -> i).toArray(); } + private void filterResult(PhotonPipelineResult result) { + result.targets.removeIf((PhotonTrackedTarget target) -> + target.getPoseAmbiguity() > Settings.Vision.POSE_AMBIGUITY_RATIO_THRESHOLD + || !Arrays.stream(whitelist).anyMatch((int id) -> id == target.getFiducialId()) + ); + } + @Override public void periodic() { super.periodic(); @@ -104,17 +117,16 @@ public void periodic() { final int index = i; if (enabled[index]) { PhotonPipelineResult latestResult = cameras[index].getLatestResult(); + filterResult(latestResult); + Optional estimatedRobotPose = poseEstimators[index].update(latestResult); if (latestResult.hasTargets()) { - if (latestResult.getBestTarget().getPoseAmbiguity() < Settings.Vision.POSE_AMBIGUITY_RATIO_THRESHOLD) { - Optional estimatedRobotPose = poseEstimators[index].update(latestResult); - estimatedRobotPose.ifPresent( - (EstimatedRobotPose robotPose) -> { - VisionData data = new VisionData(robotPose.estimatedPose, getIDs(latestResult), robotPose.timestampSeconds, latestResult.getBestTarget().getArea()); - outputs.add(data); - updateTelemetry("Vision/" + cameras[index].getName(), data); - } - ); - } + estimatedRobotPose.ifPresent( + (EstimatedRobotPose robotPose) -> { + VisionData data = new VisionData(robotPose.estimatedPose, getIDs(latestResult), robotPose.timestampSeconds, latestResult.getBestTarget().getArea()); + outputs.add(data); + updateTelemetry("Vision/" + cameras[index].getName(), data); + } + ); } } } diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index a0197060..6dc648db 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.7", + "version": "2024.2.8", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.7" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.7", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false,