diff --git a/src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto b/src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto new file mode 100644 index 0000000..98b8016 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Bottom Blue Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue Center Cage.auto b/src/main/deploy/pathplanner/autos/Blue Center Cage.auto new file mode 100644 index 0000000..a7a32d6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue Center Cage.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Blue Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue Top Cage.auto b/src/main/deploy/pathplanner/autos/Blue Top Cage.auto new file mode 100644 index 0000000..3acc852 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue Top Cage.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Blue Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/Center Barge Bottom.auto new file mode 100644 index 0000000..70a439a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Barge Bottom.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Reef to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Barge Top.auto b/src/main/deploy/pathplanner/autos/Center Barge Top.auto new file mode 100644 index 0000000..a5f3397 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Barge Top.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Mira's Auto.auto b/src/main/deploy/pathplanner/autos/Mira's Auto.auto new file mode 100644 index 0000000..3acc852 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mira's Auto.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Blue Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto b/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto new file mode 100644 index 0000000..658c0b4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Bottom Red Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red Center Cage.auto b/src/main/deploy/pathplanner/autos/Red Center Cage.auto new file mode 100644 index 0000000..47338f7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red Center Cage.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Red Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red Top Cage.auto b/src/main/deploy/pathplanner/autos/Red Top Cage.auto new file mode 100644 index 0000000..429e075 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red Top Cage.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Red Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/Taxi.auto similarity index 86% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/Taxi.auto index 70b7ab2..db06ab4 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/Taxi.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "Example Path" + "pathName": "Taxi" } } ] diff --git a/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path new file mode 100644 index 0000000..a5ccc16 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.616287878787879, + "y": 5.092234848484848 + }, + "prevControl": null, + "nextControl": { + "x": 8.269913357759823, + "y": 5.035328772529629 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.370012626262627, + "y": 5.346338383838384 + }, + "prevControl": { + "x": 4.997649306769947, + "y": 5.369630479499745 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -122.12499844038754 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 178.97696981133208 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path new file mode 100644 index 0000000..1d37d81 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.3145202020202018, + "y": 0.9655934343434341 + }, + "prevControl": null, + "nextControl": { + "x": 2.115768324814807, + "y": 1.5107373181346522 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.692929292929293, + "y": 2.6833333333333327 + }, + "prevControl": { + "x": 3.11332763609877, + "y": 2.3131602797293294 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.82647997035557 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 54.32359177813805 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path new file mode 100644 index 0000000..6b2c3d3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.565467171717171, + "y": 0.8334595959595947 + }, + "prevControl": null, + "nextControl": { + "x": 8.123574009981496, + "y": 0.3592362071562901 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.3903409090909085, + "y": 2.6833333333333327 + }, + "prevControl": { + "x": 5.06636861749931, + "y": 2.9361778277713553 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 121.75948008481289 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.30130561701642 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path b/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path new file mode 100644 index 0000000..117ae7d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.1565656565656575, + "y": 2.886616161616162 + }, + "prevControl": null, + "nextControl": { + "x": 5.532638888888889, + "y": 2.530871212121211 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1823863636363636, + "y": 1.026578282828283 + }, + "prevControl": { + "x": 0.9229528762543462, + "y": 0.9200437072077402 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 54.0394828033551 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 119.16761337957772 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef.path similarity index 72% rename from src/main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/Center Barge to Reef.path index 946269c..9e36d9d 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Center Barge to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.415049342105263, - "y": 4.785115131578947 + "x": 7.565467171717171, + "y": 4.045328282828282 }, "prevControl": null, "nextControl": { - "x": 2.905756578947368, - "y": 4.794736842105262 + "x": 8.114457611671098, + "y": 4.043616195025507 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.8294407894736837, - "y": 5.862746710526316 + "x": 5.979861111111112, + "y": 4.045328282828282 }, "prevControl": { - "x": 3.0693256578947365, - "y": 5.872368421052631 + "x": 5.5035379766577375, + "y": 4.0462802990619195 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 179.65844947344297 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -2.4366482468102095 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path new file mode 100644 index 0000000..05a8393 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.575631313127491, + "y": 6.139141414136251 + }, + "prevControl": null, + "nextControl": { + "x": 6.3210457316672, + "y": 6.004686892287159 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.197222222222222, + "y": 5.52929292929293 + }, + "prevControl": { + "x": 6.234912180385907, + "y": 5.781923450986739 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -121.60750224624891 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.44374777291927 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path new file mode 100644 index 0000000..6a9c102 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.585795454545455, + "y": 1.9413510101010092 + }, + "prevControl": null, + "nextControl": { + "x": 6.5546611575005915, + "y": 2.13657272729177 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.319191919191919, + "y": 2.6833333333333327 + }, + "prevControl": { + "x": 5.563570773660507, + "y": 2.6306173444056773 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 118.92642583525368 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path new file mode 100644 index 0000000..ad0027f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.786742424242425, + "y": 4.035164141414142 + }, + "prevControl": null, + "nextControl": { + "x": 7.026767676767678, + "y": 2.185290404040403 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3755050505050503, + "y": 0.8842803030303026 + }, + "prevControl": { + "x": 1.5707222528658078, + "y": 1.040454064918909 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 50.964487101253106 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -178.99491399474581 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Reef to Feed.path b/src/main/deploy/pathplanner/paths/Center Reef to Feed.path new file mode 100644 index 0000000..ce674ff --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Reef to Feed.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.817234848484849, + "y": 4.045328282828282 + }, + "prevControl": null, + "nextControl": { + "x": 6.62020202020202, + "y": 4.797474747474748 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.2175505050505055, + "y": 5.45814393939394 + }, + "prevControl": { + "x": 5.45268897997343, + "y": 5.373232823449551 + }, + "nextControl": { + "x": 3.388005050505051, + "y": 6.11881313131313 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.121401515151515, + "y": 7.003093434343434 + }, + "prevControl": { + "x": 0.32627122293847843, + "y": 7.229156472141377 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -52.30575953331084 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -177.95459151111288 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path new file mode 100644 index 0000000..8ff7750 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.565467171717171, + "y": 7.348674242424242 + }, + "prevControl": null, + "nextControl": { + "x": 7.8906231049309685, + "y": 7.339915637757156 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.305113636363636, + "y": 7.348674242424242 + }, + "prevControl": { + "x": 5.928529040722133, + "y": 7.353679406601922 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.47479945510887 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 177.6140559696112 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path new file mode 100644 index 0000000..2a1b6ba --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.575631313131314, + "y": 7.287689393939393 + }, + "prevControl": null, + "nextControl": { + "x": 7.270707070707072, + "y": 7.013257575757576 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.309027777777779, + "y": 5.3768308080808085 + }, + "prevControl": { + "x": 4.945156854248459, + "y": 5.08761998976538 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.0183674276091 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.30972280213487 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Feed to Reef.path b/src/main/deploy/pathplanner/paths/Top Feed to Reef.path new file mode 100644 index 0000000..3c7f204 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Feed to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.263699494949495, + "y": 7.135227272727272 + }, + "prevControl": null, + "nextControl": { + "x": 1.6862557056350642, + "y": 6.8478211564289 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6217803030303033, + "y": 5.417487373737374 + }, + "prevControl": { + "x": 3.111746968518841, + "y": 5.776730048021274 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -60.228308784269046 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -55.124671655397854 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path new file mode 100644 index 0000000..fe0d749 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.595959595959596, + "y": 3.0289141414141416 + }, + "prevControl": null, + "nextControl": { + "x": 8.242449416720314, + "y": 3.1104783377417067 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.349684343434343, + "y": 2.6934974747474745 + }, + "prevControl": { + "x": 4.985594317577342, + "y": 2.6542548990156853 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.25643716352924 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.06080905426438 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Reef to Feed.path b/src/main/deploy/pathplanner/paths/Top Reef to Feed.path new file mode 100644 index 0000000..32f6914 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Reef to Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.17689393939394, + "y": 5.224368686868687 + }, + "prevControl": null, + "nextControl": { + "x": 5.9669821266547345, + "y": 4.892012503312241 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.192550505050505, + "y": 7.0234217171717175 + }, + "prevControl": { + "x": 0.9300747161944047, + "y": 7.106516384157039 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -52.56142842766699 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -121.75948008481281 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 25d3767..5be92c8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -341,7 +341,7 @@ public final class Constants { public static final class VisionConstants { public static final String CAMERA_NAME = "Camera_Module_v1"; - public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI)); + public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters @@ -354,7 +354,9 @@ public final class Constants { public static final class FieldConstants { public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); - public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); + public static final double HORISONTAL_SCORING_POSITION_OFFSET = 0; + // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right + public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20); diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 18e54f5..0281e90 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -148,6 +148,7 @@ public class Robot extends TimedRobot { // continue until interrupted by another command, remove // this line or comment it out. if (m_autonomousCommand != null) { + CommandScheduler.getInstance().cancel(m_autonomousCommand); m_autonomousCommand.cancel(); } m_robotTime.startMatchTime(); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e211436..7735d99 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,6 +16,8 @@ import java.util.List; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.robot.Constants.OIConstants; @@ -28,6 +30,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.Commands; // Autos @@ -35,6 +39,10 @@ import frc4388.utility.controller.VirtualController; import frc4388.robot.commands.GotoPositionCommand; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; + +import com.pathplanner.lib.auto.AutoBuilder; + +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; // Subsystems // import frc4388.robot.subsystems.LED; @@ -87,17 +95,39 @@ public class RobotContainer { // ! /* Autos */ private String lastAutoName = "defualt.auto"; + private final SendableChooser autoChooser; private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, () -> autoplaybackName.get(), // lastAutoName new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false); + private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision); + + private Command placeCoral = new SequentialCommandGroup( + new InstantCommand(() -> System.out.println("Placing Some Coral")), + new WaitCommand(3) + ); + + private Command aprilAlign = new SequentialCommandGroup( + new InstantCommand(() -> System.out.println("Aligning...")), + new WaitCommand(1) + ); + + private Command grabCoral = new SequentialCommandGroup( + new InstantCommand(() -> System.out.println("Yoinking some coral...")), + new WaitCommand(2) + ); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition); + NamedCommands.registerCommand("april-allign", aprilAlign); + NamedCommands.registerCommand("place-coral", placeCoral); + NamedCommands.registerCommand("grab-coral", grabCoral); + configureButtonBindings(); configureVirtualButtonBindings(); new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); @@ -115,6 +145,8 @@ public class RobotContainer { .withName("SwerveDrive DefaultCommand")); m_robotSwerveDrive.setToSlow(); + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); // this.subsystems.add(m_robotSwerveDrive); // this.subsystems.add(m_robotMap.leftFront); // this.subsystems.add(m_robotMap.rightFront); @@ -265,15 +297,17 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { + //return autoPlayback; - //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); - try{ - // Load the path you want to follow using its name in the GUI - return new PathPlannerAuto("New Auto"); - } catch (Exception e) { - DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); - return Commands.none(); - } + //return new GotoPositionCommand(m_robotSwerveDrive, m_vision) + return autoChooser.getSelected(); + // try{ + // // Load the path you want to follow using its name in the GUI + // return new PathPlannerAuto("New Auto"); + // } catch (Exception e) { + // DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); + // return Commands.none(); + // } // zach told me to do the below comment //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); // return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f0410e9..0820891 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -32,6 +32,7 @@ import frc4388.utility.Status; import frc4388.utility.Subsystem; import frc4388.utility.Status.ReportLevel; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.config.PIDConstants; @@ -52,6 +53,8 @@ public class SwerveDrive extends Subsystem { public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to // 25% + public Pose2d initalPose2d = null; + public double rotTarget = 0.0; public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); @@ -75,11 +78,9 @@ public class SwerveDrive extends Subsystem { // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { - var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()); - // pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1)); - return pose;// getRotation().times(-1) + return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d); }, // Robot pose supplier - swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting + this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting // pose) () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() @@ -108,6 +109,11 @@ public class SwerveDrive extends Subsystem { } + public void setOdoPose(Pose2d pose) { + initalPose2d = pose; + swerveDriveTrain.resetPose(pose); + } + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, // Translation2d rightStick){ // // double ang = Math.atan2(rightStick.getY(), rightStick.getX());