diff --git a/src/main/deploy/pathplanner/autos/Blue Top Cage.auto b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Left.auto similarity index 57% rename from src/main/deploy/pathplanner/autos/Blue Top Cage.auto rename to src/main/deploy/pathplanner/autos/1 Coral Center Barge Left.auto index 555bb6c..3dd88fc 100644 --- a/src/main/deploy/pathplanner/autos/Blue Top Cage.auto +++ b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Left.auto @@ -4,10 +4,16 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { - "pathName": "Top Blue Cage to Reef" + "pathName": "Center Barge to Reef 4 Left" } }, { @@ -16,34 +22,16 @@ "name": "place-coral-left-l4" } }, - { - "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": "place-coral-left-l4" + "name": "lower-algae-removal" } } ] } }, "resetOdom": true, - "folder": null, + "folder": "1 Coral", "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/1 Coral Center Barge Right.auto similarity index 57% rename from src/main/deploy/pathplanner/autos/Red Bottom Cage.auto rename to src/main/deploy/pathplanner/autos/1 Coral Center Barge Right.auto index 3439f6b..e0d6f82 100644 --- a/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto +++ b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Right.auto @@ -4,10 +4,16 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { - "pathName": "Bottom Red Cage to Reef" + "pathName": "Center Barge to Reef 4 Right" } }, { @@ -16,34 +22,16 @@ "name": "place-coral-right-l4" } }, - { - "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": "place-coral-left-l4" + "name": "lower-algae-removal" } } ] } }, "resetOdom": true, - "folder": null, + "folder": "1 Coral", "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/2 Coral Cage 1.auto similarity index 52% rename from src/main/deploy/pathplanner/autos/Red Center Cage.auto rename to src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto index 7bfeb66..ae18da3 100644 --- a/src/main/deploy/pathplanner/autos/Red Center Cage.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto @@ -4,28 +4,34 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "Center Red Cage to Reef" - } - }, { "type": "named", "data": { - "name": "place-coral-right-l4" + "name": "prepare-l4" } }, { "type": "path", "data": { - "pathName": "Bottom Reef to Feed" + "pathName": "Cage 1 to Reef" } }, { "type": "named", "data": { - "name": "align-feed" + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" } }, { @@ -37,19 +43,43 @@ { "type": "path", "data": { - "pathName": "Bottom Feed to Reef" + "pathName": "Top Feed to Reef 6" } }, { "type": "named", "data": { - "name": "place-coral-left-l4" + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" } } ] } }, "resetOdom": true, - "folder": null, + "folder": "2 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto new file mode 100644 index 0000000..42d1639 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 2 to Reef 5" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Coral", + "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/2 Coral Cage 3.auto similarity index 51% rename from src/main/deploy/pathplanner/autos/Center Barge Bottom.auto rename to src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto index 70a439a..72db1a3 100644 --- a/src/main/deploy/pathplanner/autos/Center Barge Bottom.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto @@ -4,28 +4,34 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "Center Barge to Reef" - } - }, { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "prepare-l4" } }, { "type": "path", "data": { - "pathName": "Center Reef to Bottom Feed" + "pathName": "Cage 3 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" } }, { @@ -37,25 +43,43 @@ { "type": "path", "data": { - "pathName": "Bottom Feed to Reef" + "pathName": "Top Feed to Reef 6" } }, { "type": "named", "data": { - "name": "april-allign" + "name": "await-coral" } }, { "type": "named", "data": { - "name": "place-coral" + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" } } ] } }, "resetOdom": true, - "folder": null, + "folder": "2 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto new file mode 100644 index 0000000..3df4f64 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 4 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Coral", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto new file mode 100644 index 0000000..0a519ac --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 5 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Coral", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto new file mode 100644 index 0000000..68d7700 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 6 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Coral", + "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/2 Coral Center Barge Left.auto similarity index 55% rename from src/main/deploy/pathplanner/autos/Blue Center Cage.auto rename to src/main/deploy/pathplanner/autos/2 Coral Center Barge Left.auto index a7a32d6..8351dab 100644 --- a/src/main/deploy/pathplanner/autos/Blue Center Cage.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Left.auto @@ -4,28 +4,34 @@ "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" + "name": "prepare-l4" } }, { "type": "path", "data": { - "pathName": "Top Reef to Feed" + "pathName": "Center Barge to Reef 4 Right" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 4 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" } }, { @@ -37,25 +43,37 @@ { "type": "path", "data": { - "pathName": "Top Feed to Reef" + "pathName": "Top Feed to Reef 6" } }, { "type": "named", "data": { - "name": "april-allign" + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" } }, { "type": "named", "data": { - "name": "place-coral" + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" } } ] } }, "resetOdom": true, - "folder": null, + "folder": "2 Coral", "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/2 Coral Center Barge Right.auto similarity index 54% rename from src/main/deploy/pathplanner/autos/Center Barge Top.auto rename to src/main/deploy/pathplanner/autos/2 Coral Center Barge Right.auto index a5f3397..0628011 100644 --- a/src/main/deploy/pathplanner/autos/Center Barge Top.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Right.auto @@ -4,28 +4,34 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "Center Barge to Reef" - } - }, { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "prepare-l4" } }, { "type": "path", "data": { - "pathName": "Center Reef to Feed" + "pathName": "Center Barge to Reef 4 Left" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 4 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" } }, { @@ -37,25 +43,37 @@ { "type": "path", "data": { - "pathName": "Top Feed to Reef" + "pathName": "Bottom Feed to Reef 2" } }, { "type": "named", "data": { - "name": "april-allign" + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" } }, { "type": "named", "data": { - "name": "place-coral" + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" } } ] } }, "resetOdom": true, - "folder": null, + "folder": "2 Coral", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto new file mode 100644 index 0000000..218687a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto @@ -0,0 +1,109 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 2 to Reef 5" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto new file mode 100644 index 0000000..a09d168 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -0,0 +1,121 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 5 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/3 Coral Center Barge Bottom.auto new file mode 100644 index 0000000..88b79f7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Center Barge Bottom.auto @@ -0,0 +1,109 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Left" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 4 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto b/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto deleted file mode 100644 index 4d51c0d..0000000 --- a/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Top Blue Cage to Reef" - } - }, - { - "type": "named", - "data": { - "name": "place-coral-left-l4" - } - } - ] - } - }, - "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 deleted file mode 100644 index 3acc852..0000000 --- a/src/main/deploy/pathplanner/autos/Mira's Auto.auto +++ /dev/null @@ -1,61 +0,0 @@ -{ - "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/Blue Bottom Cage.auto b/src/main/deploy/pathplanner/autos/New Auto.auto similarity index 58% rename from src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto rename to src/main/deploy/pathplanner/autos/New Auto.auto index 98b8016..9333c35 100644 --- a/src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -7,49 +7,49 @@ { "type": "path", "data": { - "pathName": "Bottom Blue Cage to Reef" - } - }, - { - "type": "named", - "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "pathName": "Cage 1 to Reef" } }, { "type": "path", "data": { - "pathName": "Top Reef to Feed" - } - }, - { - "type": "named", - "data": { - "name": "grab-coral" + "pathName": "Cage 2 to Reef 5" } }, { "type": "path", "data": { - "pathName": "Top Feed to Reef" + "pathName": "Cage 3 to Reef" } }, { - "type": "named", + "type": "path", "data": { - "name": "april-allign" + "pathName": "Cage 4 to Reef" } }, { - "type": "named", + "type": "path", "data": { - "name": "place-coral" + "pathName": "Cage 5 to Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 6 to Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Left" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Right" } } ] diff --git a/src/main/deploy/pathplanner/autos/Red Top Cage.auto b/src/main/deploy/pathplanner/autos/Red Top Cage.auto deleted file mode 100644 index 429e075..0000000 --- a/src/main/deploy/pathplanner/autos/Red Top Cage.auto +++ /dev/null @@ -1,61 +0,0 @@ -{ - "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/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto index db06ab4..a717089 100644 --- a/src/main/deploy/pathplanner/autos/Taxi.auto +++ b/src/main/deploy/pathplanner/autos/Taxi.auto @@ -5,9 +5,9 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Taxi" + "name": "taxi" } } ] diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path new file mode 100644 index 0000000..943d856 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.3040625, + "y": 1.3559375 + }, + "prevControl": null, + "nextControl": { + "x": 2.892340424329296, + "y": 2.067688275398428 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5221875, + "y": 2.3675 + }, + "prevControl": { + "x": 2.589485733053583, + "y": 1.921600370835535 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 59.28109573597083 + }, + "reversed": false, + "folder": "Bottom Feed to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 53.426969021480645 + }, + "useDefaultConstraints": false +} \ 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/Cage 1 to Reef.path similarity index 70% rename from src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path rename to src/main/deploy/pathplanner/paths/Cage 1 to Reef.path index 6bb67cc..710c3b1 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.344772727272727, - "y": 2.6688636363636364 + "x": 6.9834375, + "y": 7.1084375 }, "prevControl": null, "nextControl": { - "x": 6.4715625, - "y": 1.432386363636364 + "x": 6.292598684210526, + "y": 6.699835526315789 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.1823863636363636, - "y": 1.026578282828283 + "x": 5.590213815789473, + "y": 5.622203947368421 }, "prevControl": { - "x": 0.9229528762543462, - "y": 0.9200437072077402 + "x": 5.846151315789473, + "y": 6.024391447368421 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 55.05578949900953 + "rotation": -118.81079374297312 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": 121.42956561483854 + "rotation": -115.9743939624313 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path new file mode 100644 index 0000000..77f25d6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.0078125, + "y": 6.0725 + }, + "prevControl": null, + "nextControl": { + "x": 6.902067334527623, + "y": 5.78751784036735 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.590213815789473, + "y": 5.593338815789473 + }, + "prevControl": { + "x": 5.872115174798128, + "y": 6.170888760790882 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 5.15, + "y": 4.85 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.52970589993409 + }, + "reversed": false, + "folder": "Barge to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -123.4533094540727 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path similarity index 72% rename from src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path rename to src/main/deploy/pathplanner/paths/Cage 3 to Reef.path index f63510a..5e34982 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.616287878787879, - "y": 5.092234848484848 + "x": 6.9590625, + "y": 4.9878124999999995 }, "prevControl": null, "nextControl": { - "x": 8.269913357759823, - "y": 5.035328772529629 + "x": 6.709257689548163, + "y": 4.997689578268469 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.370012626262627, - "y": 5.346338383838384 + "x": 5.619078947368421, + "y": 5.593338815789473 }, "prevControl": { - "x": 4.997649306769947, - "y": 5.369630479499745 + "x": 6.094391447368421, + "y": 5.5202138157894725 }, "nextControl": null, "isLocked": false, @@ -45,10 +45,10 @@ "rotation": -121.26879518614867 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": 179.34296858150984 + "rotation": -124.91940201245771 }, "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/Cage 4 to Reef.path similarity index 70% rename from src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path rename to src/main/deploy/pathplanner/paths/Cage 4 to Reef.path index 7e6f511..c5ef9d6 100644 --- a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.585795454545455, - "y": 1.9413510101010092 + "x": 6.97125, + "y": 3.1474999999999995 }, "prevControl": null, "nextControl": { - "x": 5.96969696969697, - "y": 1.1282196969696958 + "x": 6.166875, + "y": 2.8184375 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.319191919191919, - "y": 2.6833333333333327 + "x": 5.55172697368421, + "y": 2.4470394736842103 }, "prevControl": { - "x": 5.75625, - "y": 2.256439393939394 + "x": 6.3804769736842095, + "y": 2.7395394736842107 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 122.87136694597014 + "rotation": 120.96375653207355 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": 179.5295681977034 + "rotation": 118.9091836511478 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path new file mode 100644 index 0000000..452dec9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.016603535353535, + "y": 1.9718434343434341 + }, + "prevControl": null, + "nextControl": { + "x": 6.670197826108406, + "y": 2.0396270358261894 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.5805921052631575, + "y": 2.4374177631578946 + }, + "prevControl": { + "x": 5.9030178398827875, + "y": 2.377301864673819 + }, + "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": 1.0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": "Barge to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 123.5866498700801 + }, + "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/Cage 6 to Reef.path similarity index 69% rename from src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path rename to src/main/deploy/pathplanner/paths/Cage 6 to Reef.path index 25f3281..27ace9e 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.3145202020202018, - "y": 0.9655934343434341 + "x": 7.057260101010102, + "y": 0.8537878787878788 }, "prevControl": null, "nextControl": { - "x": 4.39747159090909, - "y": 1.3027556818181814 + "x": 6.315277777777778, + "y": 1.5144570707070704 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.3048863636363635, - "y": 2.5292613636363637 + "x": 5.561348684210526, + "y": 2.456661184210526 }, "prevControl": { - "x": 5.893210227272728, - "y": 1.9808238636363626 + "x": 5.883902345826687, + "y": 2.038842371079213 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 114.22774531795413 + "rotation": 120.98817835541992 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": 53.446462811652175 + "rotation": 132.5633517531898 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Barge to Reef.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Left.path similarity index 76% rename from src/main/deploy/pathplanner/paths/Center Barge to Reef.path rename to src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Left.path index cfbbb95..a30c744 100644 --- a/src/main/deploy/pathplanner/paths/Center Barge to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Left.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.565467171717171, - "y": 4.045328282828282 + "x": 7.1296875, + "y": 4.0493749999999995 }, "prevControl": null, "nextControl": { - "x": 7.269289772727273, - "y": 4.064886363636362 + "x": 6.833510101010102, + "y": 4.06893308080808 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.172414772727272, - "y": 4.045328282828282 + "x": 6.3496875, + "y": 3.8421874999999996 }, "prevControl": { - "x": 5.696091638273898, - "y": 4.0462802990619195 + "x": 6.599301094934041, + "y": 3.8282931405651617 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": -179.96096735022735 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, "rotation": -179.2890804030095 diff --git a/src/main/deploy/pathplanner/paths/Top Feed to Reef.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Right.path similarity index 70% rename from src/main/deploy/pathplanner/paths/Top Feed to Reef.path rename to src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Right.path index fce25be..b662f54 100644 --- a/src/main/deploy/pathplanner/paths/Top Feed to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Right.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.116818181818182, - "y": 6.956647727272727 + "x": 7.1053125, + "y": 4.061562499999999 }, "prevControl": null, "nextControl": { - "x": 2.123068181818182, - "y": 6.275650252525252 + "x": 6.809135101010102, + "y": 4.081120580808079 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.6196875, - "y": 5.35122159090909 + "x": 6.3496875, + "y": 4.195625 }, "prevControl": { - "x": 2.8392584714142375, - "y": 5.792423901494171 + "x": 6.599301094934041, + "y": 4.181730640565162 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -59.239047023115106 + "rotation": -179.96096735022735 }, "reversed": false, - "folder": null, + "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": -52.93323259086456 + "rotation": -179.2890804030095 }, "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 deleted file mode 100644 index 434e054..0000000 --- a/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "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.268371212121212, - "y": 5.417487373737374 - }, - "prevControl": { - "x": 6.306061170284897, - "y": 5.670117895431184 - }, - "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.86136963407526 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -179.99261564513898 - }, - "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 deleted file mode 100644 index 5bee8d7..0000000 --- a/src/main/deploy/pathplanner/paths/Center Reef to Feed.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.982954545454545, - "y": 4.03497159090909 - }, - "prevControl": null, - "nextControl": { - "x": 7.109744318181819, - "y": 3.8355397727272718 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.232244318181818, - "y": 5.281420454545455 - }, - "prevControl": { - "x": 7.1397939460232385, - "y": 4.676387369317844 - }, - "nextControl": { - "x": 5.574119318181818, - "y": 5.720170454545454 - }, - "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": -53.93780932479241 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 179.1574757392596 - }, - "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/New Path.path similarity index 72% rename from src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path rename to src/main/deploy/pathplanner/paths/New Path.path index 66ebba7..bde395e 100644 --- a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.575631313131314, - "y": 7.287689393939393 + "x": 4.0, + "y": 6.0 }, "prevControl": null, "nextControl": { - "x": 6.599873737373737, - "y": 8.029671717171716 + "x": 4.2495147804709745, + "y": 5.984431624152738 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.344772727272727, - "y": 5.391107954545454 + "x": 4.0, + "y": 6.0 }, "prevControl": { - "x": 5.121161616161616, - "y": 5.00487058080808 + "x": 4.249520880858856, + "y": 5.984529705386749 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -122.99770510121631 + "rotation": 0.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path new file mode 100644 index 0000000..dbd1a43 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5221875, + "y": 2.4040625 + }, + "prevControl": null, + "nextControl": { + "x": 2.9564891755960816, + "y": 2.1114702488693777 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3284375, + "y": 1.3803125 + }, + "prevControl": { + "x": 2.0502196323692035, + "y": 1.7292616486680126 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.5, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 55.37584492005105 + }, + "reversed": false, + "folder": "Reef to Bottom Feed", + "idealStartingState": { + "velocity": 0, + "rotation": 58.10920819815426 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path new file mode 100644 index 0000000..46f8cb1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.426644736842105, + "y": 2.851151315789474 + }, + "prevControl": null, + "nextControl": { + "x": 4.23151774045974, + "y": 2.4081021765718877 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.7415296052631577, + "y": 1.5618421052631584 + }, + "prevControl": { + "x": 3.369389020277641, + "y": 1.8741228567589594 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 55.05578949900953 + }, + "reversed": false, + "folder": "Reef to Bottom Feed", + "idealStartingState": { + "velocity": 0, + "rotation": 121.42956561483854 + }, + "useDefaultConstraints": false +} \ 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/Reef 4 to Bottom Feed.path similarity index 66% rename from src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path rename to src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path index 200490f..7fa944f 100644 --- a/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 6.092642045454545, - "y": 3.9651704545454542 + "x": 5.9750822368421055, + "y": 3.9865131578947364 }, "prevControl": null, "nextControl": { - "x": 9.024289772727268, - "y": 3.446647727272728 + "x": 7.0046052631578934, + "y": 1.6388157894736843 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.3755050505050503, - "y": 0.8842803030303026 + "x": 1.3040625, + "y": 1.368125 }, "prevControl": { - "x": 1.5707222528658078, - "y": 1.040454064918909 + "x": 1.7992598684210526, + "y": 2.302713815789474 }, "nextControl": null, "isLocked": false, @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 0.0, "rotation": 52.857102599919905 }, "reversed": false, - "folder": null, + "folder": "Reef to Bottom Feed", "idealStartingState": { "velocity": 0, "rotation": -179.46454101443553 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ 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/Reef 4 to Top Feed.path similarity index 69% rename from src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path rename to src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path index 226a1a8..ef11e1e 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.565467171717171, - "y": 0.8334595959595947 + "x": 5.946217105263157, + "y": 4.178947368421053 }, "prevControl": null, "nextControl": { - "x": 8.123574009981496, - "y": 0.3592362071562901 + "x": 6.604342105263157, + "y": 6.518947368421053 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.3903409090909085, - "y": 2.6833333333333327 + "x": 1.452878289473684, + "y": 6.565131578947369 }, "prevControl": { - "x": 5.06636861749931, - "y": 2.9361778277713553 + "x": 2.0499142743221688, + "y": 5.561413144603935 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -57.4259428654274 + "rotation": -53.93780932479242 }, "reversed": false, - "folder": null, + "folder": "Reef to Top Feed", "idealStartingState": { "velocity": 0, - "rotation": -0.9548412538721401 + "rotation": 178.9390883097358 }, "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/Reef 5 to Top Feed.path similarity index 67% rename from src/main/deploy/pathplanner/paths/Top Reef to Feed.path rename to src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path index 40748ed..519045a 100644 --- a/src/main/deploy/pathplanner/paths/Top Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.284943181818181, - "y": 5.401079545454545 + "x": 5.41702302631579, + "y": 5.256578947368421 }, "prevControl": null, "nextControl": { - "x": 4.844554924242423, - "y": 6.744466540404039 + "x": 4.4044191919191915, + "y": 5.9460227272727275 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.2230429292929292, - "y": 7.003093434343434 + "x": 1.5076388888888888, + "y": 6.484722222222222 }, "prevControl": { - "x": 1.9644034090909093, - "y": 5.750085227272726 + "x": 3.184818220656944, + "y": 6.004734862812045 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.5, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -45,10 +45,10 @@ "rotation": -54.83470564502973 }, "reversed": false, - "folder": null, + "folder": "Reef to Top Feed", "idealStartingState": { "velocity": 0, "rotation": -121.0370223454415 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path new file mode 100644 index 0000000..77ed74f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5099747474747476, + "y": 5.742739898989899 + }, + "prevControl": null, + "nextControl": { + "x": 2.36577546257746, + "y": 6.2120596924932325 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4143914473684207, + "y": 6.584375 + }, + "prevControl": { + "x": 2.7066049155713703, + "y": 6.056626627340928 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.5, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.93780932479242 + }, + "reversed": false, + "folder": "Reef to Top Feed", + "idealStartingState": { + "velocity": 0, + "rotation": -58.96173664449721 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path deleted file mode 100644 index c4ec989..0000000 --- a/src/main/deploy/pathplanner/paths/Taxi.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "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": { - "x": 6.681698232005139, - "y": 7.343669078246562 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.8889204545454543, - "y": 7.348674242424242 - }, - "prevControl": { - "x": 4.138853417354843, - "y": 7.3544633789160185 - }, - "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.59913485447024 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -179.85896618052556 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path b/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path new file mode 100644 index 0000000..74f69ec --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.31625, + "y": 6.730625 + }, + "prevControl": null, + "nextControl": { + "x": 2.5451175614850996, + "y": 6.277541643530879 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.540467171717172, + "y": 5.7630681818181815 + }, + "prevControl": { + "x": 2.590802656557517, + "y": 6.105344244914677 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.239047023115106 + }, + "reversed": false, + "folder": "Top Feed to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -52.93323259086456 + }, + "useDefaultConstraints": false +} \ 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 deleted file mode 100644 index 0cefb5a..0000000 --- a/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.595959595959596, - "y": 3.0289141414141416 - }, - "prevControl": null, - "nextControl": { - "x": 7.319147727272727, - "y": 2.3198579545454536 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.349684343434343, - "y": 2.6934974747474745 - }, - "prevControl": { - "x": 6.401761363636364, - "y": 1.5819602272727276 - }, - "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": 123.49004753500587 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -178.9832812445108 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 5bf438c..948f9b8 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,18 +2,28 @@ "robotWidth": 0.9, "robotLength": 0.9, "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], + "pathFolders": [ + "Barge to Reef", + "Bottom Feed to Reef", + "Reef to Top Feed", + "Reef to Bottom Feed", + "Top Feed to Reef" + ], + "autoFolders": [ + "1 Coral", + "2 Coral", + "3 Coral" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, + "robotMass": 58.18, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, + "driveWheelRadius": 0.0508, + "driveGearing": 6.12, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4807698..7f0cbfa 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -21,7 +21,6 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; @@ -55,6 +54,7 @@ import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; import frc4388.utility.ReefPositionHelper; import frc4388.utility.Trim; +import frc4388.utility.configurable.ConfigurableDouble; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -69,7 +69,7 @@ public final class Constants { public static final class SwerveDriveConstants { - public static final double MAX_ROT_SPEED = 3.5; + public static final double MAX_ROT_SPEED = Math.PI * 2; public static final double AUTO_MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 1.0; public static double ROTATION_SPEED = MAX_ROT_SPEED; @@ -114,7 +114,7 @@ public final class Constants { private static final class ModuleSpecificConstants { //2025 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.4794921875+0.25); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; @@ -122,7 +122,7 @@ public final class Constants { private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.62841796875-0.5); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; @@ -130,7 +130,7 @@ public final class Constants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(-0.867431640625+0.25); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; @@ -138,7 +138,7 @@ public final class Constants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.425537109375-0.25+0.25); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; @@ -146,40 +146,6 @@ public final class Constants { private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); } - /* private static final class ModuleSpecificConstants { // 2024 - //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); - private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; - private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; - private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; - private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); - - //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625); - private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; - private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; - private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; - private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); - - //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5); - private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; - private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; - private static final boolean BACK_LEFT_ENCODER_INVERTED = false; - private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); - - //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5); - private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; - private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; - private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; - private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); - } */ - public static final class IDs { public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4); public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 5); @@ -228,7 +194,7 @@ public final class Constants { .withKS(0).withKV(0.124); public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); - public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST + public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1); } public static final class Configurations { @@ -322,43 +288,76 @@ public final class Constants { } public static final class LiDARConstants { + public static final int REEF_LIDAR_DIO_CHANNEL = 7; + public static final int REVERSE_LIDAR_DIO_CHANNEL = 4; + + public static final int HUMAN_PLAYER_STATION_DISTANCE = 40; + public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole - public static final int LIDAR_DIO_CHANNEL = 7; public static final int LIDAR_MICROS_TO_CM = 10; public static final int SECONDS_TO_MICROS = 1000000; } public static final class AutoConstants { - public static final Gains XY_GAINS = new Gains(3,0.01,0.0); - public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); + // public static final Gains XY_GAINS = new Gains(5,0.6,0.0); + public static final Gains XY_GAINS = new Gains(8,0,0.0); + // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP); + // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI); + // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD); + // public static final Gains XY_GAINS = new Gains(3,0.3,0.0); + + // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); + // public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); + // public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5); public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0); public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0); public static final double XY_TOLERANCE = 0.07; // Meters public static final double ROT_TOLERANCE = 5; // Degrees + + public static final double MIN_XY_PID_OUTPUT = 0.0; + public static final double MIN_ROT_PID_OUTPUT = 0.0; + + public static final double VELOCITY_THRESHHOLD = 0.01; // X is tangent to reef side // Y is normal to reef side - public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field - public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16); + public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field + public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1); + public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18); - public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); - public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); + public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP; + // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5); - public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); - public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); + public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1); - public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(1); + public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6); + public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5-2); - public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); + public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE; + + // public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); + // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); + // // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5); + + // public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); + // public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1); + + // public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + // public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5); + + // public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); public static final int L4_DRIVE_TIME = 250; //Milliseconds // public static final int L3_DRIVE_TIME = 500; public static final int L2_DRIVE_TIME = 250; //Milliseconds public static final int ALGAE_DRIVE_TIME = 500; + public static final double STOP_VELOCITY = 0.0; } public static final class VisionConstants { @@ -368,14 +367,14 @@ public final class Constants { public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); - public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters + public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters // Photonvision thing // The standard deviations of our vision estimated poses, which affect correction rate // X, Y, Theta // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2 - public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); - public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1); } public static final class FieldConstants { @@ -403,6 +402,7 @@ public final class Constants { public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED; public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW; public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK; + public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW; public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES; public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES; @@ -416,39 +416,47 @@ public final class Constants { public static final double LEFT_AXIS_DEADBAND = 0.1; } - public static final class ClimberConstants { - public static final CanDevice CLIMBER_ID = new CanDevice("Climber", 311); - public static final double CLIMBER_SPEED = 0.1; - } + public static final class ElevatorConstants { public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15); public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16); + public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75; + public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20; + + // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND + public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND + public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND + public static final double GEAR_RATIO_ELEVATOR = -9.0; //Max for elevator = 50% public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR; public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe + public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe public static final double SCORING_THREE_ELEVATOR = -9.25; - public static final double DEALGAE_L2_ELEVATOR = -23.5; - public static final double DEALGAE_L3_ELEVATOR = -33.75; + public static final double DEALGAE_L2_ELEVATOR = -4; + public static final double DEALGAE_L3_ELEVATOR = -26.5; public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position public static final double GEAR_RATIO_ENDEFECTOR = -100.0; public static final double ENDEFECTOR_DRIVE_SLOW = -0.08; //Max for endefector = 60% + public static final double L2_SCORE_ELEVATOR = -5; + public static final double L2_LEAVE_ELEVATOR = -11; public static final double L2_SCORE_ENDEFFECTOR = -19; public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; - public static final double DEALGAE_L2_ENDEFFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR; + public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; - public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value + public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; + public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR; public static final Slot0Configs ELEVATOR_PID = new Slot0Configs() diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c31278f..37a6cbb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,6 +14,7 @@ import java.io.File; import java.util.ArrayList; import java.util.List; import java.util.function.BooleanSupplier; +import java.util.function.Supplier; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Rotation2d; @@ -27,6 +28,7 @@ import frc4388.utility.controller.XboxController; import frc4388.utility.controller.ButtonBox; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.robot.Constants.FieldConstants; +import frc4388.robot.Constants.LiDARConstants; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.AutoConstants; @@ -36,6 +38,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -43,12 +47,16 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; // Autos import frc4388.utility.controller.VirtualController; +import frc4388.robot.commands.DriveUntilLiDAR; import frc4388.robot.commands.GotoLastApril; import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.MoveForTimeCommand; +import frc4388.robot.commands.MoveUntilSuply; +import frc4388.robot.commands.WhileTrueCommand; import frc4388.robot.commands.waitElevatorRefrence; import frc4388.robot.commands.waitEndefectorRefrence; import frc4388.robot.commands.waitFeedCoral; +import frc4388.robot.commands.waitSupplier; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; @@ -61,7 +69,6 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Elevator.CoordinationState; import frc4388.robot.subsystems.Lidar; -import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Elevator; // import frc4388.robot.subsystems.Endeffector; import frc4388.robot.subsystems.SwerveDrive; @@ -84,15 +91,16 @@ import frc4388.utility.configurable.ConfigurableString; */ public class RobotContainer { /* RobotMap */ + public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); - public final Lidar m_lidar = new Lidar(); - public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED); + public final Lidar m_reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef"); + public final Lidar m_reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse"); + public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); - public final Climber m_robotClimber = new Climber(m_robotMap.climber); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -110,8 +118,9 @@ public class RobotContainer { // ! Teleop Commands public void stop() { - new InstantCommand(()->{}, m_robotSwerveDrive).schedule();;;;; + new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); m_robotSwerveDrive.stopModules(); + Constants.AutoConstants.Y_OFFSET_TRIM.set(0); } // ! /* Autos */ @@ -119,18 +128,23 @@ public class RobotContainer { private SendableChooser autoChooser; private Command autoCommand; + private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove); + // private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed); + // private Command waitDebugerManual = new waitSupplier(m_driverXbox::getYButtonPressed); + private Command waitDebuger = new waitSupplier(() -> true); + // 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 GotoLastApril(m_robotSwerveDrive, m_vision); - private Command AprilLidarAlignL4Right = new SequentialCommandGroup( + private Command AprilLidarAlignL4RightFullAuto = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new ConditionalCommand(Commands.none(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT) + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true) ), () -> m_robotElevator.isL4Primed()), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), @@ -139,8 +153,18 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), + waitDebuger.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), + new ParallelRaceGroup( + new WaitCommand(1), + new MoveUntilSuply( + m_robotSwerveDrive, + new Translation2d(0,-0.5), + new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + ), + new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), @@ -148,8 +172,92 @@ public class RobotContainer { new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + + // new ConditionalCommand( + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + // () -> m_robotElevator.hasCoral()) + + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + ); + + private Command AprilLidarAlignL4RightSemiAuto = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + new ConditionalCommand(Commands.none(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true) + ), () -> m_robotElevator.isL4Primed()), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), + waitDebuger.asProxy(), + // new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // waitDebuger.asProxy(), + // new ParallelRaceGroup( + // new WaitCommand(1), + // new MoveUntilSuply( + // m_robotSwerveDrive, + // new Translation2d(0,-0.5), + // new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + // ), + // new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive), + // () -> m_robotElevator.hasCoral()) + + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + ); + + private Command WannaSeeMeDunk = new SequentialCommandGroup( + new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + new waitEndefectorRefrence(m_robotElevator), + new MoveForTimeCommand( + m_robotSwerveDrive, + new Translation2d(0,1), + new Translation2d(), + AutoConstants.L4_DRIVE_TIME, + true + ), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive) + ); + + /* private Command AprilLidarAlignL4Right = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + new ConditionalCommand(Commands.none(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true) + ), () -> m_robotElevator.isL4Primed()), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), + waitDebuger.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), + + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + + new waitEndefectorRefrence(m_robotElevator), + + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + + new ConditionalCommand( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + () -> m_robotElevator.hasCoral()), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -160,7 +268,7 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT) + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true) ), () -> m_robotElevator.isL4Primed()), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), @@ -169,19 +277,118 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT), - - new LidarAlign(m_robotSwerveDrive, m_lidar), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), + waitDebuger.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), new waitEndefectorRefrence(m_robotElevator), + new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + new ConditionalCommand( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + () -> m_robotElevator.hasCoral()), + + + + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + ); */ + + private Command AprilLidarAlignL4LeftFullAuto = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + + new ConditionalCommand(Commands.none(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true) + ), () -> m_robotElevator.isL4Primed()), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), + waitDebuger.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), + new ParallelRaceGroup( + new WaitCommand(1), + new MoveUntilSuply( + m_robotSwerveDrive, + new Translation2d(0,-0.5), + new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + ), + new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive), + + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + + new waitEndefectorRefrence(m_robotElevator), + + + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + + // // new ConditionalCommand( + // // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + // () -> m_robotElevator.hasCoral()), + + + + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + ); + + private Command AprilLidarAlignL4LeftSemiAuto = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + + new ConditionalCommand(Commands.none(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true) + ), () -> m_robotElevator.isL4Primed()), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), + waitDebuger.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // waitDebuger.asProxy(), + // new ParallelRaceGroup( + // new WaitCommand(1), + // new MoveUntilSuply( + // m_robotSwerveDrive, + // new Translation2d(0,-0.5), + // new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + // ), + // new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + + // new waitEndefectorRefrence(m_robotElevator), + + + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + + // // // new ConditionalCommand( + // // // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + // // () -> m_robotElevator.hasCoral()), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -191,14 +398,16 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT) + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT, true) ), () -> m_robotElevator.isL3Primed()), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true), + waitDebuger.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -214,7 +423,7 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT) + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT, true) ),() -> m_robotElevator.isL3Primed()), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), @@ -223,9 +432,11 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true), + waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), @@ -239,12 +450,18 @@ public class RobotContainer { private Command AprilLidarAlignL2Left = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT, true), + waitDebuger.asProxy(), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true), + waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator), + new waitElevatorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), @@ -254,75 +471,147 @@ public class RobotContainer { private Command AprilLidarAlignL2Right = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT, true), + waitDebuger.asProxy(), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true), + waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator), + new waitElevatorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); - private Command leftAlgaeRemove = new SequentialCommandGroup( + // private Command leftAlgaeRemove = new SequentialCommandGroup( + // new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), + // new waitEndefectorRefrence(m_robotElevator), + // new waitElevatorRefrence(m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), + // new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + // new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + // ); + + private Command lowerAlgaeRemove = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true), + waitDebuger.asProxy(), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), + new Translation2d(1,0), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME / 2, true), + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME * 2, true), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); - private Command rightAlgaeRemove = new SequentialCommandGroup( + // private Command rightAlgaeRemove = new SequentialCommandGroup( + // new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), + // new waitEndefectorRefrence(m_robotElevator), + // new waitElevatorRefrence(m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), + // new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(1,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + // new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + // ); + private Command upperAlgaeRemove = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true), + waitDebuger.asProxy(), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), + new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME, true), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command thrustIntake = new SequentialCommandGroup( - new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 100, true), - new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive) + new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod(), m_robotSwerveDrive), + new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 300, true), + new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive), + new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod(), m_robotSwerveDrive) ); private Boolean operatorManualMode = false; + // public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right.asProxy(), () -> !m_robotElevator.hasCoral()); + + // public Command LoopAprilLidarAlignL4Left = new SequentialCommandGroup(AprilLidarAlignL4Left.asProxy(), new ConditionalCommand(AprilLidarAlignL4Left.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL4Right = new SequentialCommandGroup(AprilLidarAlignL4Right.asProxy(), new ConditionalCommand(AprilLidarAlignL4Right.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL3Left = new SequentialCommandGroup(AprilLidarAlignL3Left.asProxy(), new ConditionalCommand(AprilLidarAlignL3Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL3Right = new SequentialCommandGroup(AprilLidarAlignL3Right.asProxy(), new ConditionalCommand(AprilLidarAlignL3Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL2Left = new SequentialCommandGroup(AprilLidarAlignL2Left.asProxy(), new ConditionalCommand(AprilLidarAlignL2Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL2Right = new SequentialCommandGroup(AprilLidarAlignL2Right.asProxy(), new ConditionalCommand(AprilLidarAlignL2Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral())); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + NamedCommands.registerCommand("taxi", new SequentialCommandGroup( + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0, -1), + new Translation2d(), 1000, true + ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); + + NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy()); + NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator)); + + NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, + new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); + // NamedCommands.registerCommand("feed-driveback", Commands.none()); + NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY)); - NamedCommands.registerCommand("grab-coral", new waitFeedCoral(m_robotElevator)); NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0, 1), - new Translation2d(), 200, true - ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); + new Translation2d(), 300, true + + ), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)), + new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); - NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); - NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); + NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4LeftFullAuto); + NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4RightFullAuto); NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left); NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right); NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right); - NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup( - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar) + + NamedCommands.registerCommand("lower-algae-removal", lowerAlgaeRemove); + NamedCommands.registerCommand("upper-algae-removal", upperAlgaeRemove); + + NamedCommands.registerCommand("prepare-l4", new SequentialCommandGroup( + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Hovering)), + // new waitElevatorRefrence(m_robotElevator), + // new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour)) )); configureButtonBindings(); @@ -417,10 +706,10 @@ public class RobotContainer { // While Left Trigger Pressed: Trims new Trigger(() -> getDeadbandedDriverController().getPOV() == 0 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) - .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp())); + .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown())); new Trigger(() -> getDeadbandedDriverController().getPOV() == 180 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) - .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown())); + .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp())); new Trigger(() -> getDeadbandedDriverController().getPOV() == 90 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp())); @@ -428,6 +717,14 @@ public class RobotContainer { new Trigger(() -> getDeadbandedDriverController().getPOV() == 270 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust *= 2;})) + .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust /= 2;})); + + new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod())) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod())); + // While Left Trigger NOT Pressed: Fine Alignment new Trigger(() -> getDeadbandedDriverController().getPOV() != -1 && !(getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)) .whileTrue(new RunCommand( @@ -440,11 +737,16 @@ public class RobotContainer { ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, + // new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); + .onTrue(WannaSeeMeDunk.asProxy()); + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_reefLidar)); // ? /* Operator Buttons */ @@ -457,10 +759,10 @@ public class RobotContainer { // Button box new JoystickButton(getButtonBox(), ButtonBox.Five) - .onTrue(AprilLidarAlignL4Left); + .onTrue(AprilLidarAlignL4LeftSemiAuto); new JoystickButton(getButtonBox(), ButtonBox.One) - .onTrue(AprilLidarAlignL4Right); + .onTrue(AprilLidarAlignL4RightSemiAuto); new JoystickButton(getButtonBox(), ButtonBox.Six) .onTrue(AprilLidarAlignL3Left); @@ -476,11 +778,11 @@ public class RobotContainer { // Lower algae removal new JoystickButton(getButtonBox(), ButtonBox.Eight) - .onTrue(leftAlgaeRemove); + .onTrue(lowerAlgaeRemove); // Upper algae removal new JoystickButton(getButtonBox(), ButtonBox.Four) - .onTrue(rightAlgaeRemove); + .onTrue(upperAlgaeRemove); // Cancel button @@ -511,18 +813,18 @@ public class RobotContainer { .onTrue (new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed), m_robotElevator)) .onFalse(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go), m_robotElevator)); - new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> {operatorManualMode = !operatorManualMode;})); - new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking())); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking())); // Auto Scoring new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(AprilLidarAlignL4Left); + .onTrue(AprilLidarAlignL4LeftSemiAuto); new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(AprilLidarAlignL4Right); + .onTrue(AprilLidarAlignL4RightSemiAuto); new Trigger(() -> getDeadbandedOperatorController().getLeftBumperButton() && !operatorManualMode) .onTrue(AprilLidarAlignL3Left); @@ -538,20 +840,12 @@ public class RobotContainer { //Controller Lower Algae Removal new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180 && !operatorManualMode) - .onTrue(leftAlgaeRemove); + .onTrue(lowerAlgaeRemove); //Controller Upper Algae Removal new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0 && !operatorManualMode) - .onTrue(rightAlgaeRemove); + .onTrue(upperAlgaeRemove); - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90) - .onTrue(new InstantCommand(() -> m_robotClimber.climberOut())) - .onFalse(new InstantCommand(() -> m_robotClimber.stopClimber())); - - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270) - .onTrue(new InstantCommand(() -> m_robotClimber.climberIn())) - .onFalse(new InstantCommand(() -> m_robotClimber.stopClimber())); - // ? /* Programer Buttons (Controller 3)*/ // * /* Auto Recording */ @@ -627,6 +921,8 @@ public class RobotContainer { // File dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\"); String[] autos = dir.list(); + if(autos == null) return; + for (String auto : autos) { if (auto.endsWith(".auto")) autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", "")); @@ -634,7 +930,15 @@ public class RobotContainer { } autoChooser.onChange((filename) -> { - autoCommand = new PathPlannerAuto(filename); + if (filename.equals("Taxi")) { + autoCommand = new SequentialCommandGroup( + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0, -1), + new Translation2d(), 1000, true + ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)); + } else { + autoCommand = new PathPlannerAuto(filename); + } System.out.println("Robot Auto Changed " + filename); }); // SmartDashboard.putData(autoChooser); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index cd58c9c..577f6bf 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,16 +7,25 @@ package frc4388.robot; +import com.ctre.phoenix6.hardware.TalonFX; + import org.photonvision.PhotonCamera; import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.wpilibj.DigitalInput; -import frc4388.robot.Constants.ClimberConstants; import frc4388.robot.Constants.ElevatorConstants; +// import edu.wpi.first.wpilibj.motorcontrol.Spark; +// import frc4388.robot.Constants.LEDConstants; +import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.VisionConstants; +// import frc4388.robot.subsystems.SwerveModule; +import frc4388.utility.RobotGyro; /** * Defines and holds all I/O objects on the Roborio. This is useful for unit @@ -46,13 +55,14 @@ public class RobotMap { /* Elevator Subsystem */ public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); + public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); + public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - /* Climber Subsytem */ - public final TalonFX climber = new TalonFX(ClimberConstants.CLIMBER_ID.id); - - void configureDriveMotorControllers() {} - + void configureDriveMotorControllers() { + // endeffector.saf + } + } diff --git a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java new file mode 100644 index 0000000..cda04a7 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java @@ -0,0 +1,54 @@ +package frc4388.robot.commands; + +import java.time.Instant; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.Lidar; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.TimesNegativeOne; + +// Command to repeat a joystick movement for a specific time. +public class DriveUntilLiDAR extends Command { + private final SwerveDrive swerveDrive; + private final Translation2d leftStick; + private final Translation2d rightStick; + private final Lidar m_lidar; + private final double mindistance; + private final boolean robotRelative; + + public DriveUntilLiDAR( + SwerveDrive swerveDrive, + Translation2d leftStick, + Translation2d rightStick, + Lidar lidar, + double mindistance, + boolean robotRelative) { + addRequirements(swerveDrive); + + this.swerveDrive = swerveDrive; + this.leftStick = leftStick; + this.rightStick = rightStick; + this.m_lidar = lidar; + this.mindistance = mindistance; + this.robotRelative = robotRelative; + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + swerveDrive.driveFine(leftStick, rightStick, 0.3); + } + + @Override + public boolean isFinished() { + if (Math.abs(m_lidar.getDistance()) < mindistance) { + swerveDrive.softStop(); + return true; + } + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index 5f7f285..0686440 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -1,5 +1,7 @@ package frc4388.robot.commands; +import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; + import java.util.Optional; import edu.wpi.first.math.geometry.Pose2d; @@ -29,25 +31,27 @@ public class GotoLastApril extends Command { private PID xPID = new PID(AutoConstants.XY_GAINS, 0); private PID yPID = new PID(AutoConstants.XY_GAINS, 0); - private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); + // private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); private Pose2d targetpos; SwerveDrive swerveDrive; Vision vision; double distance; Side side; + boolean waitVelocityZero; /** * Command to drive robot to position. * @param SwerveDrive m_robotSwerveDrive */ - public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side) { + public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) { this.swerveDrive = swerveDrive; this.vision = vision; this.distance = distance; this.side = side; - // addRequirements(swerveDrive); + this.waitVelocityZero = waitVelocityZero && false; + addRequirements(swerveDrive); } @@ -56,24 +60,43 @@ public class GotoLastApril extends Command { tagRelativeXError = val; } - Alliance alliance = null; - @Override public void initialize() { + // double kP = AutoConstants.P_XY_GAINS.get(); + // double kI = AutoConstants.I_XY_GAINS.get(); + // double kD = AutoConstants.D_XY_GAINS.get(); + // xPID = new PID(new Gains( + // kP, + // kI, + // kD), + // 0); + // yPID = new PID(new Gains( + // kP, + // kI, + // kD), + // 0); + + // System.out.println("kP: "+kP); + // System.out.println("kI: "+kI); + // System.out.println("kD: "+kD); xPID.initialize(); yPID.initialize(); - this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, - Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), - distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())); - Optional a = DriverStation.getAlliance(); - if(!a.isEmpty()) - alliance = a.get(); + this.targetpos = ReefPositionHelper.getNearestPosition( + this.vision.getPose2d(), + side, + Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), + distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get()) + ); } double xerr; double yerr; double roterr; + double xoutput; + double youtput; + double rotoutput; + @Override public void execute() { xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis); @@ -85,28 +108,27 @@ public class GotoLastApril extends Command { roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees())); - boolean invert = Math.abs(roterr) > 180; - if(roterr > 180){ roterr -= 360; }else if(roterr < -180){ roterr += 360; } - SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees()); - SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees()); - SmartDashboard.putNumber("Rotational PID error", roterr); + // SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees()); + // SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees()); + // SmartDashboard.putNumber("Rotational PID error", roterr); - // SmartDashboard.putNumber("PID X Error", xerr); - // SmartDashboard.putNumber("PID Y Error", yerr); + SmartDashboard.putNumber("PID X Error", xerr); + SmartDashboard.putNumber("PID Y Error", yerr); + SmartDashboard.putNumber("PID Rot Error", roterr); - double xoutput = xPID.update(xerr); - double youtput = yPID.update(yerr); - double rotoutput = rotPID.update(roterr); + xoutput = xPID.update(xerr); + youtput = yPID.update(yerr); + // rotoutput = rotPID.update(roterr); xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; - rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1; + // rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1; @@ -116,24 +138,28 @@ public class GotoLastApril extends Command { // 0,0 ); - Translation2d rightStick = new Translation2d( - Math.max(Math.min(rotoutput, 1), -1), - 0 - ); - - + // Translation2d rightStick = new Translation2d( + // Math.max(Math.min(rotoutput, 1), -1), + // 0 + // ); setTagRelativeXError( new Translation2d(xerr, yerr).getAngle() .rotateBy(targetpos.getRotation()) .getCos()); - swerveDrive.driveWithInput(leftStick, rightStick, true); + swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation()); + // swerveDrive.driveWithInputOrientation(leftStick, rightStick, true); } @Override public final boolean isFinished() { - boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE); + boolean finished = ( + (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && + (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && + (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) && + (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) + ); // System.out.println(finished); if(finished) diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index ce5c147..8da22a3 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -41,7 +41,7 @@ public class LidarAlign extends Command { this.currentFinderTick = 0; this.speed = 0.4; // TODO: find good speed for this this.foundReef = false; - this.headedRight = !(GotoLastApril.tagRelativeXError < 0); + this.headedRight = (GotoLastApril.tagRelativeXError < 0); this.additionalDistance = 0; this.bounces = 0; } @@ -97,7 +97,7 @@ public class LidarAlign extends Command { currentFinderTick = 0; foundReef = false; return false; - } else if (bounces == 2) { + } else if (bounces >= 3) { swerveDrive.stopModules(); return true; } else { diff --git a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java new file mode 100644 index 0000000..dd0d3e2 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java @@ -0,0 +1,47 @@ +package frc4388.robot.commands; + +import java.time.Instant; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.TimesNegativeOne; + +// Command to repeat a joystick movement for a specific time. +public class MoveUntilSuply extends Command { + private final SwerveDrive swerveDrive; + private final Translation2d leftStick; + private final Translation2d rightStick; + private final Supplier truth; + private final boolean robotRelative; + + public MoveUntilSuply( + SwerveDrive swerveDrive, + Translation2d leftStick, + Translation2d rightStick, + Supplier truth, + boolean robotRelative) { + addRequirements(swerveDrive); + + this.swerveDrive = swerveDrive; + this.leftStick = leftStick; + this.rightStick = rightStick; + this.truth = truth; + this.robotRelative = robotRelative; + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative); + } + + @Override + public boolean isFinished() { + return truth.get(); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java new file mode 100644 index 0000000..8db5f64 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java @@ -0,0 +1,105 @@ +package frc4388.robot.commands; + +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +import java.util.function.BooleanSupplier; + +/** + * A command composition that runs one of two commands, depending on the value of the given + * condition when this command is initialized. + * + *

The rules for command compositions apply: command instances that are passed to it cannot be + * added to any other composition or scheduled individually, and the composition requires all + * subsystems its components require. + * + *

This class is provided by the NewCommands VendorDep + */ +public class WhileTrueCommand extends Command { + private final Command m_whileTrue; + private final BooleanSupplier m_condition; + + /** + * Creates a new WhileTrueCommand. + * + * @param whileTrue the command to run while the condition is true + * @param condition the condition to determine which command to run + */ + @SuppressWarnings("this-escape") + public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) { + m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand"); + m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand"); + + //CommandScheduler.getInstance().registerComposedCommands(whileTrue); + + // addRequirements(whileTrue.getRequirements()); + } + + @Override + public void initialize() { + if(m_condition.getAsBoolean()) + m_whileTrue.initialize(); + } + + @Override + public void execute() { + m_whileTrue.execute(); + + System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean()); + + if(!m_whileTrue.isFinished()) + return; + + if(m_condition.getAsBoolean()){ + m_whileTrue.end(false); + m_whileTrue.initialize(); + } + } + + @Override + public void end(boolean interrupted) { + m_whileTrue.end(interrupted); + } + + @Override + public boolean isFinished() { + return !m_condition.getAsBoolean() && m_whileTrue.isFinished(); + } + + @Override + public boolean runsWhenDisabled() { + return m_whileTrue.runsWhenDisabled(); + } + + @Override + public InterruptionBehavior getInterruptionBehavior() { + if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) { + return InterruptionBehavior.kCancelSelf; + } else { + return InterruptionBehavior.kCancelIncoming; + } + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + builder.addStringProperty("whileTrue", m_whileTrue::getName, null); + builder.addStringProperty( + "selected", + () -> { + if (m_whileTrue == null) { + return "null"; + } else { + return m_whileTrue.getName(); + } + }, + null); + } +} diff --git a/src/main/java/frc4388/robot/commands/waitSupplier.java b/src/main/java/frc4388/robot/commands/waitSupplier.java new file mode 100644 index 0000000..0ea5a3b --- /dev/null +++ b/src/main/java/frc4388/robot/commands/waitSupplier.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class waitSupplier extends Command { + /** Creates a new waitSupplier. */ + private final Supplier truth; + public waitSupplier(Supplier truth) { + this.truth = truth; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return truth.get(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java deleted file mode 100644 index 34d8e65..0000000 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.subsystems; - -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.ClimberConstants; - -public class Climber extends SubsystemBase { - /** Creates a new Climber. */ - - public TalonFX climberMotor; - - public Climber(TalonFX climberTalonFX) { - climberMotor = climberTalonFX; - climberMotor.setNeutralMode(NeutralModeValue.Brake); - - } - - public void stopClimber(){ - climberMotor.set(0); - } - - - public void climberOut(){ - climberMotor.set(ClimberConstants.CLIMBER_SPEED); - - } - - public void climberIn(){ - climberMotor.set(-ClimberConstants.CLIMBER_SPEED); - - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 7e775d2..cbd10bb 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -16,6 +16,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -47,8 +48,12 @@ public class Elevator extends Subsystem { private boolean disableAutoIntake = false; + private boolean seededZeroEndefector = false; + private boolean seededZeroElevator = false; + private DigitalInput basinBeamBreak; private DigitalInput endeffectorLimitSwitch; + private DigitalInput intakeIR; public enum CoordinationState { Waiting, // for coral into the though @@ -56,6 +61,7 @@ public class Elevator extends Subsystem { Ready, // Has coral in endefector Hovering, // Has coral in endefector L2Score, + L2ScoreLeave, PrimedThree, // Arm and elevator Waiting to score in the level 3 position ScoringThree, // Arm and elevator in the level three position PrimedFour, // Arm and elevator Waiting to score in the level 4 position @@ -68,13 +74,15 @@ public class Elevator extends Subsystem { private CoordinationState currentState; - public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { + // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { + public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) { elevatorMotor = elevatorTalonFX; endeffectorMotor = endeffectorTalonFX; this.led = led; this.basinBeamBreak = basinLimitSwitch; this.endeffectorLimitSwitch = endeffectorLimitSwitch; + this.intakeIR = intakeDigitalInput; elevatorMotor.setNeutralMode(NeutralModeValue.Brake); endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); @@ -104,12 +112,15 @@ public class Elevator extends Subsystem { public void transitionState(CoordinationState state) { + // elevatorMotor.enable(); + + currentState = state; switch (currentState) { case Waiting: { wait = System.currentTimeMillis() + maxWait; PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0)); led.setMode(LEDConstants.WAITING_PATTERN); break; } @@ -122,78 +133,85 @@ public class Elevator extends Subsystem { } case Ready: { - PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR); + PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0)); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); led.setMode(LEDConstants.DOWN_PATTERN); break; } case Hovering: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); + PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); led.setMode(LEDConstants.READY_PATTERN); break; } case L2Score: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case L2ScoreLeave: { + PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case PrimedFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case ScoringFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case PrimedThree: { PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case ScoringThree: { PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL2Primed: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL2Go: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL3Primed: { PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); break; } case BallRemoverL3Go: { PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } @@ -237,11 +255,15 @@ public class Elevator extends Subsystem { // } // } + public boolean getEndeffectorLimit() { + return endeffectorLimitSwitch.get(); + } + private void periodicWaiting() { if (!basinBeamBreak.get()) transitionState(CoordinationState.Ready); - if(!endeffectorLimitSwitch.get()) - transitionState(CoordinationState.Hovering); + // if(!endeffectorLimitSwitch.get()) + // transitionState(CoordinationState.Hovering); } // private void periodicWaitingTripped() { @@ -253,7 +275,7 @@ public class Elevator extends Subsystem { if (elevatorAtReference() && !endeffectorLimitSwitch.get()) transitionState(CoordinationState.Hovering); if(elevatorAtReference() && endeffectorLimitSwitch.get()) - transitionState(CoordinationState.Waiting); + transitionState(CoordinationState.Hovering); } private void periodicScoring() { @@ -288,10 +310,33 @@ public class Elevator extends Subsystem { @Override public void periodic() { + // double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble(); + // double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble(); + // double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble(); + // double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble(); + + + // if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){ + // PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble()); + // } + // This method will be called once per scheduler run + // SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity); + // SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque); SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); + SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); SmartDashboard.putString("State", currentState.toString()); + + if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) { + endeffectorMotor.setPosition(0); + seededZeroEndefector = !seededZeroEndefector; + } + + if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) { + elevatorMotor.setPosition(0); + seededZeroElevator = !seededZeroElevator; + } if (disableAutoIntake) return; @@ -302,21 +347,32 @@ public class Elevator extends Subsystem { } else if (currentState == CoordinationState.Ready) { periodicReady(); } + + if(!intakeIR.get()){ + led.setMode(LEDConstants.DOWN_PATTERN); + } + + // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { // periodicScoring(); // } } - public boolean isL4Primed(){ + public boolean isL4Primed() { return currentState == CoordinationState.PrimedFour; } - public boolean isL3Primed(){ + public boolean isL3Primed() { return currentState == CoordinationState.PrimedThree; } - public boolean hasCoral(){ - return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get(); + public boolean hasCoral() { + return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get(); + } + + public boolean readyToMove() { + return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get(); + // return hasCoral(); } public void armShuffle(){ diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index 3ac5cff..e660301 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -15,13 +15,31 @@ import frc4388.utility.Status.ReportLevel; // https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface public class Lidar extends Subsystem { - private double distance = -1; - Counter LidarPWM = new Counter(LiDARConstants.LIDAR_DIO_CHANNEL); + private Counter LidarPWM; + private String name = "Lidar"; - public Lidar() { + private double distance = -1; + public Lidar(int port, String name) { + this.name = name; + LidarPWM = new Counter(port); LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement LidarPWM.reset(); + + + subsystemLayout = Shuffleboard.getTab("Subsystems") + .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .withSize(2, 2); + + sbDistance = subsystemLayout + .add("Distance", 0) + .withWidget(BuiltInWidgets.kGraph) + .getEntry(); + + sbWithinDistance = subsystemLayout + . add("Within Distance", 0) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); } @Override @@ -41,23 +59,13 @@ public class Lidar extends Subsystem { return distance < LiDARConstants.LIDAR_DETECT_DISTANCE; } - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) - .withSize(2, 2); - - GenericEntry sbDistance = subsystemLayout - .add("Distance", 0) - .withWidget(BuiltInWidgets.kGraph) - .getEntry(); - - GenericEntry sbWithinDistance = subsystemLayout - .add("Within Distance", 0) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); + ShuffleboardLayout subsystemLayout; + GenericEntry sbDistance; + GenericEntry sbWithinDistance; @Override public String getSubsystemName() { - return "Lidar"; + return "Lidar " + name; } @Override diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 484480a..c1f9335 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -58,6 +58,8 @@ public class SwerveDrive extends Subsystem { public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to // 25% + public double lastOdomSpeed; + public Pose2d initalPose2d = null; @@ -116,6 +118,7 @@ public class SwerveDrive extends Subsystem { } public void setOdoPose(Pose2d pose) { + if (pose == null) return; initalPose2d = pose; swerveDriveTrain.resetPose(pose); } @@ -141,7 +144,7 @@ public class SwerveDrive extends Subsystem { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput return; // don't bother doing swerve drive math and return early. - leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); stopped = false; if (fieldRelative) { @@ -184,10 +187,12 @@ public class SwerveDrive extends Subsystem { public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) { stopped = false; // Create robot-relative speeds. + if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0); swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + } @@ -211,18 +216,33 @@ public class SwerveDrive extends Subsystem { .withTargetDirection(rightStick.getAngle())); } + public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) { + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(heading); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + ); + swerveDriveTrain.setControl(ctrl); + } + public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { leftStick = leftStick.rotateBy(heading); var ctrl = new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) - .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); - ctrl.HeadingController.setPID( - SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, - SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, - SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD - ); + .withTargetDirection(heading); + // ctrl.HeadingController.setPID( + // SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + // SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + // SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + // ); swerveDriveTrain.setControl(ctrl); } @@ -288,6 +308,7 @@ public class SwerveDrive extends Subsystem { swerveDriveTrain.tareEverything(); robotKnowsWhereItIs = false; rotTarget = 0; + // vision.resetRotations(); } @@ -313,9 +334,13 @@ public class SwerveDrive extends Subsystem { SmartDashboard.putNumber("RotTartget", rotTarget); double time = Vision.getTime(); - + double freq = swerveDriveTrain.getOdometryFrequency(); - vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); + Optional curpose = swerveDriveTrain.samplePoseAt(time); + Optional lastPose = swerveDriveTrain.samplePoseAt(time - freq); + + vision.setLastOdomPose(curpose); + setLastOdomSpeed(curpose, lastPose, freq); if (vision.isTag()) { Pose2d pose = vision.getPose2d(); @@ -326,7 +351,9 @@ public class SwerveDrive extends Subsystem { rotTarget += deltaAngle; } - swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); + vision.addVisionMeasurement(swerveDriveTrain); + // swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); + //back to the ~~future~~ *past* } // if(e.isPresent()) @@ -392,11 +419,35 @@ public class SwerveDrive extends Subsystem { setToSlow(); } + public void startTurboPeriod() { + tmp_gear_index = gear_index; + setToTurbo(); + } + public void endSlowPeriod() { setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]); gear_index = tmp_gear_index; } + + + public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ + if(curPose.isPresent() && lastPose.isPresent()){ + this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; + + + // double rotDiff = curPose.get().getRotation().getDegrees() - lastPose.get().getRotation().getDegrees(); + + // if(rotDiff >= 180){ + // vision.subtractRotation(); + // }else if(rotDiff <= -180){ + // vision.addRotation(); + // } + SmartDashboard.putNumber("Speed", lastOdomSpeed); + } + } + + @Override public String getSubsystemName() { return "Swerve Drive Controller"; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index ed5ebe9..4f34193 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -9,6 +9,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import java.time.Instant; +import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -24,6 +26,9 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; @@ -45,16 +50,23 @@ public class Vision extends Subsystem { private PhotonCamera[] cameras; private PhotonPoseEstimator[] estimators; + private List poses = new ArrayList<>(); private boolean isTagDetected = false; private boolean isTagProcessed = false; - private Pose2d lastVisionPose = new Pose2d(); + + private double lastLatency = 0; + + public double getLastLatency() { + return lastLatency; + } + + public Pose2d lastVisionPose = new Pose2d(); private Pose2d lastPhysOdomPose = new Pose2d(); private Matrix curStdDevs; private Field2d field = new Field2d(); - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") .getLayout(getSubsystemName(), BuiltInLayouts.kList) @@ -92,25 +104,42 @@ public class Vision extends Subsystem { photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight}; + // resetRotations(); } @Override public void periodic() { update(); field.setRobotPose(getPose2d()); + // cameras[0]. } + // public int[] rotations; + // public Instant[] lastUpdateTimes; + + // public void resetRotations(){ + // rotations = new int[cameras.length]; + // lastUpdateTimes = new Instant[cameras.length]; + // } + + private Instant lastVisionTime = null; + private void update() { isTagProcessed = false; isTagDetected = false; + Instant now = Instant.now(); int cams = 0; - double X = 0; - double Y = 0; - double Yaw = 0; + // double X = 0; + // double Y = 0; + // double Yaw = 0; + double latency = 0; + + // Pose2d pose = null; + poses.clear(); for(int i = 0; i < cameras.length; i++){ PhotonCamera camera = cameras[i]; @@ -124,6 +153,7 @@ public class Vision extends Subsystem { var result = results.get(results.size()-1); + latency += result.getTimestampSeconds(); isTagDetected = isTagDetected | result.hasTargets(); @@ -136,20 +166,40 @@ public class Vision extends Subsystem { // If the tag was failed to be processed if(estimatedRobotPose.isEmpty()) continue; + + poses.add(estimatedRobotPose.get()); + + // if(pose == null) + // pose = estimatedRobotPose.get().estimatedPose.toPose2d(); + // else + // pose = pose.interpolate(pose, 0.5); + // X += pose.getX(); + // Y += pose.getY(); + + // if(X > 6) + + // Yaw += (pose.getRotation().getDegrees() + 180) % 360; + // cams++; - Pose2d pose = estimatedRobotPose.get().estimatedPose.toPose2d(); - X += pose.getX(); - Y += pose.getY(); - Yaw += pose.getRotation().getDegrees(); - cams++; isTagProcessed = true; } - if(isTagProcessed){ - lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(Yaw/cams)); - } + // lastLatency = latency / cams; + + // if(isTagProcessed){ + + + // lastVisionPose = pose; + // // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); + // // lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360)); + + // // SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations()); + // // SmartDashboard.putNumber("Rotations", rotations); + + // lastVisionTime = now; + // } } @@ -179,66 +229,66 @@ public class Vision extends Subsystem { return visionEst; // Will be empty visionEst = estimator.update(change); - updateEstimationStdDevs(visionEst, change.getTargets(), estimator); + // updateEstimationStdDevs(visionEst, change.getTargets(), estimator); return visionEst; } - /** - * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard - * deviations based on number of tags, estimation strategy, and distance from the tags. - * - * @param estimatedPose The estimated pose to guess standard deviations for. - * @param targets All targets in this camera frame - */ - private void updateEstimationStdDevs( - Optional estimatedPose, - List targets, - PhotonPoseEstimator estimator) { - if (estimatedPose.isEmpty()) { - // No pose input. Default to single-tag std devs - curStdDevs = VisionConstants.kSingleTagStdDevs; + // /** + // * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard + // * deviations based on number of tags, estimation strategy, and distance from the tags. + // * + // * @param estimatedPose The estimated pose to guess standard deviations for. + // * @param targets All targets in this camera frame + // */ + // private void updateEstimationStdDevs( + // Optional estimatedPose, + // List targets, + // PhotonPoseEstimator estimator) { + // if (estimatedPose.isEmpty()) { + // // No pose input. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; - } else { - // Pose present. Start running Heuristic - var estStdDevs = VisionConstants.kSingleTagStdDevs; - int numTags = 0; - double avgDist = 0; + // } else { + // // Pose present. Start running Heuristic + // var estStdDevs = VisionConstants.kSingleTagStdDevs; + // int numTags = 0; + // double avgDist = 0; - // Precalculation - see how many tags we found, and calculate an average-distance metric - for (var tgt : targets) { - var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) continue; + // // Precalculation - see how many tags we found, and calculate an average-distance metric + // for (var tgt : targets) { + // var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); + // if (tagPose.isEmpty()) continue; - double distance = tagPose - .get() - .toPose2d() - .getTranslation() - .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + // double distance = tagPose + // .get() + // .toPose2d() + // .getTranslation() + // .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); - if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { - numTags++; - avgDist += distance; - } - } + // if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { + // numTags++; + // avgDist += distance; + // } + // } - if (numTags == 0) { - // No tags visible. Default to single-tag std devs - curStdDevs = VisionConstants.kSingleTagStdDevs; - } else { - // One or more tags visible, run the full heuristic. - avgDist /= numTags; - // Decrease std devs if multiple targets are visible - if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; - // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) - estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); - curStdDevs = estStdDevs; - } - } - } + // if (numTags == 0) { + // // No tags visible. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; + // } else { + // // One or more tags visible, run the full heuristic. + // avgDist /= numTags; + // // Decrease std devs if multiple targets are visible + // if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; + // // Increase std devs based on (average) distance + // if (numTags == 1 && avgDist > 4) + // estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + // else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + // curStdDevs = estStdDevs; + // } + // } + // } /** * Returns the latest standard deviations of the estimated pose from {@link @@ -253,21 +303,23 @@ public class Vision extends Subsystem { - - - - - public void setLastOdomPose(Optional pose){ if(pose.isPresent()) lastPhysOdomPose = pose.get(); } + // public double getLastOdomSpeed(){ + // return lastOdomSpeed; + // } + public Pose2d getPose2d() { - if(isTagDetected && isTagProcessed) - return lastVisionPose; - else + if(lastPhysOdomPose != null) return lastPhysOdomPose; + return new Pose2d(); + // if(isTagDetected && isTagProcessed) + // // return lastVisionPose; + // else + // return lastPhysOdomPose; } public static double getTime() { @@ -279,7 +331,11 @@ public class Vision extends Subsystem { } - + public void addVisionMeasurement( SwerveDrivetrain drivetrain){ + for(EstimatedRobotPose pose : poses){ + drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds)); + } + } diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index a3ab63e..75c4ad0 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -5,6 +5,7 @@ import java.util.Optional; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc4388.robot.Constants.AutoConstants; @@ -14,7 +15,8 @@ public class ReefPositionHelper { public enum Side { LEFT, RIGHT, - CENTER + CENTER, + FAR_LEFT } public static final Pose2d[] RED_TAGS = { @@ -56,6 +58,8 @@ public class ReefPositionHelper { minDistance = dist; } } + + System.out.println(minPos.getRotation().getDegrees()); return minPos; } @@ -81,11 +85,14 @@ public class ReefPositionHelper { switch(side) { case LEFT: return -(AutoConstants.X_SCORING_POSITION_OFFSET); + case FAR_LEFT: + return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8)); case RIGHT: return (AutoConstants.X_SCORING_POSITION_OFFSET); case CENTER: return 0; } + assert false; return 0; } diff --git a/src/main/java/frc4388/utility/Trim.java b/src/main/java/frc4388/utility/Trim.java index 7a9d017..9b06575 100644 --- a/src/main/java/frc4388/utility/Trim.java +++ b/src/main/java/frc4388/utility/Trim.java @@ -81,21 +81,22 @@ public class Trim { } public boolean load() { - try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) { - double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8)); - currentValue = fileValue; - clampModify(); - modified = false; - if (fileValue != currentValue) { - System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping..."); - modified = true; - } - return true; - } catch (Exception e) { - // e.printStackTrace(); - System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value..."); - return false; - } + // try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) { + // double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8)); + // currentValue = fileValue; + // clampModify(); + // modified = false; + // if (fileValue != currentValue) { + // System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping..."); + // modified = true; + // } + // return true; + // } catch (Exception e) { + // // e.printStackTrace(); + // System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value..."); + // return false; + // } + return false; } public void dump() {